#include "WCP2dToy/ToyMatrixExclusive.h"
using namespace WCP;

#include "TMath.h"

typedef std::pair<int, double> EigenPair;
struct EigenCompare {
    bool operator()(const EigenPair &a, const EigenPair &b) const
    {
        if (a.second == b.second) {
            return a.first < b.first;
        }
        return a.second > b.second;
    }
};
typedef std::set<EigenPair, EigenCompare> EigenSet;

WCP2dToy::ToyMatrixExclusive::ToyMatrixExclusive(WCP2dToy::ToyMatrix &toymatrix)
{
    Eigen = new TMatrixDEigen(*(toymatrix.Get_MC()));
    EigenValue = new TVectorD(Eigen->GetEigenValuesRe());
    trans = new TMatrixD(Eigen->GetEigenVectors());
    // transT= new TMatrixD(*trans);
    // transT->Transpose(*trans);

    mcindex = toymatrix.Get_mcindex();
    mwindex = toymatrix.Get_mwindex();
    swindex = toymatrix.Get_swindex();
    numz = 0;
    TVectorD temp(mcindex);
    for (int i = 0; i != mcindex; i++) {
        // std::cout << (*EigenValue)[i] << std::endl;
        if (fabs((*EigenValue)[i]) < 1e-5) {
            for (int j = 0; j != mcindex; j++) {
                temp[j] += (*trans)(j, i);
            }
            numz++;
        }
    }

    EigenSet eset;
    for (int i = 0; i != mcindex; i++) {
        EigenPair abc(i, fabs(temp[i]));
        eset.insert(abc);
    }
    chi2 = 1e9;
    std::vector<int> flag;
    for (auto it = eset.begin(); it != eset.end(); it++) {
        if (flag.size() != numz) {
            flag.push_back(it->first);
        }
        else {
            break;
        }
        //   std::cout << it->first << " " << it->second << std::endl;
    }

    //  trans->Print();
    // std::cout << (*trans)(44,45) << std::endl; // second number is column, first number is row
    // std::cout << num_size << " " << numz << " " <<
    // TMath::Factorial(num_size)/TMath::Factorial(numz)/TMath::Factorial(num_size-numz) << std::endl;

    // for (int i=0;i!=numz;i++){
    //   flag.push_back(i);
    // }

    // numz = 26;
    // //flag.push_back(1);
    // flag.push_back(2);  flag.push_back(3);  flag.push_back(4);
    // // flag.push_back(5);  flag.push_back(6);
    // flag.push_back(10); flag.push_back(13); flag.push_back(14);
    // flag.push_back(15); flag.push_back(16); flag.push_back(17);  flag.push_back(19);
    // flag.push_back(21); flag.push_back(22); // flag.push_back(24);
    // flag.push_back(25); flag.push_back(26); flag.push_back(27); flag.push_back(28); //flag.push_back(29);
    // flag.push_back(30); flag.push_back(31); flag.push_back(32);  flag.push_back(34);
    // flag.push_back(35); flag.push_back(37); flag.push_back(38); //flag.push_back(39);
    // flag.push_back(41); flag.push_back(42);  flag.push_back(44);
    // //flag.push_back(45);

    MA = new TMatrixD(mwindex, mcindex - numz);
    MAT = new TMatrixD(mcindex - numz, mwindex);
    Vx = new TMatrixD(mcindex - numz, mcindex - numz);
    Vx_inv = new TMatrixD(mcindex - numz, mcindex - numz);
    MC = new TMatrixD(mcindex - numz, mcindex - numz);
    MC_inv = new TMatrixD(mcindex - numz, mcindex - numz);

    Cxt = new TVectorD(mcindex - numz);
    dCxt = new TVectorD(mcindex - numz);

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

    std::cout << numz << " " << flag.size() << " " << mcindex - numz - 1 << std::endl;
    ncount = 0;
    int flag1 = 1;
    int ncount1 = 0;

    double limit = TMath::Factorial(mcindex) / TMath::Factorial(numz) / TMath::Factorial(mcindex - numz);

    std::cout << limit << std::endl;

    if (flag.size() != 0) {
        while (ncount1 < 100 && ncount < 1e6 && ncount < fabs(2 * limit)) {
            if (ncount % 100000 == 0 && ncount != 0) std::cout << ncount << std::endl;
            for (int i = 0; i != flag.size(); i++) {
                for (int j = 0; j != mcindex - numz - 1; j++) {
                    if (Solve(flag, toymatrix) == 1 && chi2 < mwindex + 5 * sqrt(mwindex)) flag1 = 0;
                    move(flag, i, mcindex);
                }
            }
            if (flag1 == 0) ncount1++;
        }
    }
    else {
        Solve(flag, toymatrix);
    }
}

