#include "WCP2dToy/ToyMatrixMarkov.h"

using namespace WCP;
#include "TMath.h"
#include "TRandom.h"

void WCP2dToy::ToyMatrixMarkov::find_subset(WCP2dToy::ToyMatrixKalman &toymatrix,WCP2dToy::ToyMatrix &toymatrix1, std::vector<int>& vec){
  //std::cout << toymatrix.Get_already_removed().size() << " " << toymatrix.Get_no_need_remove().size() << " " << vec.size() << std::endl;

  if (toymatrix.Get_numz()!=0){
    for (int i=0;i!=toymatrix.Get_mcindex();i++){
      auto it1 = find(toymatrix.Get_already_removed().begin(),toymatrix.Get_already_removed().end(),i);
      auto it2 = find(toymatrix.Get_no_need_remove().begin(),toymatrix.Get_no_need_remove().end(),i);
      if (it1 == toymatrix.Get_already_removed().end() && it2 == toymatrix.Get_no_need_remove().end()){
	std::vector<int> already_removed = toymatrix.Get_already_removed();
	already_removed.push_back(i);
	WCP2dToy::ToyMatrixKalman kalman(already_removed,toymatrix.Get_no_need_remove(),toymatrix1,0,0);

	//	std::cout << "abc: " << i << " " <<kalman.Get_numz() << " " << toymatrix.Get_numz() << std::endl;
	if (kalman.Get_numz()==toymatrix.Get_numz()){
	}else{
	  find_subset(kalman,toymatrix1,vec);
	  //move to "no need to remove"??
	  toymatrix.Get_no_need_remove().push_back(i);
	  if (vec.size()!=0) break;
	}
      }
    }

    if (vec.size() == 0){
      for (int i=0;i!=toymatrix.Get_mcindex();i++){
	auto it1 = find(toymatrix.Get_already_removed().begin(),toymatrix.Get_already_removed().end(),i);
	auto it2 = find(toymatrix.Get_no_need_remove().begin(),toymatrix.Get_no_need_remove().end(),i);
	if (it1 == toymatrix.Get_already_removed().end() && it2 == toymatrix.Get_no_need_remove().end()){
	  for (int j=0;j!=toymatrix.Get_mcindex();j++){
	    if (i==j) continue;
	    auto it3 = find(toymatrix.Get_already_removed().begin(),toymatrix.Get_already_removed().end(),j);
	    auto it4 = find(toymatrix.Get_no_need_remove().begin(),toymatrix.Get_no_need_remove().end(),j);
	    if (it3 == toymatrix.Get_already_removed().end() && it4 == toymatrix.Get_no_need_remove().end()){
	      std::vector<int> already_removed = toymatrix.Get_already_removed();
	      already_removed.push_back(i);
	      already_removed.push_back(j);
	      WCP2dToy::ToyMatrixKalman kalman(already_removed,toymatrix.Get_no_need_remove(),toymatrix1,0,0);
	      
	      // std::cout << "abc: " << i << " " << j << " " << kalman.Get_numz() << " " << toymatrix.Get_numz() << std::endl;

	      if (kalman.Get_numz()==toymatrix.Get_numz()){
	      }else{
		find_subset(kalman,toymatrix1,vec);
		//move to "no need to remove"??
		// toymatrix.Get_no_need_remove().push_back(i);
		// toymatrix.Get_no_need_remove().push_back(j);
		if (vec.size()!=0) break;
	      }
	    }
	  }
	  if (vec.size()!=0) break;
	}
      }
    }
    

  }else{
    for (int i=0;i!=toymatrix.Get_mcindex();i++){
      auto it1 = find(toymatrix.Get_already_removed().begin(),toymatrix.Get_already_removed().end(),i);
      if (it1 == toymatrix.Get_already_removed().end()){
	vec.push_back(i);
      }
    }
  }
}

