#include "WCP2dToy/MatrixSolver.h"
#include "TDecompChol.h"

#include "WCPRess/LassoModel.h"
#include "WCPRess/ElasticNetModel.h"

using namespace Eigen;

using namespace WCP;

WCP2dToy::MatrixSolver::MatrixSolver(GeomCellSelection& allmcell, GeomWireSelection& mwires,
                                     WCP::GeomCellMap& cell_wire_map, WCP::GeomWireMap& wire_cell_map,
                                     WCP::WireChargeMap& wire_charge_map, WCP::WireChargeMap& wire_charge_error_map)
{
    //
    solve_flag = -1;

    // build up the index
    mcindex = 0;
    mwindex = 0;
    swindex = 0;

    for (int j = 0; j != allmcell.size(); j++) {
        const MergeGeomCell* mcell = (MergeGeomCell*) allmcell[j];
        if (mcimap.find(mcell) == mcimap.end()) {
            mcimap[mcell] = mcindex;
            mcindex++;
            const GeomWireSelection wires = cell_wire_map[mcell];
            // std::cout << wires.size() << std::endl;
            for (int k = 0; k != wires.size(); k++) {
                // construct merged wire index
                const MergeGeomWire* mwire = (MergeGeomWire*) wires[k];
                // require all the wire must be good to be used in the matrix solving
                if (mwimap.find(mwire) == mwimap.end() && wire_charge_map[mwire] > 10) {
                    // if (mwimap.find(mwire) == mwimap.end() ){
                    mwimap[mwire] = mwindex;
                    mwindex++;

                    // construct single wire index
                    GeomWireSelection swires = mwire->get_allwire();
                    for (int kk = 0; kk != swires.size(); kk++) {
                        const GeomWire* wire1 = swires[kk];
                        if (swimap.find(wire1) == swimap.end()) {
                            swimap[wire1] = swindex;
                            swindex++;
                        }
                    }
                }
            }
        }
    }

    //  std::cout << swindex << " " << mwindex << " " << mcindex << std::endl;

    if (mcindex > 0) {
        // Construct Matrix
        TMatrixD* MA = new TMatrixD(mwindex, mcindex);
        TMatrixD* MB = new TMatrixD(mwindex, swindex);
        TMatrixD* MAT = new TMatrixD(mcindex, mwindex);
        TMatrixD* MBT = new TMatrixD(swindex, mwindex);

        TMatrixD* Vy = new TMatrixD(swindex, swindex);
        TMatrixD* VBy = new TMatrixD(mwindex, mwindex);
        TMatrixD* VBy_inv = new TMatrixD(mwindex, mwindex);
        TMatrixD* Vx = new TMatrixD(mcindex, mcindex);
        TMatrixD* Vx_inv = new TMatrixD(mcindex, mcindex);

        TMatrixD* MC = new TMatrixD(mcindex, mcindex);
        TMatrixD* MC_inv = new TMatrixD(mcindex, mcindex);

        // Construct Vector
        Wy = new TVectorD(swindex);

        MWy = new TVectorD(mwindex);
        MWy_pred = new TVectorD(mwindex);

        Cx = new TVectorD(mcindex);
        dCx = new TVectorD(mcindex);

        // Int_t n_data = 0;
        // Int_t irow[100000];
        // Int_t icol[100000];
        // Double_t data[100000];

        // fill in the matrix content ...
        for (auto it = wire_charge_map.begin(); it != wire_charge_map.end(); it++) {
            if (swimap.find(it->first) != swimap.end()) {
                int index = swimap[it->first];
                float charge = wire_charge_map[it->first];
                float charge_err = wire_charge_error_map[it->first];
                (*Wy)[index] = charge;
                (*Vy)(index, index) = charge_err * charge_err / 1e6;

                // if (charge_err==0)
                //   std::cout << index << " " << charge << " " << charge_err << std::endl;
            }
        }

        for (int j = 0; j != mwires.size(); j++) {
            if (mwimap.find(mwires[j]) != mwimap.end()) {
                int index = mwimap[mwires[j]];
                // construct MA
                for (int k = 0; k != wire_cell_map[mwires[j]].size(); k++) {
                    int index1 = mcimap[wire_cell_map[mwires[j]].at(k)];
                    (*MA)(index, index1) = 1;
                }

                // construct MB
                for (int k = 0; k != ((MergeGeomWire*) mwires[j])->get_allwire().size(); k++) {
                    int index1 = swimap[((MergeGeomWire*) mwires[j])->get_allwire().at(k)];
                    (*MB)(index, index1) = 1;
                }
            }
        }

        // MA->Print();
        // // construct the rest of matrix
        MBT->Transpose(*MB);
        MAT->Transpose(*MA);

        *VBy = (*MB) * (*Vy) * (*MBT);

        //    std::cout << "fill in the data" << std::endl;

        *VBy_inv = *VBy;
        //    if (fabs(VBy->Determinant())<0.01) {
        // std::cout << "Problem " << VBy->Determinant() << std::endl;
        //}
        VBy_inv->Invert();

        // std::cout << "invert the matrix " << std::endl;
        *MC = (*MAT) * (*VBy_inv) * (*MA);

        solve_flag = 0;

        // regularization need to choose to balance the  chisquare and total charge
        // calculate the total wire charge
        float total_wire_charge = 0;
        scale_factor = 1000;
        *MWy = (*MB) * (*Wy);
        for (int i = 0; i != mwindex; i++) {
            total_wire_charge += (*MWy)[i];
        }
        lambda = 3. / total_wire_charge / 2. * scale_factor;  // guessed regularization strength
        // std::cout << mwindex << " " << total_wire_charge/3. << std::endl;

        // now try to make the equation ...
        // MWy - MA * C
        // decompose VBy_inv
        // VBy_inv->Print();

        TMatrixD* UMA = new TMatrixD(mwindex, mcindex);
        TVectorD* UMWy = new TVectorD(mwindex);

        TDecompChol test(*VBy_inv);
        test.Decompose();
        TMatrixD U(mwindex, mwindex);
        U = test.GetU();
        *UMA = U * (*MA);
        U *= 1. / scale_factor;  // error needs to be scale down by 1000 ...
        *UMWy = U * (*MWy);

        //  std::cout << total_wire_charge/3./scale_factor/mcindex*0.005 << std::endl;
        TOL = total_wire_charge / 3. / scale_factor / mcindex * 0.005;  //  0.5% charge stability ...
        //  TOL = 1e-3; // original

        // fill in the results ...
        W = VectorXd::Zero(mwindex);
        G = MatrixXd::Zero(mwindex, mcindex);
        for (int i = 0; i != mwindex; i++) {
            W(i) = (*UMWy)(i);
            //      std::cout << "W: " << i << " " << W(i) << std::endl;
            for (int j = 0; j != mcindex; j++) {
                G(i, j) = (*UMA)(i, j);
                //	std::cout << "G: " << i << " " << j << " " << G(i,j) << std::endl;
            }
        }

        delete MA;
        delete MB;
        delete MAT;
        delete MBT;
        delete Vy;
        delete VBy;
        delete Vx;
        delete VBy_inv;
        delete Vx_inv;
        delete MC;
        delete MC_inv;

        delete UMA;
        delete UMWy;

        // Use direct to solve ...
        // double det;
        // if (mcindex <= mwindex){
        //   det = MC->Determinant();
        // }else{
        //   det = 0;
        // }

        // if (det > 1e-5){
        //   Direct_Solve();
        // }

        // else{
        // first time L1 solve ...
        // L1_Solve();
        //}

        // std::cout << det << std::endl;
    }
}
void WCP2dToy::MatrixSolver::L1_Solve(std::map<const GeomCell*, double, WCP::GeomCellComparep>& cell_weight_map)
{
    // MA->Print();
    // UMA.Print();

    // std::cout << "params: " << lambda << " " << 100000 << " " << TOL << " " << true << std::endl;

    WCP::LassoModel m2(lambda, 100000, TOL, true);
    m2.SetData(G, W);
    // set weights
    for (auto it = mcimap.begin(); it != mcimap.end(); it++) {
        const GeomCell* mcell = it->first;
        int index = it->second;
        m2.SetLambdaWeight(index, cell_weight_map[mcell]);
        // std::cout << "reg: " << index << " " << cell_weight_map[mcell] << std::endl;
    }

    m2.Fit();

    VectorXd beta = m2.Getbeta();
    int nbeta = beta.size();
    L1_ndf = mwindex;
    for (int i = 0; i != nbeta; i++) {
        (*Cx)[i] = beta(i) * scale_factor;
        // print hack ...
        // std::cout << "solved charge " << i << " " << (*Cx)[i] << std::endl;
        (*dCx)[i] = 0;
        if (beta(i) != 0) L1_ndf--;
    }
    L1_chi2_base = m2.chi2_base();
    L1_chi2_penalty = m2.chi2_l1();

    // std::cout << "Xin" << " " << m2.chi2_base() << " " << m2.chi2_l1()  << " " << L1_ndf << std::endl;

    // for (int i=0;i!=nbeta;i++){
    //   std::cout << beta(i) << std::endl;
    // }

    // Y.Print();
    //  MWy->Print();
    // Modify MWy, MA

    solve_flag = 2;
}