WCP2dToy::ToyMatrixExclusive::~ToyMatrixExclusive()
{
    delete Eigen;
    delete EigenValue;
    delete MA;
    delete MAT;
    delete MC;
    delete MC_inv;
    delete Vx;
    delete Vx_inv;
    delete Cxt;
    delete dCxt;
    // delete Cx, dCx;
    delete trans;
    // delete transT;
}

int WCP2dToy::ToyMatrixExclusive::Solve(std::vector<int> &flag, WCP2dToy::ToyMatrix &toymatrix)
{
    const TMatrixD *MA_big = toymatrix.Get_MA();
    const TMatrixD *VBy = toymatrix.Get_VBy();
    const TMatrixD *VBy_inv = toymatrix.Get_VBy_inv();
    const TMatrixD *MB = toymatrix.Get_MB();
    const TVectorD *Wy = toymatrix.Get_Wy();

    int index = 0;
    for (int i = 0; i != mcindex; i++) {
        auto it = find(flag.begin(), flag.end(), i);
        if (it == flag.end()) {
            for (int j = 0; j != mwindex; j++) {
                (*MA)(j, index) = (*MA_big)(j, i);
            }
            // std::cout << index << std::endl;
            index++;
        }
    }

    // MA->Print();

    MAT->Transpose(*MA);
    *MC = (*MAT) * (*VBy_inv) * (*MA);
    double det = MC->Determinant();
    ncount++;

    if (fabs(det) < 1e-5) {
        return 0;
    }
    else {
        *MC_inv = *MC;
        MC_inv->Invert();
        *Cxt = (*MC_inv) * (*MAT) * (*VBy_inv) * (*MB) * (*Wy);
        *Vx_inv = (*MAT) * (*VBy_inv) * (*MA);
        *Vx = *Vx_inv;
        Vx->Invert();

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

        TVectorD sol = (*MB) * (*Wy) - (*MA) * (*Cxt);

        // sol.Print();

        TVectorD sol1 = (*VBy_inv) * sol;
        chi2t = sol * (sol1) / 1e6;

        for (int i = 0; i != mcindex - numz; i++) {
            if ((*Cxt)[i] < 0) {
                chi2t += 10 * pow((*Cxt)[i] / (*dCxt)[i], 2);
            }
        }

        if (chi2t < chi2) {
            // copy the Cx etc
            chi2 = chi2t;
            toymatrix.Set_chi2(chi2);
            toymatrix.Set_ndf(mwindex - (mcindex - flag.size()));
            toymatrix.Set_Solve_Flag(3);
            index = 0;
            for (int i = 0; i != mcindex; i++) {
                auto it = find(flag.begin(), flag.end(), i);
                if (it == flag.end()) {
                    toymatrix.Set_value((*Cxt)[index], i);
                    toymatrix.Set_error((*dCxt)[index], i);
                    index++;
                }
                else {
                    toymatrix.Set_value(0, i);
                    toymatrix.Set_error(0, i);
                }
            }
            // for (int i=0;i!=numz;i++){
            // 	std::cout << flag[i] << std::endl;
            // }
            std::cout << "xin: " << chi2 << " " << ncount << std::endl;
        }
        // std::cout << det << std::endl;
        return 1;
    }
}

void WCP2dToy::ToyMatrixExclusive::move(std::vector<int> &flag, int index, int max)
{
    int temp = flag.at(index);
    auto it = find(flag.begin(), flag.end(), temp);
    while (it != flag.end()) {
        temp++;
        if (temp >= max) temp = 0;
        it = find(flag.begin(), flag.end(), temp);
    }
    flag.at(index) = temp;
}
