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

#include "TMath.h"

int WCP2dToy::ToyMatrixKalman::Cal_numz(WCP2dToy::ToyMatrix &toymatrix)
{
    // std::cout << no_need_remove.size() << std::endl;
    if (no_need_remove.size() != 0) {
        TMatrixD MA2(mwindex, no_need_remove.size());
        TMatrixD MA2T(no_need_remove.size(), mwindex);
        TMatrixD MC2(no_need_remove.size(), no_need_remove.size());

        const TMatrixD *MA_big = toymatrix.Get_MA();
        const TMatrixD *VBy_inv = toymatrix.Get_VBy_inv();

        int index2 = 0;
        for (int k = 0; k != mcindex; k++) {  // loop all possibility
            auto it3 = find(no_need_remove.begin(), no_need_remove.end(), k);
            if (it3 != no_need_remove.end()) {  // not in removed
                for (int j = 0; j != mwindex; j++) {
                    MA2(j, index2) = (*MA_big)(j, k);
                }
                index2++;
            }
        }

        MA2T.Transpose(MA2);
        MC2 = (MA2T) * (*VBy_inv) * (MA2);
        TMatrixDEigen Eigen2(MC2);
        TVectorD EigenValue2(Eigen2.GetEigenValuesRe());

        int numz2 = 0;
        for (int k = 0; k != no_need_remove.size(); k++) {
            if (fabs(EigenValue2[k]) < 1e-5) {
                numz2++;
            }
        }
        return numz2;
    }
    else {
        return 0;
    }
}