GeomCellSelection WCP2dToy::MatrixSolver::get_all_cells()
{
    GeomCellSelection cells;
    for (auto it = mcimap.begin(); it != mcimap.end(); it++) {
        cells.push_back(it->first);
    }
    return cells;
}

double WCP2dToy::MatrixSolver::get_mcell_charge(MergeGeomCell* mcell)
{
    double charge = 0;

    if (mcimap.find(mcell) != mcimap.end()) {
        charge = (*Cx)[mcimap[mcell]];
    }

    return charge;
}

// void WCP2dToy::MatrixSolver::Direct_Solve(){
//   *MC_inv = *MC;
//   MC_inv->Invert();
//   *Cx = (*MC_inv) * (*MAT) * (*VBy_inv) * (*MB) * (*Wy);

//   *Vx_inv = (*MAT) * (*VBy_inv) * (*MA);
//   *Vx = *Vx_inv;
//   Vx->Invert();

//   for (int i=0;i!=mcindex;i++){
//     (*dCx)[i] = sqrt( (*Vx)(i,i)) * 1000.;
//   }

//   TVectorD sol = (*MB) * (*Wy) - (*MA) * (*Cx);
//   TVectorD sol1 =  (*VBy_inv) * sol;
//   direct_chi2 = sol * (sol1)/1e6;

