#include "WCP2dToy/ToyMatrix.h"

#include "TDecompSVD.h"

using namespace WCP;

void WCP2dToy::ToyMatrix::JudgeSimpleBlob(WCP2dToy::ToyTiling& toytiling, WCP2dToy::MergeToyTiling& mergetiling){
   GeomCellSelection allmcell = mergetiling.get_allcell();
   num_blob = 0;
   for (int j=0;j!=allmcell.size();j++){
     MergeGeomCell *mcell =(MergeGeomCell*)allmcell.at(j);
     
     double charge =Get_Cell_Charge(mcell);
     if (charge>recon_threshold){
       mcell->FindEdges();
       // std::cout << mcell->get_allcell().size() << " " << mcell->get_edgecells().size() << std::endl;
       int nwire = 0, max_wire = 0;
       int ncell = 0;
       if(mcell->IsBlob()) {
	 mcell->FindCorners(toytiling.cmap(), toytiling.wmap());
	 num_blob ++; 
	 // GeomWireSelection n_mwires = mergetiling[i]->wires(*mcell);
	 // for (int k=0;k!=n_mwires.size();k++){
	 //   int ncells = 0;
	 //   for (int kk= 0; kk!=mergetiling[i]->cells(*n_mwires.at(k)).size();kk++){
	 //     if (toymatrix[i]->Get_Cell_Charge(mergetiling[i]->cells(*n_mwires.at(k)).at(kk))>recon_threshold){
	 // 	ncells ++;
	 //     }
	 //   }
	 //   std::cout << ncells << std::endl;
	 
	 // find simple blob ... 
	 // find the merged wire regarding the merged blob
	 GeomWireSelection wires = mergetiling.wires(*mcell);
	 // find the number of wires 
	 for (int k =0; k!=wires.size();k++){
	   MergeGeomWire* mwire = (MergeGeomWire*)wires.at(k);
	   //std::cout << mwire->get_allwire().size() << std::endl;
	   nwire += mwire->get_allwire().size();
	   if (mwire->get_allwire().size() > max_wire) 
	     max_wire = mwire->get_allwire().size();
	   GeomCellSelection cells = mergetiling.cells(*mwire);
	   for (int kk=0; kk!= cells.size(); kk++){
	     if (cells.at(kk) != mcell && Get_Cell_Charge(cells.at(kk))>recon_threshold){
	       ncell += ((MergeGeomCell*)cells.at(kk))->get_allcell().size();
	     }
	   }
	 }
	 nwire -= max_wire;
	 // for the merged blob passed the cut, find the number of cells
	 //std::cout << "Xin: " << nwire << " " << max_wire << " " << ncell << std::endl;
	 if (ncell < nwire){
	   mcell->SetSimpleBlob(true);
	   simple_blob_reduction = true;
	 }else{
	   mcell->SetSimpleBlob(false);
	 }
       }
     }
     
     
    // 	GeomCellSelection corners = mcell->get_cornercells();
    // 	total_corner_cells.insert(total_corner_cells.end(),corners.begin(),corners.end());
    // 	for (int k=0;k!=mcell->get_allcell().size();k++){
    // 	  total_recon_cells.push_back(mcell->get_allcell().at(k));
    // 	  //get charge
    // 	  double sc_charge = 0;
    // 	  GeomCellMap scmap = toytiling[i]->cmap();
    // 	  WireChargeMap wcmap = toytiling[i]->wcmap();
    // 	  GeomWireSelection wires = scmap[mcell->get_allcell().at(k)];
    // 	  double aa[3];
    // 	  for (int q=0;q!=wires.size();q++){
    // 	    sc_charge += wcmap[wires.at(q)];
    // 	    aa[q] = wcmap[wires.at(q)];
    // 	  }
    // 	  total_scmap[mcell->get_allcell().at(k)] = sc_charge/3;
    // 	  total_scrms[mcell->get_allcell().at(k)] = rms(aa[0],aa[1],aa[2])*3./sc_charge;
    // 	  if (sc_charge/3 > charge_max) charge_max = sc_charge/3;
    // 	  if (sc_charge/3 < charge_min) charge_min = sc_charge/3;
    // 	}
    //   }
   }

    // toymatrix[i]->Set_blob(num_blob);
    
    // cout << "# of blobs " << toymatrix[i]->Get_blob() << endl;

}