WCP2dToy::ToyMatrixMarkov::ToyMatrixMarkov(WCP2dToy::ToyMatrix &toycur,WCP2dToy::MergeToyTiling &mergecur, WCP::GeomCellSelection *allmcell1, WCP::GeomCellSelection &cells, int recon_t1, int recon_t2)
  : penalty_ncpt(0)
{
  ncount = 0;
  first_flag = 0;
  toymatrix = &toycur;
  allmcell = allmcell1;
  mcindex = toymatrix->Get_mcindex();
  recon_threshold1 = recon_t1;
  recon_threshold2 = recon_t2;
  use_time_threshold = 0.75;

  GeomCellSelection allmcell_c = mergecur.get_allcell();

  //form two vectors 
  std::vector<int> already_removed; //dummy
  
  for (int i=0;i!=allmcell_c.size();i++){
    MergeGeomCell *mcell_c = (MergeGeomCell*)allmcell_c[i];
    int index_c = toycur.Get_mcindex(mcell_c);
    
    cell_penal[index_c] = 0;

    auto it = find(cells.begin(),cells.end(),mcell_c);
    if (it !=cells.end()){
      use_time.push_back(index_c);
    }
    
  }

  for (int i=0;i!=allmcell_c.size();i++){
    auto it = find(use_time.begin(),use_time.end(),i);
    if (it==use_time.end())
      already_removed.push_back(i);
  }
  use_time.clear();
  
  //initialize
  //toymatrixkalman = new WCP2dToy::ToyMatrixKalman(*toymatrix);  // hold the current results 

  toymatrixkalman = new WCP2dToy::ToyMatrixKalman(already_removed, use_time, *toymatrix,1,0);
  std::cout << "With Time: " << toymatrixkalman->Get_numz() << " " << allmcell_c.size() << " " <<  already_removed.size() << std::endl;
  // Find a sub-set that is not degenerated
  // put things into use_time
  find_subset(*toymatrixkalman,toycur,use_time);
  already_removed.clear();
  
  std::cout << use_time.size() << std::endl;
  // recalculate
  delete toymatrixkalman;
  toymatrixkalman = new WCP2dToy::ToyMatrixKalman(already_removed, use_time, toycur,1,0);
  toymatrix->Set_Solve_Flag(0);
  toymatrix->Set_chi2(-1);

  if (use_time.size() !=0){
    std::cout << "Start making guesses " << std::endl;
    if (allmcell_c.size()<50){
      while (ncount < 1e4 
	     && (cur_chi2 > 5*(cur_dof+0.1) || ncount < 6000) 
	     && (cur_chi2 > 4*(cur_dof+0.1) || ncount < 3000) 
	     && (cur_chi2 > 3*(cur_dof+0.1) || ncount < 1500) 
	     && (cur_chi2 > 2*(cur_dof+0.1) || ncount < 800)
	     && (cur_chi2 > 1*(cur_dof+0.1) || ncount < 10)
	     ){
	if (ncount == 1 || (ncount % 1000 ==0&&ncount >0) )
	  std::cout << "MCMC: " << ncount << " " << cur_chi2 << " " << cur_dof << std::endl;
	
	make_guess();
	next_chi2 = toymatrix->Get_Chi2();
	next_dof = toymatrix->Get_ndf();
      }
    }else if (allmcell_c.size()<75){
      while (ncount < 5000 
	     && (cur_chi2 > 5*(cur_dof+0.1) || ncount < 6000) 
	     && (cur_chi2 > 4*(cur_dof+0.1) || ncount < 3000) 
	     && (cur_chi2 > 3*(cur_dof+0.1) || ncount < 1500) 
	     && (cur_chi2 > 2*(cur_dof+0.1) || ncount < 800)
	     && (cur_chi2 > 1*(cur_dof+0.1) || ncount < 10)
	     ){
	if (ncount == 1 || (ncount % 1000 ==0&&ncount >0) )
	  std::cout << "MCMC: " << ncount << " " << cur_chi2 << " " << cur_dof << std::endl;
	
	make_guess();
	next_chi2 = toymatrix->Get_Chi2();
	next_dof = toymatrix->Get_ndf();
      }
    }else if (allmcell_c.size()<110){
      while (ncount < 2000
	     && (cur_chi2 > 5*(cur_dof+0.1) || ncount < 6000) 
	     && (cur_chi2 > 4*(cur_dof+0.1) || ncount < 3000) 
	     && (cur_chi2 > 3*(cur_dof+0.1) || ncount < 1500) 
	     && (cur_chi2 > 2*(cur_dof+0.1) || ncount < 800)
	     && (cur_chi2 > 1*(cur_dof+0.1) || ncount < 10)
	     ){
	if (ncount == 1 || (ncount % 1000 ==0&&ncount >0) )
	  std::cout << "MCMC: " << ncount << " " << cur_chi2 << " " << cur_dof << std::endl;
	
	make_guess();
	next_chi2 = toymatrix->Get_Chi2();
	next_dof = toymatrix->Get_ndf();
      }
      //    }else if (allmcell_c.size()<150){
    }else if (allmcell_c.size()<250){
      for (int i=0;i!=200;i++){
	make_guess();
	next_chi2 = toymatrix->Get_Chi2();
	next_dof = toymatrix->Get_ndf();
      }
    }
  
  }else{
    if (allmcell_c.size()<30){
      while (ncount < 1e4 
	     && (cur_chi2 > 5*(cur_dof+0.1) || ncount < 6000) 
	     && (cur_chi2 > 4*(cur_dof+0.1) || ncount < 3000) 
	     && (cur_chi2 > 3*(cur_dof+0.1) || ncount < 1500) 
	     && (cur_chi2 > 2*(cur_dof+0.1) || ncount < 800)
	     && (cur_chi2 > 1*(cur_dof+0.1) || ncount < 10)
	     ){
	if (ncount == 1 || (ncount % 1000 ==0&&ncount >0) )
	  std::cout << "MCMC: " << ncount << " " << cur_chi2 << " " << cur_dof << std::endl;
	
	make_guess();
	next_chi2 = toymatrix->Get_Chi2();
	next_dof = toymatrix->Get_ndf();
      }
    }else if (allmcell_c.size()<60){
      while (ncount < 5000 
	     && (cur_chi2 > 5*(cur_dof+0.1) || ncount < 6000) 
	     && (cur_chi2 > 4*(cur_dof+0.1) || ncount < 3000) 
	     && (cur_chi2 > 3*(cur_dof+0.1) || ncount < 1500) 
	     && (cur_chi2 > 2*(cur_dof+0.1) || ncount < 800) 
	     && (cur_chi2 > 1*(cur_dof+0.1) || ncount < 10) 
	     ){
	if (ncount == 1 || (ncount % 1000 ==0&&ncount >0) )
	  std::cout << "MCMC: " << ncount << " " << cur_chi2 << " " << cur_dof << std::endl;
	
	make_guess();
	next_chi2 = toymatrix->Get_Chi2();
	next_dof = toymatrix->Get_ndf();
      }
    }
  }
}