void WCP2dToy::ToyMatrixKalman::init(WCP2dToy::ToyMatrix &toymatrix)
{
    ncount = 0;
    numz = 0;

    mcindex = toymatrix.Get_mcindex();
    mwindex = toymatrix.Get_mwindex();
    swindex = toymatrix.Get_swindex();

    int n_removed = already_removed.size();

    // MA = new TMatrixD(mwindex,mcindex-n_removed);
    // MAT = new TMatrixD(mcindex-n_removed,mwindex);

    // MC = new TMatrixD(mcindex-n_removed,mcindex-n_removed);
    // MC_inv = new TMatrixD(mcindex-n_removed,mcindex-n_removed);

    // std::cout << " Xin " << already_removed.size() << " " << no_need_remove.size() << std::endl;

    TMatrixD MA(mwindex, mcindex - n_removed);
    TMatrixD MAT(mcindex - n_removed, mwindex);

    TMatrixD MC(mcindex - n_removed, mcindex - n_removed);
    TMatrixD MC_inv(mcindex - n_removed, mcindex - n_removed);

    const TMatrixD *MA_big = toymatrix.Get_MA();
    const TMatrixD *VBy_inv = toymatrix.Get_VBy_inv();

    int index2 = 0;
    for (int i = 0; i != mcindex; i++) {
        auto it1 = find(already_removed.begin(), already_removed.end(), i);
        if (it1 == already_removed.end()) {
            for (int j = 0; j != mwindex; j++) {
                MA(j, index2) = (*MA_big)(j, i);
            }
            index2++;
        }
    }
    MAT.Transpose(MA);

    MC = (MAT) * (*VBy_inv) * (MA);

    TMatrixDEigen Eigen(MC);
    TVectorD EigenValue(Eigen.GetEigenValuesRe());

    for (int i = 0; i != mcindex - n_removed; i++) {
        if (fabs(EigenValue[i]) < 1e-5) {
            numz++;
        }
    }

    if (numz != 0) {
        if (flag_no_need == 1) {
            for (int i = 0; i != mcindex; i++) {  // loop all possibility
                // std::cout << i << " " << mcindex << std::endl;

                auto it1 = find(already_removed.begin(), already_removed.end(), i);
                auto it2 = find(no_need_remove.begin(), no_need_remove.end(), i);
                if (it1 == already_removed.end() && it2 == no_need_remove.end()) {  // not in removed
                    TMatrixD MA1(mwindex, mcindex - n_removed - 1);
                    TMatrixD MA1T(mcindex - n_removed - 1, mwindex);
                    TMatrixD MC1(mcindex - n_removed - 1, mcindex - n_removed - 1);

                    int index1 = 0;
                    for (int k = 0; k != mcindex; k++) {
                        auto it1 = find(already_removed.begin(), already_removed.end(), k);
                        if (it1 == already_removed.end() && k != i) {
                            for (int j = 0; j != mwindex; j++) {
                                MA1(j, index1) = (*MA_big)(j, k);
                            }
                            index1++;
                        }
                    }
                    MA1T.Transpose(MA1);

                    MC1 = (MA1T) * (*VBy_inv) * (MA1);

                    TMatrixDEigen Eigen1(MC1);
                    TVectorD EigenValue1(Eigen1.GetEigenValuesRe());

                    int numz1 = 0;
                    for (int k = 0; k != mcindex - n_removed - 1; k++) {
                        if (fabs(EigenValue1[k]) < 1e-5) {
                            numz1++;
                        }
                    }

                    if (numz == numz1) {
                        no_need_remove.push_back(i);
                        // if (check_flag!=0){
                        // //   TMatrixD MA2(mwindex,no_need_remove.size()+1);
                        // //   TMatrixD MA2T(no_need_remove.size()+1,mwindex);
                        // //   TMatrixD MC2(no_need_remove.size()+1,no_need_remove.size()+1);

                        // //   int index2 = 0;
                        // //   for (int k=0;k!=mcindex;k++){ //loop all possibility
                        // //     auto it3 = find(no_need_remove.begin(),no_need_remove.end(),k);
                        // //     if (it3 != no_need_remove.end() || k==i){ // not in removed
                        // //   	for (int j=0;j!=mwindex;j++){
                        // //   	  MA2(j,index2) = (*MA_big)(j,k);
                        // //   	}
                        // //   	index2 ++;
                        // //     }
                        // //   }

                        // //   MA2T.Transpose(MA2);
                        // //   MC2 = (MA2T) * (*VBy_inv) * (MA2);
                        // //   TMatrixDEigen Eigen2(MC2);
                        // //   TVectorD EigenValue2(Eigen2.GetEigenValuesRe());

                        // //   int numz2 = 0;
                        // //   for (int k=0;k!=no_need_remove.size()+1;k++){
                        // //     if (fabs(EigenValue2[k])<1e-5){
                        // //   	numz2 ++;
                        // //     }
                        // //   }
                        // //   if (numz2==0){

                        //   // }else{
                        //   // //   //std::cout << numz2 << std::endl;
                        //   // }
                        // }else{
                        //   no_need_remove.push_back(i);
                        // }
                    }
                }
            }
        }
    }
    else {
        MC_inv = MC;
        MC_inv.Invert();

        const TMatrixD *MB = toymatrix.Get_MB();
        const TVectorD *Wy = toymatrix.Get_Wy();

        TVectorD Cxt(mcindex - n_removed);
        TVectorD dCxt(mcindex - n_removed);
        TMatrixD Vx(mcindex - n_removed, mcindex - n_removed);
        TMatrixD Vx_inv(mcindex - n_removed, mcindex - n_removed);

        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 - n_removed; i++) {
            dCxt[i] = sqrt(Vx(i, i)) * 1000.;
        }

        TVectorD sol = (*MB) * (*Wy) - (MA) * (Cxt);
        TVectorD sol1 = (*VBy_inv) * sol;
        double chi2 = sol * (sol1) / 1e6;

        for (int i = 0; i != mcindex - already_removed.size(); i++) {
            if (Cxt[i] < 0) {
                chi2 += 10 * pow(Cxt[i] / dCxt[i], 2);
            }
        }
        chi2 += chi2_penalty;

        // std::cout << numz << " " << chi2 << " ";
        // for (int i=0;i!=already_removed.size();i++){
        //   std::cout << already_removed.at(i) << " ";
        // }
        // std::cout << std::endl;

        if (chi2 < toymatrix.Get_Chi2() || toymatrix.Get_Chi2() == -1) {
            // copy the Cx etc
            toymatrix.Set_chi2(chi2);
            toymatrix.Set_ndf(mwindex - (mcindex - already_removed.size()));
            toymatrix.Set_Solve_Flag(2);
            int index = 0;
            for (int i = 0; i != mcindex; i++) {
                auto it = find(already_removed.begin(), already_removed.end(), i);
                if (it == already_removed.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);
                }
            }
        }

        ncount++;
    }
}

WCP2dToy::ToyMatrixKalman::ToyMatrixKalman(WCP2dToy::ToyMatrix &toymatrix, int flag_no_need, double chi2_penalty)
  : flag_no_need(flag_no_need)
  , chi2_penalty(chi2_penalty)
{
    check_flag = 0;
    init(toymatrix);

    //  std::cout << numz << std::endl;
    //  std::cout << already_removed.size() << " " << no_need_remove.size() << std::endl;
}

WCP2dToy::ToyMatrixKalman::ToyMatrixKalman(std::vector<int> &already_removed1, std::vector<int> &no_need_remove1,
                                           WCP2dToy::ToyMatrix &toymatrix, int check, int flag_no_need,
                                           double chi2_penalty)
  : flag_no_need(flag_no_need)
  , chi2_penalty(chi2_penalty)
{
    already_removed = already_removed1;
    no_need_remove = no_need_remove1;
    check_flag = check;
    init(toymatrix);

    //  std::cout << numz << std::endl;
    //  std::cout << already_removed.size() << " " << no_need_remove.size() << std::endl;
}

WCP2dToy::ToyMatrixKalman::~ToyMatrixKalman()
{
    // already_removed.clear();
    // no_need_remove.clear();

    // delete MA, MAT;
    // delete MC, MC_inv;
    // delete Eigen, EigenValue;
}
