#include "hit_analyse_v2.h"
#include <random>

HIT_ANALYSE_V2::HIT_ANALYSE_V2(QObject *parent) : QObject(parent)
{



}

// Define your own functions for matrix operations
struct Matrix2x2 {
    double data[2][2];
};

Matrix2x2 InvertMatrix2x2(const Matrix2x2& mat) {
    Matrix2x2 result;
    double det = mat.data[0][0] * mat.data[1][1] - mat.data[0][1] * mat.data[1][0];
    if (det != 0.0) {
        double invDet = 1.0 / det;
        result.data[0][0] = mat.data[1][1] * invDet;
        result.data[0][1] = -mat.data[0][1] * invDet;
        result.data[1][0] = -mat.data[1][0] * invDet;
        result.data[1][1] = mat.data[0][0] * invDet;
    } else {
        // Handle the case when the matrix is not invertible
        // You might want to implement error handling here.
        std::cerr << "Matrix not invertible! " << std::endl;
    }
    return result;
}

struct Vector2 {
    double data[2];
};

QString HIT_ANALYSE_V2::analyseBeamData(QVector<BufferData> dataframe, const int dev_nr, const int vector_length){

    double position=100;
    double focus=8;
    double intensity=1000.0;
    QString dataString;


    double *  signal_list;
    std::vector<double> channel_list;
    std::vector<double> short_signal_list;
    std::vector<double> short_channel_list;

    // Create a random number generator with a Gaussian distribution
    std::random_device rd;
    std::mt19937 gen(rd());
    std::normal_distribution<double> dist(0.0, 17.0); // Mean of 0 and Sigma of 17

    // Create a vector to store the generated values
    std::vector<short int> result(vector_length);


    // Fill the vector with random noise values
    //add a gaussian profile, focus is FWHM, position is random between 50 and 250
    bool fixeddata = false;
    if (!fixeddata){
    position = 100;// + (rand() % (int)(250 - 50 + 1));
    signal_list = (double*)(dataframe[dev_nr].sensor_data);
    channel_list = fixed_channel;
    for (int i = int(position) - (int)(focus); i < int(position) + (int)(focus) ; i++) {
      //  double randomValue = dist(gen);
     //   signal_list[i] = static_cast<short int>(std::round(randomValue));
      //  signal_list[i] = dataframe[dev_nr].sensor_data[i];
        //channel_list[i] = i;
      signal_list[i] += static_cast<short int>(std::round(intensity*exp(-4*log(2)*pow((channel_list[i]-position)/focus,2))));

       // std::cerr << channel_list[i] << ", ";
    }

   // std::cerr <<std::endl;
    }
    else{
    signal_list = fixedsignalarray;
    channel_list = fixed_channel;
    }

/*
    // Fill signal_list and channel_list with your data

    double SumT = 0.0, SumS = 0.0, SumS2 = 0.0, SumST = 0.0, SumT2 = 0.0, SumY = 0.0, SumYS = 0.0, SumYT = 0.0;
    double b_den = 0.0, b_num = 0.0, b = 0.0, p = 0.0, c = 0.0, SumYYP = 0.0, SumYYM = 0.0, MeanY = 0.0;
    double S[vector_length];
    double T[vector_length];

    for (int k = 0; k < vector_length; k++) {
        if (k == 0) {
            S[k] = 0.0;
            T[k] = 0.0;
        } else {
            S[k] = S[k - 1] + 0.5 * (signal_list[k] + signal_list[k - 1]) * (channel_list[k] - channel_list[k - 1]);
            T[k] = T[k - 1] + 0.5 * (channel_list[k] * signal_list[k] + channel_list[k - 1] * signal_list[k - 1]) *
                                  (channel_list[k] - channel_list[k - 1]);
        }
        SumS += S[k];
        SumT += T[k];
        SumY += signal_list[k];
        SumS2 += S[k] * S[k];
        SumST += S[k] * T[k];
        SumT2 += T[k] * T[k];
        SumYS += signal_list[k] * S[k];
        SumYT += signal_list[k] * T[k];
        MeanY += signal_list[k];
    }
    MeanY /= vector_length;

    // Calculate M1 matrix elements
    double M1_00 = SumT2;
    double M1_01 = SumST;
    double M1_02 = SumT;
    double M1_10 = SumST;
    double M1_11 = SumS2;
    double M1_12 = SumS;
    double M1_20 = SumT;
    double M1_21 = SumS;
    double M1_22 = vector_length;

    // Calculate M2 vector elements
    double M2_0 = SumYT;
    double M2_1 = SumYS;
    double M2_2 = SumY;

    // Calculate the inverse of M1
    double detM1 = M1_00 * (M1_11 * M1_22 - M1_12 * M1_21) -
                   M1_01 * (M1_10 * M1_22 - M1_12 * M1_20) +
                   M1_02 * (M1_10 * M1_21 - M1_11 * M1_20);

    if (detM1 == 0.0) {
        std::cerr << "M1 is not invertible." << std::endl;
        //return 1;
    }

    double invM1_00 = (M1_11 * M1_22 - M1_12 * M1_21) / detM1;
    double invM1_01 = (M1_02 * M1_21 - M1_01 * M1_22) / detM1;
    double invM1_02 = (M1_01 * M1_12 - M1_02 * M1_11) / detM1;
    double invM1_10 = (M1_12 * M1_20 - M1_10 * M1_22) / detM1;
    double invM1_11 = (M1_00 * M1_22 - M1_02 * M1_20) / detM1;
    double invM1_12 = (M1_02 * M1_10 - M1_00 * M1_12) / detM1;
    double invM1_20 = (M1_10 * M1_21 - M1_11 * M1_20) / detM1;
    double invM1_21 = (M1_01 * M1_20 - M1_00 * M1_21) / detM1;
    double invM1_22 = (M1_00 * M1_11 - M1_01 * M1_10) / detM1;

    // Calculate ABC vector
    double ABC_0 = invM1_00 * M2_0 + invM1_01 * M2_1 + invM1_02 * M2_2;
    double ABC_1 = invM1_10 * M2_0 + invM1_11 * M2_1 + invM1_12 * M2_2;
    double ABC_2 = invM1_20 * M2_0 + invM1_21 * M2_1 + invM1_22 * M2_2;

    // Calculate b, p, and c
    p = -ABC_0 / 2.0;
    c = -ABC_1 / ABC_0;

    for (int k = 0; k < vector_length; k++) {
        double exp_term = exp(-p * (channel_list[k] - c) * (channel_list[k] - c));
        b_num += exp_term * signal_list[k];
        b_den += exp_term;
    }

    b = b_num / b_den;

    for (int k = 0; k < vector_length; k++) {
        double y_pred = b * exp(-p * (channel_list[k] - c) * (channel_list[k] - c));
        SumYYM += (signal_list[k] - MeanY) * (signal_list[k] - MeanY);
        SumYYP += (y_pred - MeanY) * (y_pred - MeanY);
    }

    double R_squared = SumYYP / SumYYM;
    //std::cout << "R-squared = " << R_squared << endl;
    position = -ABC_1/ ABC_0;
    //sigma = sqrt(1.0 / (2.0 * ABC_0));
    focus = 2.3548/sqrt(2*p);
    intensity = b;
*/
    double SumArea = 0.0, SumY2 = 0.0, SumXY2 = 0.0, SumX2Y2 = 0.0, SumX3Y2 = 0.0;
    double SumY2LnY = 0.0, SumXY2LnY = 0.0, Ymax = 0.0, Pomax = 0.0;
    double fac_c = 0.0, Yn = 0.0, sigma = 0.0, amp = 0.0;
    double SumYYP = 0.0, SumYYM = 0.0, MeanY = 0.0, window_start = 0.0, window_end = 0.0;

    // ...

    Matrix2x2 M1, M1inv;
    Vector2 ABC, M2;

    for (int i = 0; i < vector_length; i++) {
        if (signal_list[i] > Ymax) {
            Ymax = signal_list[i];
            Pomax = channel_list[i];
        }
        if (i > 0 && signal_list[i] > 34) {
            SumArea += signal_list[i] * (channel_list[i] - channel_list[i - 1]);
        }
    }

    // Estimate sigma
    sigma = SumArea / Ymax / 2.5066;

    // Set a +-3 sigma window
    window_start = Pomax - 3 * sigma;
    window_end = Pomax + 3 * sigma;
 //   std::cerr<< Pomax << " " << Ymax <<  " " << sigma << std::endl;


    for (int i = 0; i < vector_length; i++) {
        if (signal_list[i] > 34 && channel_list[i] > window_start && channel_list[i] < window_end) {
            short_signal_list.push_back(signal_list[i]);
            short_channel_list.push_back(channel_list[i]);
        }
    }
    //signal_list.clear();
    //channel_list.clear();
    // Recalculate SumArea using the sieved data
    SumArea = 0.0;
    for (int i = 1; i < short_signal_list.size(); i++) {
        SumArea += short_signal_list[i] * (short_channel_list[i] - short_channel_list[i - 1]);
    }


    const int shortlist_length = short_channel_list.size();

    if (shortlist_length <= 3) {
        intensity = -1;
        focus = -1;
        position = -128;
        dataString += QString::number(intensity) + ',' + QString::number(position) + ',' + QString::number(focus)
                      + ',' + QString::number(0);


        return dataString;
    }

    // Re-Estimate sigma
    sigma = SumArea / Ymax / 2.5066;
    fac_c = -1.0 / (2.0 * sigma * sigma);
   // std::cerr << sigma << std::endl;
    for(int k=0; k<shortlist_length;k++){

        SumY2 += short_signal_list[k]*short_signal_list[k];
        SumXY2 += short_signal_list[k]*short_signal_list[k]*short_channel_list[k];
        SumX2Y2 +=  short_signal_list[k]*short_signal_list[k]*short_channel_list[k]*short_channel_list[k];
        SumX3Y2 +=  short_signal_list[k]*short_signal_list[k]*short_channel_list[k]*short_channel_list[k]*short_channel_list[k];

        SumY2LnY += short_signal_list[k]*short_signal_list[k]*log(short_signal_list[k]);
        SumXY2LnY += short_channel_list[k]*short_signal_list[k]*short_signal_list[k]*log(short_signal_list[k]);
      //  std::cerr<< shortlist_length << " " << short_channel_list[k] << " " << short_signal_list[k] << " " << short_signal_list[k] << " " << log(short_signal_list[k]) << std::endl;
        MeanY+=short_signal_list[k];
    }
    MeanY/=shortlist_length;

    // Use custom matrix and vector functions for calculations
    M1.data[0][0] = SumY2;
    M1.data[0][1] = SumXY2;
    M1.data[1][0] = SumXY2;
    M1.data[1][1] = SumX2Y2;

  //  std::cerr << M1.data[0][0]  << " " << M1.data[0][1] << " " << M1.data[1][0] << " " << M1.data[1][1] << std::endl;
    M2.data[0] = SumY2LnY - fac_c * SumX2Y2;
    M2.data[1] = SumXY2LnY - fac_c * SumX3Y2;
  //  std::cerr << M2.data[0] << " " << M2.data[1]  << std::endl;
    M1inv = InvertMatrix2x2(M1);
    ABC.data[0] = M1inv.data[0][0] * M2.data[0] + M1inv.data[0][1] * M2.data[1];
    ABC.data[1] = M1inv.data[1][0] * M2.data[0] + M1inv.data[1][1] * M2.data[1];

   // std::cerr << ABC.data[0]  << " " << ABC.data[1] << std::endl;


//iterate to improve the fit.
    int N_iter = 1;
    for (int i = 0; i < N_iter; i++) {
        SumY2 = 0.0;
        SumXY2 = 0.0;
        SumX2Y2 = 0.0;
        SumX3Y2 = 0.0;
        SumY2LnY = 0.0;
        SumXY2LnY = 0.0;

        for (int k = 0; k < shortlist_length; k++) {
            Yn = exp(ABC.data[0] + ABC.data[1] * short_channel_list[k] + fac_c * short_channel_list[k] * short_channel_list[k]);
            SumY2 += Yn * Yn;
            SumXY2 += Yn * Yn * short_channel_list[k];
            SumX2Y2 += Yn * Yn * short_channel_list[k] * short_channel_list[k];
            SumX3Y2 += Yn * Yn * short_channel_list[k] * short_channel_list[k] * short_channel_list[k];
            SumY2LnY += Yn * Yn * log(short_signal_list[k]);
            SumXY2LnY += short_channel_list[k] * Yn * Yn * log(short_signal_list[k]);
        }

        M1.data[0][0] = SumY2;
        M1.data[0][1] = SumXY2;
        M1.data[1][0] = SumXY2;
        M1.data[1][1] = SumX2Y2;

        M2.data[0] = SumY2LnY - fac_c * SumX2Y2;
        M2.data[1] = SumXY2LnY - fac_c * SumX3Y2;

        M1inv = InvertMatrix2x2(M1);
        ABC.data[0] = M1inv.data[0][0] * M2.data[0] + M1inv.data[0][1] * M2.data[1];
        ABC.data[1] = M1inv.data[1][0] * M2.data[0] + M1inv.data[1][1] * M2.data[1];
    }

    position = -ABC.data[1]/fac_c/2;

    amp = exp(ABC.data[0]-ABC.data[1]*ABC.data[1]/4/fac_c);

    sigma=SumArea/amp/2.5066;

    // cout << sigma << " " << mean << " " << amp << endl;


    for(int k=0; k<shortlist_length;k++){
        SumYYM+= (short_signal_list[k]-MeanY)*(short_signal_list[k]-MeanY);
        SumYYP+= (amp*exp(-(short_channel_list[k]-position)*(short_channel_list[k]-position)/2/(sigma*sigma)) - MeanY )*(amp*exp(-(short_channel_list[k]-position)*(short_channel_list[k]-position)/2/(sigma*sigma)) - MeanY );
    }



    focus = 2.3548*sigma;
    intensity = amp;
    double R_squared = SumYYP/SumYYM;

    dataString += QString::number(intensity) + ',' + QString::number(position) + ',' + QString::number(focus)
                         + ',' + QString::number(R_squared);


    return dataString;


}



HIT_ANALYSE_V2::~HIT_ANALYSE_V2()
{

}