WCP2dToy::ToyMatrix::ToyMatrix(){
  solve_flag = -1;
  chi2 = -1;
  svd_flag = 0;
  num_blob = 0;
  simple_blob_reduction = false;
  recon_threshold = 2000;
  
  mwindex = 1;
  mcindex = 1;

  MA = new TMatrixD(mwindex,mcindex);
  MB = new TMatrixD(mwindex,swindex);
  MAT = new TMatrixD(mcindex,mwindex);
  MBT = new TMatrixD(swindex,mwindex);
  
  Vy = new TMatrixD(swindex,swindex);
  VBy = new TMatrixD(mwindex,mwindex);
  VBy_inv = new TMatrixD(mwindex,mwindex);
  Vx = new TMatrixD(mcindex,mcindex);
  Vx_inv = new TMatrixD(mcindex,mcindex);
  
  MC = new TMatrixD(mcindex,mcindex);
  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);
  
}


WCP2dToy::ToyMatrix::ToyMatrix(WCP2dToy::ToyTiling& toytiling, WCP2dToy::MergeToyTiling& mergetiling, int svd_flag1, int recon_t){
  solve_flag = -1;
  chi2 = -1;
  svd_flag = svd_flag1;
  num_blob = 0;
  simple_blob_reduction = false;
  recon_threshold = recon_t;
  
  // build up index
  Buildup_index(mergetiling);
  
  //std::cout << mcindex << " " << mwindex << " " << swindex << std::endl;
  if (mcindex >0){
  //Construct Matrix
    MA = new TMatrixD(mwindex,mcindex); // merged wire to merged cell
    MB = new TMatrixD(mwindex,swindex); // merged wire to single wire (single channel?)
    MAT = new TMatrixD(mcindex,mwindex); 
    MBT = new TMatrixD(swindex,mwindex);
    
    Vy = new TMatrixD(swindex,swindex); // single wire (single channel)
    VBy = new TMatrixD(mwindex,mwindex);
    VBy_inv = new TMatrixD(mwindex,mwindex);
    Vx = new TMatrixD(mcindex,mcindex);
    Vx_inv = new TMatrixD(mcindex,mcindex);
    
    MC = new TMatrixD(mcindex,mcindex);
    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);
    
    GeomWireSelection allwire = toytiling.get_allwire();
    WireChargeMap wcmap = toytiling.wcmap();
    WireChargeMap wcemap = toytiling.wcemap();
    
    for (int j=0;j!=allwire.size();j++){
      if (swimap.find(allwire[j])!=swimap.end()){
	int index = swimap[allwire[j]];
	float charge = wcmap[allwire[j]];
	float charge_err = wcemap[allwire[j]];
	(*Wy)[index] =charge;
	(*Vy)(index,index) = charge_err*charge_err/1e6;
	// fill covariance matrix as well
	// Vy ...
	// need to differentiate which plane it is
	// WirePlaneType_t plane = allwire[j]->plane();
	// Double_t charge_noise;
	// if (plane == 0){
	//   charge_noise = 14000*0.05;
	// }else if (plane==1){
	//   charge_noise = 14000*0.03;
	// }else if (plane==2){
	//   charge_noise = 14000*0.02;
	// }
	// (*Vy)(index,index) = (charge_noise*charge_noise + 0.05*0.05 * charge*charge)/1e6;
      }
    }
    
    
    GeomWireSelection allmwire = mergetiling.get_allwire();
    for (int j=0;j!=allmwire.size();j++){
      if (mwimap.find(allmwire[j])!=mwimap.end()){
	int index = mwimap[allmwire[j]];
	//construct MA
	for (int k=0; k!=mergetiling.cells(*allmwire[j]).size();k++){
	  int index1 = mcimap[mergetiling.cells(*allmwire[j]).at(k)];
	  (*MA)(index,index1) = 1;
	}
	
	//construct MB
	for (int k=0;k!=((MergeGeomWire*)allmwire[j])->get_allwire().size();k++){
	  int index1 = swimap[((MergeGeomWire*)allmwire[j])->get_allwire().at(k)];
	  (*MB)(index,index1) = 1;
	}
      }
    }
    
    
    // // construct the rest of matrix
    MBT->Transpose(*MB);
    MAT->Transpose(*MA);
    
    *VBy = (*MB) * (*Vy) * (*MBT);
    
    
    
    *VBy_inv = *VBy;
    //    if (fabs(VBy->Determinant())<0.01) {
    // std::cout << "Problem " << VBy->Determinant() << std::endl;
    //}
    VBy_inv->Invert();
    
    *MC = (*MAT) * (*VBy_inv) * (*MA);
    //double det = MC->Determinant();
    double det;
    if (mcindex <= mwindex){
      det = MC->Determinant();
    }else{
      det = 0;
    }
    solve_flag = 0;
    if (svd_flag==0){
      if (det > 1e-5)
	Solve();
    }else{
      if (det > 1e-5){
	Solve();
      }else{
	Solve_SVD();
      }
      //Solve_SVD();
    }
  }else{
    //    solve_flag = 1;
  }
  //MA->Print();
  // MAT->Print();
  // MB->Print();
  // MBT->Print();
  // Vy->Print();
  
  
}