WCP2dToy::ToyMatrixMarkov::ToyMatrixMarkov(WCP2dToy::ToyMatrix &toybefore, WCP2dToy::ToyMatrix &toycur, WCP2dToy::ToyMatrix &toyafter, WCP2dToy::MergeToyTiling &mergebefore, WCP2dToy::MergeToyTiling &mergecur, WCP2dToy::MergeToyTiling &mergeafter, WCP::GeomCellSelection *allmcell1,int recon_t1, int recon_t2, double penalty, double penalty_ncpt)
  : penalty_ncpt(penalty_ncpt)
{
  ToyMatrixMarkov(&toybefore, &toycur, &toyafter, &mergebefore, &mergecur, &mergeafter, allmcell, recon_t1, recon_t2, penalty, penalty_ncpt);
}


WCP2dToy::ToyMatrixMarkov::ToyMatrixMarkov(WCP2dToy::ToyMatrix *toybefore, WCP2dToy::ToyMatrix *toycur, WCP2dToy::ToyMatrix *toyafter, WCP2dToy::MergeToyTiling *mergebefore, WCP2dToy::MergeToyTiling *mergecur, WCP2dToy::MergeToyTiling *mergeafter, WCP::GeomCellSelection *allmcell1,int recon_t1, int recon_t2, double penalty, double penalty_ncpt)
  : penalty_ncpt(penalty_ncpt)
{
  ncount = 0;
  first_flag = 0;
  toymatrix = toycur;
  allmcell = allmcell1;
  mcindex = toymatrix->Get_mcindex();
  recon_threshold1 = recon_t1;
  recon_threshold2 = recon_t2;
  use_time_threshold = 0.75;
  
   //find good cells with time information and then use them ... 
  
  GeomCellSelection allmcell_p;
  if (mergebefore != 0)
    allmcell_p = mergebefore->get_allcell();
  GeomCellSelection allmcell_c = mergecur->get_allcell();
  GeomCellSelection allmcell_n;
  if (mergeafter != 0)
    allmcell_n = mergeafter->get_allcell();
  
  //form two vectors 
  std::vector<int> already_removed; //dummy
 
  for (int i=0;i!=allmcell_c.size();i++){
    MergeGeomCell *mcell_c = (MergeGeomCell*)allmcell_c[i];
    int index_c = toycur->Get_mcindex(mcell_c);
    
    int flag_before = 0;
    if (toybefore != 0){
      if (toybefore->Get_Solve_Flag()!=0 ){
	for (int j=0;j!=allmcell_p.size();j++){
	  MergeGeomCell *mcell_p = (MergeGeomCell*)allmcell_p[j];
	  int index_p = toybefore->Get_mcindex(mcell_p);
	  double charge = toybefore->Get_Cell_Charge(mcell_p,1);
	  if ( charge > recon_threshold2 && mcell_c->Overlap(*mcell_p)){
	    auto it = find(use_time.begin(),use_time.end(),index_c);
	    if (it == use_time.end()){
	      use_time.push_back(index_c);
	    }
	    flag_before = 1;
	  }
	}
      }
    }

    if (flag_before == 1){
      cell_penal[index_c] += penalty;
    }

    int flag_after = 0;
    if (toyafter !=0){
      if (toyafter->Get_Solve_Flag()!=0){
	for (int j=0;j!=allmcell_n.size();j++){
	  MergeGeomCell *mcell_n = (MergeGeomCell*)allmcell_n[j];
	  int index_n = toyafter->Get_mcindex(mcell_n);
	  double charge = toyafter->Get_Cell_Charge(mcell_n,1);
	  if ( charge > recon_threshold2 && mcell_c->Overlap(*mcell_n)){
	    auto it = find(use_time.begin(),use_time.end(),index_c);
	    if (it == use_time.end()){
	      use_time.push_back(index_c);
	    }
	    flag_after = 1;
	  }
	}
      }
    }

    if (flag_after == 1){
      cell_penal[index_c] += penalty;
    }
  }
  
  // fill in the cell_connect map;
  GeomCellCellsMap& cells_map = mergecur->get_not_compatible_cells_map();
  GeomCellSelection cells = mergecur->get_allcell();
  for (int i=0;i!=cells.size();i++){
    MergeGeomCell *mcell = (MergeGeomCell*)cells.at(i);
    int index = toycur->Get_mcindex(mcell);
    if (cells_map[mcell].size() > 0){
      std::vector<int> cells_indices;
      for (int j=0;j!=cells_map[mcell].size();j++){
	int index_c = toycur->Get_mcindex(cells_map[mcell].at(j));
	cells_indices.push_back(index_c);
      }
      cells_ncpt[index] = cells_indices;
    }
  }



  for (int i=0;i!=allmcell_c.size();i++){
    auto it = find(use_time.begin(),use_time.end(),i);
    if (it==use_time.end())
      already_removed.push_back(i);
  }
  use_time.clear();
  
  //initialize
  //toymatrixkalman = new WCP2dToy::ToyMatrixKalman(*toymatrix);  // hold the current results 
  
  toymatrixkalman = new WCP2dToy::ToyMatrixKalman(already_removed, use_time, *toymatrix,1,0);
  std::cout << "With Time: " << toymatrixkalman->Get_numz() << " " << allmcell_c.size() << " " <<  already_removed.size() << std::endl;
  // Find a sub-set that is not degenerated
  // put things into use_time
  find_subset(*toymatrixkalman,*toycur,use_time);
  already_removed.clear();
  
  //std::cout << use_time.size() << std::endl;
  // recalculate
  delete toymatrixkalman;
  toymatrixkalman = new WCP2dToy::ToyMatrixKalman(already_removed, use_time, *toycur,1,0);
  toymatrix->Set_Solve_Flag(0);
  toymatrix->Set_chi2(-1);

  if (use_time.size() != 0 ){
    
    std::cout << "Start making guesses " << std::endl;
    if (allmcell_c.size()<50){
      while (ncount < 1e4 
	     && (cur_chi2 > 5*(cur_dof+0.1) || ncount < 6000) 
	     && (cur_chi2 > 4*(cur_dof+0.1) || ncount < 3000) 
	     && (cur_chi2 > 3*(cur_dof+0.1) || ncount < 1500) 
	     && (cur_chi2 > 2*(cur_dof+0.1) || ncount < 800) 
	     && (cur_chi2 > 1*(cur_dof+0.1) || ncount < 10) 
	     ){
	if (ncount == 1 || (ncount % 1000 ==0&&ncount >0) )
	  std::cout << "MCMC: " << ncount << " " << cur_chi2 << " " << cur_dof << std::endl;
	
	make_guess();
	next_chi2 = toymatrix->Get_Chi2();
	next_dof = toymatrix->Get_ndf();
      }
    }else if (allmcell_c.size()<75){
      while (ncount < 5000 
	     && (cur_chi2 > 5*(cur_dof+0.1) || ncount < 6000) 
	     && (cur_chi2 > 4*(cur_dof+0.1) || ncount < 3000) 
	     && (cur_chi2 > 3*(cur_dof+0.1) || ncount < 1500) 
	     && (cur_chi2 > 2*(cur_dof+0.1) || ncount < 800) 
	     && (cur_chi2 > 1*(cur_dof+0.1) || ncount < 10) 
	     ){
	if (ncount == 1 || (ncount % 1000 ==0&&ncount >0) )
	  std::cout << "MCMC: " << ncount << " " << cur_chi2 << " " << cur_dof << std::endl;
	
	make_guess();
	next_chi2 = toymatrix->Get_Chi2();
	next_dof = toymatrix->Get_ndf();
      }
    }else if (allmcell_c.size()<110){
      while (ncount < 2000 
	     && (cur_chi2 > 5*(cur_dof+0.1) || ncount < 6000) 
	     && (cur_chi2 > 4*(cur_dof+0.1) || ncount < 3000) 
	     && (cur_chi2 > 3*(cur_dof+0.1) || ncount < 1500) 
	     && (cur_chi2 > 2*(cur_dof+0.1) || ncount < 800) 
	     && (cur_chi2 > 1*(cur_dof+0.1) || ncount < 10) 
	     ){
	if (ncount == 1 || (ncount % 1000 ==0&&ncount >0) )
	  std::cout << "MCMC: " << ncount << " " << cur_chi2 << " " << cur_dof << std::endl;
	
	make_guess();
	next_chi2 = toymatrix->Get_Chi2();
	next_dof = toymatrix->Get_ndf();
      }
    }else if (allmcell_c.size()<250){
      //    }else if (allmcell_c.size()<150){
      for (int i=0;i!=200;i++){
	make_guess();
	next_chi2 = toymatrix->Get_Chi2();
	next_dof = toymatrix->Get_ndf();
      }
    }
  }else{ // test use ... 
    if (allmcell_c.size()<30){
      while (ncount < 1e4 
	     && (cur_chi2 > 5*(cur_dof+0.1) || ncount < 6000) 
	     && (cur_chi2 > 4*(cur_dof+0.1) || ncount < 3000) 
	     && (cur_chi2 > 3*(cur_dof+0.1) || ncount < 1500) 
	     && (cur_chi2 > 2*(cur_dof+0.1) || ncount < 800) 
	     && (cur_chi2 > 1*(cur_dof+0.1) || ncount < 10) 
	     ){
	if (ncount == 1 || (ncount % 1000 ==0&&ncount >0) )
	  std::cout << "MCMC: " << ncount << " " << cur_chi2 << " " << cur_dof << std::endl;
	
	make_guess();
	next_chi2 = toymatrix->Get_Chi2();
	next_dof = toymatrix->Get_ndf();
      }
    }else if (allmcell_c.size()<60){
      while (ncount < 5000 
	     && (cur_chi2 > 5*(cur_dof+0.1) || ncount < 6000) 
	     && (cur_chi2 > 4*(cur_dof+0.1) || ncount < 3000) 
	     && (cur_chi2 > 3*(cur_dof+0.1) || ncount < 1500) 
	     && (cur_chi2 > 2*(cur_dof+0.1) || ncount < 800) 
	     && (cur_chi2 > 1*(cur_dof+0.1) || ncount < 10) 
	     ){
	if (ncount == 1 || (ncount % 1000 ==0&&ncount >0) )
	  std::cout << "MCMC: " << ncount << " " << cur_chi2 << " " << cur_dof << std::endl;
	
	make_guess();
	next_chi2 = toymatrix->Get_Chi2();
	next_dof = toymatrix->Get_ndf();
      }
    }
  }
}



