#include <cmath>
#include <math.h>
// #include <iostream>
#include "opencv2/core/cuda.hpp"
#include "tools/lvox3_errorcode.h"
#include "tools/lvox3_gridtype.h"
#include <Eigen/Dense>
#include <QObject>
#include <QStack>

void fit(double paramFit[3], Eigen::MatrixXd data, int Ndata)
{

    std::vector<double> S(Ndata);
    S[0] = 0;

    double Sxsqr = 0;
    double SxS = 0;
    double SSksqr = 0;
    double Syx = 0;
    double SyS = 0;

    for (int i = 1; i < Ndata; ++i)
    {
        S[i] = S[i - 1] - (data(i, 1) - data(i - 1, 1)) * (data(i, 0) - data(i - 1, 0));
        Sxsqr += pow(data(i, 0) - data(0, 0), 2);
        SxS += (data(i, 0) - data(0, 0)) * S[i];
        SSksqr += pow(S[i], 2);
        Syx += (data(i, 0) - data(0, 0)) * (data(i, 1) - data(0, 1));
        SyS += S[i] * (data(i, 1) - data(0, 1));
    }

    Eigen::Vector2d out;
    Eigen::Matrix2d L(2, 2);
    Eigen::Vector2d R;

    L << Sxsqr, SxS, SxS, SSksqr;
    R << Syx, SyS;

    out = L.fullPivLu().solve(R);

    // VOIR assignation
    double c1 = static_cast<double>(out(1));
    double theta = 0;
    double Stheta = 0;
    double Sytheta = 0;
    double Sy = 0;
    double Stheta2 = 0;

    for (int i = 0; i < Ndata; ++i)
    {
        theta = exp(c1 * data(i, 0));
        Stheta += theta;
        Sytheta += theta * data(i, 1);
        Stheta2 += pow(theta, 2);
        Sy += data(i, 1);
    }

    Eigen::Vector2d out2;
    Eigen::Matrix2d L2(2, 2);
    Eigen::Vector2d R2;

    L << Ndata, Stheta, Stheta, Stheta2;
    R << Sy, Sytheta;

    out2 = L2.fullPivLu().solve(R2);

    paramFit[1] = out2(1); // Sill
    paramFit[0] = out2(0); // Nugget
    paramFit[2] = c1;      // Range
}