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

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

    // Ensure Vq is a column vector if it's a 1D vector
    std::vector<std::vector<double>> Vq_col = Vq;
    if (Vq.size() == 1) {
        Vq_col = std::vector<std::vector<double>>(Vq[0].size(), std::vector<double>(1));
        for (size_t i = 0; i < Vq[0].size(); ++i) {
            Vq_col[i][0] = Vq[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>> V(X_col.size(), std::vector<double>(Xq_col[0].size(), 0.0));
    double dX = X_col[1] - X_col[0];
    size_t N = V.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;
        for (size_t i = 0; i < Xq_col.size(); ++i) {
            double shift = (Xq_col[i][col] - X_col[0]) / dX;
            if (Xq_col[i][col] >= X_col[0] && Xq_col[i][col] <= X_col.back()) {
                f.push_back(shift);
                fMask.push_back(true);
            } else {
                fMask.push_back(false);
            }
        }

        // 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];
        }

        // Calculate adjoint interpolation
        std::vector<double> VqM;
        for (size_t i = 0; i < fMask.size(); ++i) {
            if (fMask[i]) {
                VqM.push_back(Vq_col[i][col]);
            }
        }

        std::vector<double> pG(N, 0.0), pF(N, 0.0);
        for (size_t i = 0; i < im.size(); ++i) {
            pG[im[i] + 1] += gx[i] * VqM[i];
            if (im[i] + 2 <= N) {
                pF[im[i] + 2] += fx[i] * VqM[i];
            }
        }

        pF.resize(N - im2Inval);

        for (size_t i = 0; i < N; ++i) {
            V[i][col] = pG[i] + (i < pF.size() ? pF[i] : 0.0);
        }
    }

    return V;
}