WCP2dToy::ToyMatrixMarkov::ToyMatrixMarkov(WCP2dToy::ToyMatrix *toymatrix1,WCP::GeomCellSelection *allmcell1, int recon_t1, int recon_t2)
  : penalty_ncpt(0)
{
  ncount = 0;
  first_flag = 0;
  toymatrix = toymatrix1; //save the matrix in here
  allmcell = allmcell1;
  mcindex = toymatrix->Get_mcindex();
  recon_threshold1 = recon_t1;
  recon_threshold2 = recon_t2;
  use_time_threshold = 0.75;

  for (int i=0;i!=mcindex;i++){
    cell_penal[i] = 0;
  }


  toymatrixkalman = new WCP2dToy::ToyMatrixKalman(*toymatrix,0);  // hold the current results 
  
  if (allmcell->size() < 50){
    while (ncount < 1e4 
	   && (cur_chi2 > 5*(cur_dof+0.1) || ncount < 6000) 
	   && (cur_chi2 > 4*(cur_dof+0.1) || ncount < 3000) 
	   && (cur_chi2 > 3*(cur_dof+0.1) || ncount < 1500) 
	   && (cur_chi2 > 2*(cur_dof+0.1) || ncount < 800) 
	   && (cur_chi2 > (cur_dof+0.1) || ncount < 10) 
	   ){
      if (ncount == 1 || (ncount % 1000 ==0&&ncount >0) )
	std::cout << "MCMC: " << ncount << " " << cur_chi2 << " " << cur_dof << std::endl;
      
      make_guess();
      next_chi2 = toymatrix->Get_Chi2();
      next_dof = toymatrix->Get_ndf();
    }
  }else if (allmcell->size()<75){
    while (ncount < 5000
	   && (cur_chi2 > 5*(cur_dof+0.1) || ncount < 6000) 
	   && (cur_chi2 > 4*(cur_dof+0.1) || ncount < 3000) 
	   && (cur_chi2 > 3*(cur_dof+0.1) || ncount < 1500) 
	   && (cur_chi2 > 2*(cur_dof+0.1) || ncount < 800) 
	   && (cur_chi2 > (cur_dof+0.1) || ncount < 10) 
	   ){
      if (ncount == 1 || (ncount % 1000 ==0&&ncount >0) )
	std::cout << "MCMC: " << ncount << " " << cur_chi2 << " " << cur_dof << std::endl;
      
      make_guess();
      next_chi2 = toymatrix->Get_Chi2();
      next_dof = toymatrix->Get_ndf();
    }
  }else if (allmcell->size()<110){
    while (ncount < 2000 
	   && (cur_chi2 > 5*(cur_dof+0.1) || ncount < 6000) 
	   && (cur_chi2 > 4*(cur_dof+0.1) || ncount < 3000) 
	   && (cur_chi2 > 3*(cur_dof+0.1) || ncount < 1500) 
	   && (cur_chi2 > 2*(cur_dof+0.1) || ncount < 800) 
	   && (cur_chi2 > (cur_dof+0.1) || ncount < 10) 
	   ){
      if (ncount ==1 || (ncount % 1000 ==0&&ncount >0) )
	std::cout << "MCMC: " << ncount << " " << cur_chi2 << " " << cur_dof << std::endl;
      
      make_guess();
      next_chi2 = toymatrix->Get_Chi2();
      next_dof = toymatrix->Get_ndf();
    }
  }else if (allmcell->size()<250){
    //  }else if (allmcell->size()<150){
    for (int i=0;i!=200;i++){
      make_guess();
      next_chi2 = toymatrix->Get_Chi2();
      next_dof = toymatrix->Get_ndf();
    }
  }
  
  //  std::cout << cur_chi2 << " " << ncount << std::endl;
  

  

  
}