void WCP2dToy::ToyMatrix::Buildup_index(const WCP::DetectorGDS& gds, WCP2dToy::MergeToyTiling& mergetiling){
  mcindex = 0; // merged cell
  mwindex = 0; // merged wire
  swindex = 0; // single channel
  
  GeomCellSelection allmcell = mergetiling.get_allcell();

  for (int j=0;j!=allmcell.size();j++){
    //construct merged cell index
    const MergeGeomCell *mcell = (MergeGeomCell*)allmcell[j];
    if (mcimap.find(mcell) == mcimap.end()){
      mcimap[mcell] = mcindex;
      mcindex ++;
      
      const GeomWireSelection wires = mergetiling.wires(*allmcell[j]);
       for (int k=0;k!=wires.size();k++){
	 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() && mergetiling.wcmap()[mwire] >10){
	   //if (mwimap.find(mwire) == mwimap.end() ){
	   mwimap[mwire] = mwindex;
	   mwindex ++;
	   
	   //construct single channel index
	   GeomWireSelection swires = mwire->get_allwire();
	   for (int kk = 0; kk!=swires.size(); kk++){
	     const GeomWire* wire1 = swires[kk];
	     if (scimap.find(wire1->channel()) == scimap.end()){
	       scimap[wire1->channel()] = swindex;
	       swindex ++; 
	     }
	   }
	   
	 }
       }
    }
  }
}

