#include <vector>
#include <algorithm>
#include <cmath>

std::vector<std::vector<double>> linterp(const std::vector<double>& X, const std::vector<std::vector<double>>& V, const std::vector<std::vector<double>>& Xq) {
    // Ensure X is a column vector
    std::vector<double> X_col = X;

    // Ensure V is a column vector if it's a 1D vector
    std::vector<std::vector<double>> V_col = V;
    if (V.size() == 1) {
        V_col = std::vector<std::vector<double>>(V[0].size(), std::vector<double>(1));
        for (size_t i = 0; i < V[0].size(); ++i) {
            V_col[i][0] = V[0][i];
        }
    }

    // Ensure Xq is a column vector if it's a 1D vector
    std::vector<std::vector<double>> Xq_col = Xq;
    if (Xq.size() == 1) {
        Xq_col = std::vector<std::vector<double>>(Xq[0].size(), std::vector<double>(1));
        for (size_t i = 0; i < Xq[0].size(); ++i) {
            Xq_col[i][0] = Xq[0][i];
        }
    }

    // Allocate output variable space
    std::vector<std::vector<double>> Vq(Xq_col.size(), std::vector<double>(Xq_col[0].size(), 0.0));
    double dX = X_col[1] - X_col[0];
    size_t N = V_col.size();

    // Loop every Xq column
    for (size_t col = 0; col < Xq_col[0].size(); ++col) {
        // Calculate Xq shifts
        std::vector<double> f;
        std::vector<bool> fMask(Xq_col.size(), false);
        for (size_t i = 0; i < Xq_col.size(); ++i) {
            if (Xq_col[i][col] >= X_col[0] && Xq_col[i][col] <= X_col.back()) {
                f.push_back((Xq_col[i][col] - X_col[0]) / dX);
                fMask[i] = true;
            }
        }

        // Calculate Xq index
        std::vector<size_t> im(f.size());
        for (size_t i = 0; i < f.size(); ++i) {
            im[i] = static_cast<size_t>(std::floor(f[i]));
        }
        size_t im2Inval = std::count_if(im.begin(), im.end(), [N](size_t i) { return i + 2 > N; });

        // Calculate interpolation factors
        std::vector<double> fx(f.size()), gx(f.size());
        for (size_t i = 0; i < f.size(); ++i) {
            fx[i] = f[i] - im[i];
            gx[i] = 1 - fx[i];
        }

        // Interpolate Vq values
        size_t j = 0;
        for (size_t i = 0; i < Xq_col.size(); ++i) {
            if (fMask[i]) {
                if (im2Inval > 0 && j >= f.size() - im2Inval) {
                    Vq[i][col] = gx[j] * V_col[im[j] + 1][col] + fx[j] * 0.0;
                } else {
                    Vq[i][col] = gx[j] * V_col[im[j] + 1][col] + fx[j] * V_col[im[j] + 2][col];
                }
                ++j;
            }
        }
    }

    return Vq;
}