#include "WCP2dToy/ToySignalPre.h"
#include "WCP2dToy/ToySignalSimu.h"
#include "WCPData/GeomWire.h"
#include "TFile.h"
#include "TVirtualFFT.h"
#include "TF1.h"
#include "TMatrixDSparse.h"
#include "TMatrixD.h"
#include "TVectorD.h"

using namespace WCP;

WCP2dToy::ToySignalPreFDS::ToySignalPreFDS(WCP::FrameDataSource &fds, const WCP::GeomDataSource &gds,
                                           int bins_per_frame1, int nframes_total)
  : fds(fds)
  , gds(gds)
  , max_frames(nframes_total)
{
    bins_per_frame = bins_per_frame1;

    GeomWireSelection wires_u = gds.wires_in_plane(WirePlaneType_t(0));
    GeomWireSelection wires_v = gds.wires_in_plane(WirePlaneType_t(1));
    GeomWireSelection wires_w = gds.wires_in_plane(WirePlaneType_t(2));

    nwire_u = wires_u.size();
    nwire_v = wires_v.size();
    nwire_w = wires_w.size();

    hu = new TH1F *[nwire_u];
    hv = new TH1F *[nwire_v];
    hw = new TH1F *[nwire_w];

    for (int i = 0; i != nwire_u; i++) {
        hu[i] = new TH1F(Form("U2_%d", i), Form("U2_%d", i), bins_per_frame, 0, bins_per_frame);
    }
    for (int i = 0; i != nwire_v; i++) {
        hv[i] = new TH1F(Form("V2_%d", i), Form("V2_%d", i), bins_per_frame, 0, bins_per_frame);
    }
    for (int i = 0; i != nwire_w; i++) {
        hw[i] = new TH1F(Form("W2_%d", i), Form("W2_%d", i), bins_per_frame, 0, bins_per_frame);
    }

    // define filter
    TF1 *filter_u = new TF1("filter_u", "(x>0.0)*gaus*exp(-0.5*pow(x/[3],[4]))");
    double par[5] = {1.73 / 0.959301, 1.69, 1.55, 0.19, 3.75};
    filter_u->SetParameters(par);

    TF1 *filter_v = new TF1("filter_v", "(x>0.0)*gaus*exp(-0.5*pow(x/[3],[4]))");
    double par1[5] = {1.74 / 0.941034, 1.46, 1.33, 0.23, 4.89};
    filter_v->SetParameters(par1);

    TF1 *filter_w = new TF1("filter_y", "(x>0.0)*[0]*exp(-0.5*(((x-[1])/[2])^2)^[3])");
    double par2[4] = {1.03 / 0.995635, 0.08, 0.15, 2.17};
    filter_w->SetParameters(par2);

    // move filter to time domain
    int nbin = fds.Get_Bins_Per_Frame();
    hfilter_u = new TH1F("hfilter_u", "hfilter_u", nbin, 0, nbin);
    hfilter_v = new TH1F("hfilter_v", "hfilter_v", nbin, 0, nbin);
    hfilter_w = new TH1F("hfilter_w", "hfilter_w", nbin, 0, nbin);

    for (Int_t i = 0; i != nbin; i++) {
        Double_t frequency = (nbin / 2. - fabs(i - nbin / 2.)) * 2. / nbin;
        hfilter_u->SetBinContent(i + 1, filter_u->Eval(frequency));
        hfilter_v->SetBinContent(i + 1, filter_v->Eval(frequency));
        hfilter_w->SetBinContent(i + 1, filter_w->Eval(frequency));
    }

    hfilter_time_u = new TH1F("hfilter_time_u", "hfilter_time_u", nbin * 2, -nbin / 4., nbin / 4.);
    hfilter_time_v = new TH1F("hfilter_time_v", "hfilter_time_v", nbin * 2, -nbin / 4., nbin / 4.);
    hfilter_time_w = new TH1F("hfilter_time_w", "hfilter_time_w", nbin * 2, -nbin / 4., nbin / 4.);

    double value_re[9600], value_im[9600];
    Int_t n = nbin;
    TVirtualFFT *ifft = TVirtualFFT::FFT(1, &n, "C2R M K");
    TH1 *fb;
    double baseline;

    // U-plane
    for (Int_t i = 0; i != nbin; i++) {
        Double_t filter = hfilter_u->GetBinContent(i + 1);
        value_re[i] = filter / nbin;
        value_im[i] = 0;
    }
    ifft->SetPointsComplex(value_re, value_im);
    ifft->Transform();
    fb = 0;
    fb = TH1::TransformHisto(ifft, fb, "Re");

    baseline = -1. / nbin;
    for (int i = 0; i != nbin; i++) {
        Double_t content = fb->GetBinContent(i + 1);
        content -= baseline;
        Int_t newbin = i + nbin / 2;
        if (newbin > nbin) newbin -= nbin;
        hfilter_time_u->SetBinContent(2 * newbin, content / 2.);
        hfilter_time_u->SetBinContent(2 * newbin + 1, content / 2.);
    }
    // V-plane
    for (Int_t i = 0; i != nbin; i++) {
        Double_t filter = hfilter_v->GetBinContent(i + 1);
        value_re[i] = filter / nbin;
        value_im[i] = 0;
    }
    ifft->SetPointsComplex(value_re, value_im);
    ifft->Transform();
    fb = 0;
    fb = TH1::TransformHisto(ifft, fb, "Re");

    baseline = -1. / nbin;
    for (int i = 0; i != nbin; i++) {
        Double_t content = fb->GetBinContent(i + 1);
        content -= baseline;
        Int_t newbin = i + nbin / 2;
        if (newbin > nbin) newbin -= nbin;
        hfilter_time_v->SetBinContent(2 * newbin, content / 2.);
        hfilter_time_v->SetBinContent(2 * newbin + 1, content / 2.);
    }
    // W-plane
    for (Int_t i = 0; i != nbin; i++) {
        Double_t filter = hfilter_w->GetBinContent(i + 1);
        value_re[i] = filter / nbin;
        value_im[i] = 0;
    }
    ifft->SetPointsComplex(value_re, value_im);
    ifft->Transform();
    fb = 0;
    fb = TH1::TransformHisto(ifft, fb, "Re");

    baseline = -1. / nbin;
    for (int i = 0; i != nbin; i++) {
        Double_t content = fb->GetBinContent(i + 1);
        content -= baseline;
        Int_t newbin = i + nbin / 2;
        if (newbin > nbin) newbin -= nbin;
        hfilter_time_w->SetBinContent(2 * newbin, content / 2.);
        hfilter_time_w->SetBinContent(2 * newbin + 1, content / 2.);
    }

    double xx[1000], yy[1000];
    for (int i = 0; i != 1000; i++) {
        xx[i] = hfilter_time_u->GetBinCenter(nbin + i - 500);
        yy[i] = hfilter_time_u->GetBinContent(nbin + i - 500);
    }
    gu = new TGraph(1000, xx, yy);

    for (int i = 0; i != 1000; i++) {
        xx[i] = hfilter_time_v->GetBinCenter(nbin + i - 500);
        yy[i] = hfilter_time_v->GetBinContent(nbin + i - 500);
    }
    gv = new TGraph(1000, xx, yy);

    for (int i = 0; i != 1000; i++) {
        xx[i] = hfilter_time_w->GetBinCenter(nbin + i - 500);
        yy[i] = hfilter_time_w->GetBinContent(nbin + i - 500);
    }
    gw = new TGraph(1000, xx, yy);
}