WCP2dToy::ToyMatrix::ToyMatrix(const DetectorGDS& gds,WCP2dToy::ToyTiling& toytiling, WCP2dToy::MergeToyTiling& mergetiling, int recon_t){
  solve_flag = -1;
  chi2 = -1;
  svd_flag = 0;
  num_blob = 0;
  simple_blob_reduction = false;
  recon_threshold = recon_t;
  
  //building up index 
  Buildup_index(gds,mergetiling);
  //std::cout << mcindex << " " << mwindex << " " << swindex << std::endl;
  
  if (mcindex >0){
    //Construct Matrix
    MA = new TMatrixD(mwindex,mcindex);
    MB = new TMatrixD(mwindex,swindex);
    MAT = new TMatrixD(mcindex,mwindex);
    MBT = new TMatrixD(swindex,mwindex);
    
    Vy = new TMatrixD(swindex,swindex);
    VBy = new TMatrixD(mwindex,mwindex);
    VBy_inv = new TMatrixD(mwindex,mwindex);
    Vx = new TMatrixD(mcindex,mcindex);
    Vx_inv = new TMatrixD(mcindex,mcindex);
    
    MC = new TMatrixD(mcindex,mcindex);
    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);
    
    std::map<int, float> ccmap = toytiling.ccmap();
    std::map<int, float> ccemap = toytiling.ccemap();
    
    for ( auto it = ccmap.begin(); it != ccmap.end(); it++){
      if (scimap.find(it->first) != scimap.end()){
	int index = scimap[it->first];
	float charge = ccmap[it->first];
	float charge_err = ccemap[it->first];
	(*Wy)[index] =charge;
	(*Vy)(index,index) = charge_err*charge_err/1e6;
      }
    }
    
    GeomWireSelection allmwire = mergetiling.get_allwire();
    for (int j=0;j!=allmwire.size();j++){
      if (mwimap.find(allmwire[j])!=mwimap.end()){
	int index = mwimap[allmwire[j]];
	//construct MA
	for (int k=0; k!=mergetiling.cells(*allmwire[j]).size();k++){
	  int index1 = mcimap[mergetiling.cells(*allmwire[j]).at(k)];
	  (*MA)(index,index1) = 1;
	}

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

    // construct the rest of matrix
    MBT->Transpose(*MB);
    MAT->Transpose(*MA);
    
    *VBy = (*MB) * (*Vy) * (*MBT);
    *VBy_inv = *VBy;
    VBy_inv->Invert();
    
    *MC = (*MAT) * (*VBy_inv) * (*MA);
    solve_flag = 0;
    
    double det;
    if (mcindex <= mwindex){
      det = MC->Determinant();
    }else{
      det = 0;
    }
    if (det > 1e-5){
      Solve();
    }

  }else{
    //    solve_flag = 1;
  }
}

WCP2dToy::ToyMatrix::ToyMatrix(WCP2dToy::ToyTiling& toytiling, WCP2dToy::MergeToyTiling& mergetiling, int recon_t){
  solve_flag = -1;
  chi2 = -1;
  svd_flag = 0;
  num_blob = 0;
  simple_blob_reduction = false;
  // build up index
  Buildup_index(mergetiling);
  recon_threshold = recon_t;
  
  //std::cout << mcindex << " " << mwindex << " " << swindex << std::endl;
  if (mcindex >0){
  //Construct Matrix
    MA = new TMatrixD(mwindex,mcindex);
    MB = new TMatrixD(mwindex,swindex);
    MAT = new TMatrixD(mcindex,mwindex);
    MBT = new TMatrixD(swindex,mwindex);
    
    Vy = new TMatrixD(swindex,swindex);
    VBy = new TMatrixD(mwindex,mwindex);
    VBy_inv = new TMatrixD(mwindex,mwindex);
    Vx = new TMatrixD(mcindex,mcindex);
    Vx_inv = new TMatrixD(mcindex,mcindex);
    
    MC = new TMatrixD(mcindex,mcindex);
    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);
    
    GeomWireSelection allwire = toytiling.get_allwire();
    WireChargeMap wcmap = toytiling.wcmap();
    WireChargeMap wcemap = toytiling.wcemap();
    for (int j=0;j!=allwire.size();j++){
      if (swimap.find(allwire[j])!=swimap.end()){
	int index = swimap[allwire[j]];
	float charge = wcmap[allwire[j]];
	float charge_err = wcemap[allwire[j]];
	(*Wy)[index] =charge;
	(*Vy)(index,index) = charge_err*charge_err/1e6;
	// fill covariance matrix as well
	// Vy ...
	// need to differentiate which plane it is
	// WirePlaneType_t plane = allwire[j]->plane();
	// Double_t charge_noise;
	// if (plane == 0){
	//   charge_noise = 14000*0.05;
	// }else if (plane==1){
	//   charge_noise = 14000*0.03;
	// }else if (plane==2){
	//   charge_noise = 14000*0.02;
	// }
	// (*Vy)(index,index) = (charge_noise*charge_noise + 0.05*0.05 * charge*charge)/1e6;
      }
    }
    
    
    GeomWireSelection allmwire = mergetiling.get_allwire();
    for (int j=0;j!=allmwire.size();j++){
      if (mwimap.find(allmwire[j])!=mwimap.end()){
	int index = mwimap[allmwire[j]];
	//construct MA
	for (int k=0; k!=mergetiling.cells(*allmwire[j]).size();k++){
	  int index1 = mcimap[mergetiling.cells(*allmwire[j]).at(k)];
	  (*MA)(index,index1) = 1;
	}
	
	//construct MB
	for (int k=0;k!=((MergeGeomWire*)allmwire[j])->get_allwire().size();k++){
	  int index1 = swimap[((MergeGeomWire*)allmwire[j])->get_allwire().at(k)];
	  (*MB)(index,index1) = 1;
	}
      }
    }
    
    
    // // 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;
    
    double det;
    if (mcindex <= mwindex){
      det = MC->Determinant();
    }else{
      det = 0;
    }
    if (det > 1e-5){
      Solve();
    }
  }else{
    //    solve_flag = 1;
  }
  //MA->Print();
  // MAT->Print();
  // MB->Print();
  // MBT->Print();
  // Vy->Print();

  
}