//   direct_ndf = mwindex - mcindex;

//   // std::cout << chi2 << " " << ndf << std::endl;
//   // for (int i=0;i!=mcindex;i++){
//   //   std::cout << (*Cx)[i] << " " << (*dCx)[i] << std::endl;
//   // }

//   // MA->Print();
//   *MWy = (*MB) * (*Wy);
//   *MWy_pred = (*MA)*(*Cx);
//   // for (int i=0;i!=mwindex;i++){
//   //   std::cout << (*MWy)[i] << std::endl;
//   // }
//   solve_flag = 1;
// }

int WCP2dToy::MatrixSolver::get_mc_index(const WCP::GeomCell* cell)
{
    auto it = mcimap.find(cell);
    if (it != mcimap.end()) {
        return it->second;
    }
    else {
        return -1;
    }
}
int WCP2dToy::MatrixSolver::get_mw_index(const WCP::GeomWire* wire)
{
    auto it = mwimap.find(wire);
    if (it != mwimap.end()) {
        return it->second;
    }
    else {
        return -1;
    }
};
double WCP2dToy::MatrixSolver::get_W_value(int index)
{
    if (index < mwindex && index >= 0)
        return W(index);
    else
        return 0;
};
double WCP2dToy::MatrixSolver::get_G_value(int index_mw, int index_mc)
{
    if (index_mw < mwindex && index_mc < mcindex && index_mw >= 0 && index_mc >= 0)
        return G(index_mw, index_mc);
    else
        return 0;
};

WCP2dToy::MatrixSolver::~MatrixSolver()
{
    delete Wy;
    delete Cx;
    delete dCx;
    delete MWy_pred;
    delete MWy;
}