WCP2dToy::ToyMatrixMarkov::~ToyMatrixMarkov(){
  delete toymatrixkalman;
}

void WCP2dToy::ToyMatrixMarkov::make_guess(){
  
  ncount ++; 
  
  if (first_flag ==0){
    nlevel = 0;
    //    std::cout << toymatrixkalman->Get_mcindex() << " " << toymatrixkalman->Get_numz() << std::endl;
    Iterate(*toymatrixkalman);
    first_flag = 1;
  }
  //need to save the results???
  
  cur_chi2 = toymatrix->Get_Chi2();
 

  cur_dof = toymatrix->Get_ndf();

  toymatrix->Update_pred();

  double_t max_res = 0;

  for (int i=0;i!= (*allmcell).size();i++){
    MergeGeomCell *mcell = (MergeGeomCell*)(*allmcell).at(i);
    double charge = toymatrix->Get_Cell_Charge(mcell,1);
    double charge_err = toymatrix->Get_Cell_Charge(mcell,2);
    
    cur_cell_index.push_back(toymatrix->Get_mcindex(mcell));

    if (charge ==0 && charge_err ==0 ){
      cur_cell_status.push_back(0); // removed for matrix calculation
    }else{
      cur_cell_status.push_back(1); // not removed
    }
    
    if (charge > recon_threshold1){ // hard coded to be fixed later
      cur_cell_pol.push_back(1); // on
    }else{
      cur_cell_pol.push_back(0); //off
    }

    //calculate residual for each cell, part of toymatrix
    // with residual and polarity, one can calculate probability
    cell_res.push_back(toymatrix->Get_residual(mcell));

    if (toymatrix->Get_residual(mcell) > max_res) 
      max_res = toymatrix->Get_residual(mcell);

    cell_charge.push_back(charge);

    if (cur_cell_status.at(i)==1){
      double ratio;
      
      if (cell_res.at(i)!=0){
	ratio = cell_charge.at(i)/cell_res.at(i);
      }else{
	ratio = cell_charge.at(i)/10.;
      }
      
      if (ratio > 10.){
	cell_prob.push_back(0.8); 
      }else if (ratio <=10 && ratio>=-10.){
	cell_prob.push_back(0.6);
      }else{
	cell_prob.push_back(0.2);
      }
    }else{
      if (cell_res.at(i) > 1000){
	cell_prob.push_back(0.6);
      }else{
	cell_prob.push_back(0.4);
      }
    }

    auto it = find(use_time.begin(),use_time.end(),toymatrix->Get_mcindex(mcell));
    if (it != use_time.end()){

      if (cell_prob.at(i) < use_time_threshold)
	cell_prob.at(i) = use_time_threshold;
      
    }

    //   std::cout << cur_cell_status.at(i) << " " << cur_cell_pol.at(i) <<
    //   " " << cell_res.at(i) << " " << std::endl;
  }


  cell_set.clear();
  //rank stuff ... 
  // first probability, second bias
  for (int i=0;i!= (*allmcell).size();i++){
    CellRankPair a(i,cell_prob.at(i)+gRandom->Uniform(0,1)/100.);
    cell_set.insert(a);
  }
  
  CellRankSet xp,middle,bad_one;
  
  

  
  toymatrixkalman->Get_no_need_remove().clear();
  toymatrixkalman->Get_already_removed().clear();
  
  //fill three cases
  for (auto it= cell_set.begin(); it!=cell_set.end();it++){
    //std::cout << (*it).first << " " << (*it).second << std::endl;
    int index = (*it).first ;
    double score = (*it).second;
    CellRankPair a(index,score);
    double r = gRandom->Uniform(0,1);
    if (r < cell_prob.at(index)){ 
      // start to insert
      if (xp.size()==0){
	
	xp.insert(a);
	toymatrixkalman->Get_no_need_remove().push_back(index);
      }else{
	//first push in and judge 
	toymatrixkalman->Get_no_need_remove().push_back(index);
	if (toymatrixkalman->Cal_numz(*toymatrix)==0){
	  xp.insert(a);
	}else{
	  bad_one.insert(a);
	  toymatrixkalman->Get_no_need_remove().pop_back();
	}
      }
    }else{
      middle.insert(a);
    } 
  }

  //std::cout << xp.size() << " " << middle.size() << " " << bad_one.size() << std::endl;

  while (middle.size()!=0){
    std::vector<int> del_list;
    for (auto it = middle.begin();it!=middle.end();it++){
      double r = gRandom->Uniform(0,1);
      int index = (*it).first ;
      double score = (*it).second;
      CellRankPair a(index,score);
      
      if (r < cell_prob.at(index)){ 
	del_list.push_back(index);
	//  	middle.erase(it);
	toymatrixkalman->Get_no_need_remove().push_back(index);
	if (toymatrixkalman->Cal_numz(*toymatrix)==0){
	  xp.insert(a);
	}else{
	  bad_one.insert(a);
	  toymatrixkalman->Get_no_need_remove().pop_back();
	}
      }
    }
    
    for (int i = 0; i!=del_list.size();i++){
      for (auto it = middle.begin();it!=middle.end();it++){
	if ((*it).first == del_list.at(i)){
	  middle.erase(it);
	  break;
	}
      }
    }
    
  }

  //std::cout << xp.size() << " " << middle.size() << " " << bad_one.size() << std::endl;
 
  for (int i=0;i!=mcindex;i++){
    auto it = find(toymatrixkalman->Get_no_need_remove().begin(),toymatrixkalman->Get_no_need_remove().end(),i);
    if (it==toymatrixkalman->Get_no_need_remove().end())
      toymatrixkalman->Get_already_removed().push_back(i);
  }
  toymatrixkalman->Get_no_need_remove().clear();
  
  double chi2_p = 0;
  for (int j = 0; j!=toymatrixkalman->Get_already_removed().size(); j++){
    chi2_p += cell_penal[toymatrixkalman->Get_already_removed().at(j)];
  }

  if (penalty_ncpt>0){
    //add the penalty due to not compatible cells (ncpt)
    //int flag_test = 0;
    for (auto it = cells_ncpt.begin(); it!= cells_ncpt.end(); it++){
      int index1 = it->first;
      if (find(toymatrixkalman->Get_already_removed().begin(),toymatrixkalman->Get_already_removed().end(),index1) != toymatrixkalman->Get_already_removed().end()) continue;
      std::vector<int> indices = it->second;
      
      for (int j=0;j!=indices.size();j++){
	int index2 = indices.at(j);
	if (find(toymatrixkalman->Get_already_removed().begin(),toymatrixkalman->Get_already_removed().end(),index2) == toymatrixkalman->Get_already_removed().end()){
	  chi2_p += penalty_ncpt;
	  // flag_test = 1;
	  // break;
	}
      }
      // if (flag_test ==1) break;
    }
    // if (flag_test == 1){
    //   chi2_p += penalty_ncpt;
    // }
  }


  toymatrixkalman->Set_penalty(chi2_p);

  //why initiate again???  //calculate chi2 ... 
  toymatrixkalman->init(*toymatrix);
  //std::cout << toymatrixkalman->Get_already_removed().size() << std::endl;
}