int WCP2dToy::ToySignalPreFDS::size() const { return max_frames; }

void WCP2dToy::ToySignalPreFDS::Save()
{
    TFile *file = new TFile("temp_pre.root", "RECREATE");
    for (int i = 0; i != nwire_u; i++) {
        TH1F *huu = (TH1F *) hu[i]->Clone(Form("U1_%d", i));
    }
    for (int i = 0; i != nwire_v; i++) {
        TH1F *hvv = (TH1F *) hv[i]->Clone(Form("V1_%d", i));
    }
    for (int i = 0; i != nwire_w; i++) {
        TH1F *hww = (TH1F *) hw[i]->Clone(Form("W1_%d", i));
    }

    TH1F *hfilter_uu = (TH1F *) hfilter_time_u->Clone("UU");
    TH1F *hfilter_vv = (TH1F *) hfilter_time_v->Clone("VV");
    TH1F *hfilter_ww = (TH1F *) hfilter_time_w->Clone("WW");

    gu->Write("gu");
    gv->Write("gv");
    gw->Write("gw");

    file->Write();
    file->Close();
}

int WCP2dToy::ToySignalPreFDS::jump(int frame_number)
{
    // fill the frame data ...
    if (frame.index == frame_number) {
        return frame_number;
    }

    frame.clear();

    int n = bins_per_frame;
    int m = fds.Get_Bins_Per_Frame();
    int scale = m / n;

    TMatrixDSparse A_u(m, n);
    TMatrixDSparse AT_u(n, m);
    TMatrixD MB_u(n, n);
    TMatrixD FM_u(n, m);

    double x[40], y[40];
    // treat filter ...
    for (int i = 0; i != 40; i++) {
        x[i] = (i - 20 + 0.5) * 0.5;
        y[i] = gu->Eval(x[i] + 0.5 / 4.);
        y[i] += gu->Eval(x[i] - 0.5 / 4.);
    }
    // fill matrix
    for (int i = 0; i != n; i++) {
        for (int j = 0; j != 40; j++) {
            Int_t row = i;
            Int_t column = scale * i - 20 + j + 1;
            if (column >= 0 && column < m) {
                A_u(column, row) = y[j];
            }
        }
    }
    // operate matrix and

    // std::cout << " a" << std::endl;
    AT_u.Transpose(A_u);
    // std::cout << " b" << std::endl;
    MB_u = (AT_u) * (A_u);
    std::cout << " Start to Invert U Matrix" << std::endl;
    MB_u.Invert();
    std::cout << " Finish to Invert U Matrix" << std::endl;
    FM_u = (MB_u) * (AT_u);
    // std::cout << " e" << std::endl;

    // V-plane
    TMatrixDSparse A_v(m, n);
    TMatrixDSparse AT_v(n, m);
    TMatrixD MB_v(n, n);
    TMatrixD FM_v(n, m);

    // treat filter ...
    for (int i = 0; i != 40; i++) {
        x[i] = (i - 20 + 0.5) * 0.5;
        y[i] = gv->Eval(x[i] + 0.5 / 4.);
        y[i] += gv->Eval(x[i] - 0.5 / 4.);
    }
    // fill matrix
    for (int i = 0; i != n; i++) {
        for (int j = 0; j != 40; j++) {
            Int_t row = i;
            Int_t column = scale * i - 20 + j + 1;
            if (column >= 0 && column < m) {
                A_v(column, row) = y[j];
            }
        }
    }
    // operate matrix and
    AT_v.Transpose(A_v);
    MB_v = (AT_v) * (A_v);
    std::cout << " Start to Invert V Matrix" << std::endl;
    MB_v.Invert();
    std::cout << " Finish to Invert V Matrix" << std::endl;
    FM_v = (MB_v) * (AT_v);

    TMatrixDSparse A_w(m, n);
    TMatrixDSparse AT_w(n, m);
    TMatrixD MB_w(n, n);
    TMatrixD FM_w(n, m);

    // treat filter ...
    for (int i = 0; i != 40; i++) {
        x[i] = (i - 20 + 0.5) * 0.5;
        y[i] = gw->Eval(x[i] + 0.5 / 4.);
        y[i] += gw->Eval(x[i] - 0.5 / 4.);
    }
    // fill matrix
    for (int i = 0; i != n; i++) {
        for (int j = 0; j != 40; j++) {
            Int_t row = i;
            Int_t column = scale * i - 20 + j + 1;
            if (column >= 0 && column < m) {
                A_w(column, row) = y[j];
            }
        }
    }
    // operate matrix and
    AT_w.Transpose(A_w);
    MB_w = (AT_w) * (A_w);
    std::cout << " Start to Invert W Matrix" << std::endl;
    MB_w.Invert();
    std::cout << " End to Invert W Matrix" << std::endl;
    FM_w = (MB_w) * (AT_w);

    const Frame &frame1 = fds.get();
    size_t ntraces = frame1.traces.size();

    for (size_t ind = 0; ind < ntraces; ++ind) {
        const Trace &trace = frame1.traces[ind];
        int tbin = trace.tbin;
        int chid = trace.chid;
        int nbins = trace.charge.size();

        TVectorD rx(n);
        TVectorD ry(m);

        // fill raw data ...
        for (int i = 0; i != m; i++) {
            ry[i] = trace.charge.at(i);
        }

        // if (chid ==447) ry.Print();

        TH1F *htemp;
        TGraph *gfilter;
        if (chid < nwire_u) {
            htemp = hu[chid];
            rx = FM_u * ry;
        }
        else if (chid < nwire_u + nwire_v) {
            htemp = hv[chid - nwire_u];
            rx = FM_v * ry;
        }
        else {
            htemp = hw[chid - nwire_u - nwire_v];
            rx = FM_w * ry;
        }
        for (int i = 0; i != n; i++) {
            htemp->SetBinContent(i + 1, rx[i]);
        }

        Trace t;
        t.chid = chid;
        t.tbin = tbin;
        t.charge.resize(bins_per_frame, 0.0);
        for (int j = 0; j != bins_per_frame; j++) {
            t.charge.at(j) = htemp->GetBinContent(j + 1);
        }
        frame.traces.push_back(t);
    }

    frame.index = frame_number;
    return frame.index;
}

WCP2dToy::ToySignalPreFDS::~ToySignalPreFDS()
{
    for (int i = 0; i != nwire_u; i++) {
        delete hu[i];
    }
    delete hu;
    for (int i = 0; i != nwire_v; i++) {
        delete hv[i];
    }
    delete hv;
    for (int i = 0; i != nwire_w; i++) {
        delete hw[i];
    }
    delete hw;

    delete hfilter_u;
    delete hfilter_v;
    delete hfilter_w;

    delete hfilter_time_u;
    delete hfilter_time_v;
    delete hfilter_time_w;

    delete gu;
    delete gv;
    delete gw;
}