void WCP2dToy::ToyMatrix::Update_pred(){
  *MWy = (*MB) * (*Wy);
  *MWy_pred = (*MA) * (*Cx);
}

void WCP2dToy::ToyMatrix::Print(){

  MA->Print();
  std::cout << std::endl;
  Cx->Print();

  for (int i=0;i!=mwindex;i++){
    std::cout << "wire: " << i << " " << (*MWy)[i] << " " << (*MWy_pred)[i] << " " << sqrt((*VBy)[i][i]) << std::endl;
  }
}

double WCP2dToy::ToyMatrix::Get_residual(const WCP::GeomCell *cell){
  double res=0;
  int index = mcimap[cell];

  for (int i=0;i!=mwindex;i++){
    res += (*MA)(i,index) * fabs((*MWy)[i] - (*MWy_pred)[i]);
  }
  
  return res;
}

WCP2dToy::ToyMatrix::~ToyMatrix(){
  
  
  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 Wy;
  delete Cx;
  delete dCx;
  delete MWy_pred;
  delete MWy;
}

int WCP2dToy::ToyMatrix::Solve(){
  
  // double det = MC->Determinant();

  //  std::cout << det << std::endl;
  
  //if (fabs(det)>1e-5){
    *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;
    chi2 = sol * (sol1)/1e6;
    
    ndf = mwindex - mcindex;
    
    //std::cout << chi2 << std::endl;
    //      for (int i=0;i!=mcindex;i++){
    //	std::cout << (*Cx)[i] << " " << (*dCx)[i]*1000. << std::endl;
    //}
    
    solve_flag = 1;
    //}
  
  return solve_flag;
}