void WCP2dToy::ToyMatrixMarkov::Iterate(WCP2dToy::ToyMatrixKalman &toykalman){
  nlevel++;
  if (toykalman.Get_numz()!=0&&toymatrix->Get_Chi2()<0){
    for (int i=0;i!=toykalman.Get_mcindex();i++){
      auto it1 = find(toykalman.Get_already_removed().begin(),toykalman.Get_already_removed().end(),i);
      auto it2 = find(toykalman.Get_no_need_remove().begin(),toykalman.Get_no_need_remove().end(),i);
      if (it1 == toykalman.Get_already_removed().end() && it2 == toykalman.Get_no_need_remove().end()){
	std::vector<int> already_removed = toykalman.Get_already_removed();
	already_removed.push_back(i);

	// test ... 
	if (toykalman.Get_no_need_remove().size() + already_removed.size() >= toykalman.Get_mcindex()){
	  //std::cout << already_removed.size() << " " << toykalman.Get_no_need_remove().size() << std::endl;

	  //  for (int i=0;i!=allmcell_c.size();i++){
	  //   auto it = find(use_time.begin(),use_time.end(),i);
	  //   if (it==use_time.end())
	  //     already_removed.push_back(i);
	  // }
	  // use_time.clear();
	  
	  // //initialize
	  // //toymatrixkalman = new WCP2dToy::ToyMatrixKalman(*toymatrix);  // hold the current results 
	  
	  // toymatrixkalman = new WCP2dToy::ToyMatrixKalman(already_removed, use_time, *toymatrix,1,0);
	  // std::cout << "With Time: " << toymatrixkalman->Get_numz() << " " << allmcell_c.size() << " " <<  already_removed.size() << std::endl;
	  // // Find a sub-set that is not degenerated
	  // // put things into use_time
	  // find_subset(*toymatrixkalman,toycur,use_time);

	  std::vector<int> temp_already_removed;
	  std::vector<int> temp_no_need_remove;
	  for (int k=0;k!=toykalman.Get_mcindex();k++){
	    auto it4 = find(toykalman.Get_no_need_remove().begin(),toykalman.Get_no_need_remove().end(),k);
	    if (it4 == toykalman.Get_no_need_remove().end())
	      temp_already_removed.push_back(k);
	  }
	  

	  //temp_no_need_remove.clear();
	  //initialize
	  WCP2dToy::ToyMatrixKalman temp_tk(temp_already_removed, temp_no_need_remove, *toymatrix,0,0);
	  //	  std::cout << temp_tk.Get_numz() << " " << temp_already_removed.size() << " " << temp_no_need_remove.size() << std::endl;
	  // Find a sub-set that is not degenerated
	  find_subset(temp_tk,*toymatrix,temp_no_need_remove);

	  if (temp_no_need_remove.size()!=0){
	    toykalman.Get_no_need_remove().clear();
	    for (int k=0;k!=temp_no_need_remove.size();k++){
	      toykalman.Get_no_need_remove().push_back(temp_no_need_remove.at(k));
	    }
	    already_removed.clear();
	  }
	  //std::cout << already_removed.size() << " " << toykalman.Get_no_need_remove().size() << " " << temp_no_need_remove.size() << std::endl;
	}
	

	double chi2_p = 0;
	for (int j = 0; j!=already_removed.size(); j++){
	  chi2_p += cell_penal[already_removed.at(j)];
	}

	if (penalty_ncpt>0){
	  //add the penalty due to not compatible cells (ncpt)
	  //int flag_test = 0;
	  for (auto it = cells_ncpt.begin(); it!= cells_ncpt.end(); it++){
	    int index1 = it->first;
	    if (find(already_removed.begin(),already_removed.end(),index1) != already_removed.end()) continue;
	    std::vector<int> indices = it->second;
	    
	    for (int j=0;j!=indices.size();j++){
	      int index2 = indices.at(j);
	      if (find(already_removed.begin(),already_removed.end(),index2) == already_removed.end()){
		chi2_p += penalty_ncpt;
		// flag_test = 1;
		// break;
	      }
	    }
	    // if (flag_test ==1) break;
	  }
	  // if (flag_test == 1){
	  //   chi2_p += penalty_ncpt;
	  // }
	}




	//std::cout << nlevel << " " << already_removed.size() << " " << toykalman.Get_no_need_remove().size() << " " << toykalman.Get_numz() << std::endl;

	WCP2dToy::ToyMatrixKalman kalman(already_removed,toykalman.Get_no_need_remove(),*toymatrix,0,1,chi2_p);
	//std::cout << kalman.Get_numz() << std::endl;
	// this part seems to be able to improve
	// to get the first solution, one should not take too much time ...
	// consider to improve in the future ...  i.e. add cut on the number of get_already_removed vs. what's left ... 
	Iterate(kalman);
	nlevel--;
	
	toykalman.Get_no_need_remove().push_back(i);
	if (toymatrix->Get_Chi2()>0) break;
      }
    }
  }
}