int WCP2dToy::ToyMatrix::Solve_SVD(){
  
  // if (svd_removed.size()!=0){
  //   //update *MC
  //   int n_removed = svd_removed.size();
  //   TMatrixD MA2(mwindex,mcindex - n_removed);
  //   TMatrixD MA2T(mcindex - n_removed,mwindex);
    
  //   int index2 = 0;
  //   for (int k=0;k!=mcindex;k++){ //loop all possibility
  //     auto it3 = find(svd_removed.begin(),svd_removed.end(),k);
  //     if (it3 != svd_removed.end()){ // not in removed
  // 	for (int j=0;j!=mwindex;j++){
  // 	  MA2(j,index2) = (*MA)(j,k);
  // 	}
  // 	index2 ++;
  //     }
  //   }
  //   MA2T.Transpose(MA2);
  //   TMatrixD MC2(mcindex-svd_removed.size(),mcindex-svd_removed.size());
  //   TMatrixD MC2_inv(mcindex-svd_removed.size(),mcindex-svd_removed.size());

  //   MC2 = MA2T * (*VBy_inv) * MA2;
  //   TDecompSVD svd(MC2);
  //   MC2_inv = svd.Invert();
     
        
  //   TVectorD Cxt(mcindex-n_removed);
  //   TVectorD dCxt(mcindex-n_removed);
  //   TMatrixD Vxt(mcindex-n_removed,mcindex-n_removed);
  //   TMatrixD Vxt_inv(mcindex-n_removed,mcindex-n_removed);
    
  //   Cxt = (MC2_inv) * (MA2T) * (*VBy_inv) * (*MB) * (*Wy);
  //   Vxt_inv = (MA2T) * (*VBy_inv) * (MA2);
  //   Vxt = Vxt_inv;
  //   Vxt.Invert();
    
  //   for (int i=0;i!=mcindex-n_removed;i++){
  //     dCxt[i] = sqrt( Vxt(i,i)) * 1000.;
  //   }

  //   TVectorD sol = (*MB) * (*Wy) - (MA2) * (Cxt);
  //   TVectorD sol1 =  (*VBy_inv) * sol;
  //   chi2 = sol * (sol1)/1e6;
  //   ndf = mwindex - mcindex + n_removed;
  //   for (int i=0;i!=mcindex-n_removed;i++){
  //     if (Cxt[i] <0){
  // 	chi2 += 10*pow(Cxt[i]/dCxt[i],2);
  //     }
  //   }

  //   int index = 0;
  //   for (int i=0;i!=mcindex;i++){
  //     auto it = find(svd_removed.begin(),svd_removed.end(),i);
  //     if (it==svd_removed.end()){
  // 	(*Cx)[i] = Cxt[index];
  // 	(*dCx)[i] = dCxt[index];
  // 	index ++;
  //     }else{
  // 	(*Cx)[i] = 0.;
  // 	(*dCx)[i] = 0.;
  //     }
  //   }


  // }else{
  TDecompSVD svd(*MC);
  *MC_inv = svd.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.;
    if ((*Cx)[i]<0) svd_removed.push_back(i);
  }
  TVectorD sol = (*MB) * (*Wy) - (*MA) * (*Cx);
  TVectorD sol1 =  (*VBy_inv) * sol;
  //chi2 = sol * (sol1)/1e6;
  //ndf = mwindex - mcindex;
  solve_flag = 0;
  //}
    
  return solve_flag;
}



double WCP2dToy::ToyMatrix::Get_Cell_Charge( const WCP::GeomCell *cell, int flag )  {
  // flag == 1 charge
  // flag == 2 uncertainty
  int index = mcimap[cell];
  if (flag==1){
    return (*Cx)[index];
  }else if (flag==2){
    return (*dCx)[index];
  }
}


void WCP2dToy::ToyMatrix::Buildup_index(WCP2dToy::MergeToyTiling& mergetiling){

  mcindex = 0;
  mwindex = 0;
  swindex = 0;

  GeomCellSelection allmcell = mergetiling.get_allcell();
 
  for (int j=0;j!=allmcell.size();j++){
    //construct merged cell index
    const MergeGeomCell *mcell = (MergeGeomCell*)allmcell[j];
    if (mcimap.find(mcell) == mcimap.end()){
      mcimap[mcell] = mcindex;
      mcindex ++;
      
      const GeomWireSelection wires = mergetiling.wires(*allmcell[j]);
      // cout << wires.size() << 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() && mergetiling.wcmap()[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 << mcindex << " " << mwindex << " " << swindex << std::endl;
}


