#include "WCP2dToy/SimpleBlobToyTiling.h"
#include "TRandom.h"
#include "TDecompSVD.h"

// #include <lsp/nnls.h>

//#include <boost/numeric/ublas/matrix.hpp>
//#include <boost/numeric/ublas/vector.hpp>
//#include <boost/numeric/ublas/io.hpp>

//#include <sstream>


using namespace WCP;

double WCP2dToy::SimpleBlobToyTiling::Get_Cell_Charge( const WCP::GeomCell *cell, int flag )  {
  // flag == 1 charge
  // flag == 2 uncertainty
  int index = scimap_save[cell];
  if (flag==1){
    return Cx_save.at(index);
  }else if (flag==2){
    return dCx_save.at(index);
  }
}


WCP2dToy::SimpleBlobToyTiling::SimpleBlobToyTiling(WCP2dToy::ToyTiling& toytiling1, WCP2dToy::MergeToyTiling& mergetiling1, WCP2dToy::ToyMatrix& toymatrix1,
							WCP2dToy::MergeToyTiling& prev_mergetiling, WCP2dToy::ToyMatrix& prev_toymatrix,
							WCP2dToy::MergeToyTiling& next_mergetiling, WCP2dToy::ToyMatrix& next_toymatrix, int recon_t){
  toytiling = &toytiling1;
  mergetiling = &mergetiling1;
  toymatrix = &toymatrix1;
  recon_threshold = recon_t;
  

  nsimple_blob = 0;
  MergeGeomCell *mcorner_cell;
  if (toymatrix->GetSimpleBlobReduction()){
    GeomCellSelection mcells = mergetiling->get_allcell();
    for (int i=0;i!=mcells.size();i++){
      MergeGeomCell *mcell = (MergeGeomCell*)mcells.at(i);
      if (mcell->IsSimpleBlob() && mcell->IsBlob()){
	//save all the simple blob
	sbcells.push_back(mcell);
	if (corner_smcells.size() < nsimple_blob + 1){
	  GeomCellSelection cellss;
	  corner_smcells.push_back(cellss);
	}
	if (corner_mcells.size() < nsimple_blob + 1){
	  GeomCellSelection cellss;
	  corner_mcells.push_back(cellss);
	}
	
	CellIndexMap indexmap = mcell->get_cornercells_index();
	
	for (int j=0;j!=12;j++){
	  int index1 = mcell->index1(j);
	  int index2 = mcell->index2(j);
	  GeomCellSelection cells = mcell->get_cornercells(index1,index2);
	  if (cells.size()!=0){
	    //std::cout << index1 << " " << index2 << " " << cells.size() << std::endl;
	    int flag = -1;
	    //form merged cell and start to fill rank ... 
	    for (int k=0;k!=cells.size();k++){
	      if (k==0){
		mcorner_cell = new MergeGeomCell(10000,*cells.at(k));
		cell_rank[mcorner_cell] = 0;
		mcorner_cell->ewires.insert(index1);
		mcorner_cell->ewires.insert(index2);
	      }else{
		mcorner_cell->AddCell(*cells.at(k));
	      }
	      if (indexmap[cells.at(k)] == 3) {
		flag=k;
		cell_rank[mcorner_cell] += 5; // should be adjusted later?
	      }
	    }
	    int flag1 = 0;
	    
	    // if (flag!=-1){
	    //   for (int k=0;k!=corner_smcells[nsimple_blob].size();k++){
	    // 	// see if any previous merged cell contain this cell
	    // 	// if so, add all cells to this merged cell 
	    // 	// and delete this cell
	    // 	// change flag1 value;
	    // 	GeomCellSelection tcells = ((MergeGeomCell*)corner_smcells[nsimple_blob].at(k))->get_allcell();
	    // 	auto it = find(tcells.begin(),tcells.end(),cells.at(flag));
	    // 	if (it!=tcells.end()){
	    // 	  delete mcorner_cell;
	    // 	  flag1 = 1;
	    // 	  ((MergeGeomCell*)corner_smcells[nsimple_blob].at(k))->ewires.insert(index1);
	    // 	  ((MergeGeomCell*)corner_smcells[nsimple_blob].at(k))->ewires.insert(index2);
	    // 	  for (int kk = 0;kk!=cells.size();kk++){
	    // 	    auto it1 = find(tcells.begin(),tcells.end(),cells.at(kk));
	    // 	    if (it1==tcells.end()){
	    // 	      ((MergeGeomCell*)corner_smcells[nsimple_blob].at(k))->AddCell(*cells.at(kk));
	    // 	    }
	    // 	  }
	    // 	}
	    //   }
	    // }

	    //save the special cells ...  //need to add time later ... 
	    if (flag1==0){
	      if (flag!=-1){
		
		corner_smcells[nsimple_blob].push_back(mcorner_cell);
		
	      }else{
		
		corner_mcells[nsimple_blob].push_back(mcorner_cell);
		  
	      }
	    }
	  }
	}
       

	// going through smcells array and properly calculate index
	for (auto it = corner_smcells[nsimple_blob].begin(); it!=corner_smcells[nsimple_blob].end();it++){
	  MergeGeomCell *mcell = (MergeGeomCell*) (*it);
	  for (int j=0;j!=prev_mergetiling.get_allcell().size();j++){
	    MergeGeomCell *prev_mcell = (MergeGeomCell*) prev_mergetiling.get_allcell().at(j);
	    if (prev_toymatrix.Get_Cell_Charge(prev_mcell)>recon_threshold){
	      int temp_val = mcell->Overlap1(*prev_mcell);
	      if (temp_val){
		cell_rank[mcell] += temp_val;
		break;
	      }
	    }
	  }
	  //see next time slice
	  for (int j=0;j!=next_mergetiling.get_allcell().size();j++){
	    MergeGeomCell *next_mcell = (MergeGeomCell*) next_mergetiling.get_allcell().at(j);
	    if (next_toymatrix.Get_Cell_Charge(next_mcell)>recon_threshold){
	      int temp_val = mcell->Overlap1(*next_mcell);
	      if (temp_val){
		cell_rank[mcell] += temp_val;
		break;
	      }
	    }
	  }
	}
	
	// going through the mcells array and judge if any of them are special
	// if so move to the smcells
	for (auto it = corner_mcells[nsimple_blob].begin(); it!=corner_mcells[nsimple_blob].end();it++){
	  MergeGeomCell *mcell = (MergeGeomCell*) (*it);
	  int flag = 0;
	  //see previous time slice
	  for (int j=0;j!=prev_mergetiling.get_allcell().size();j++){
	    MergeGeomCell *prev_mcell = (MergeGeomCell*) prev_mergetiling.get_allcell().at(j);
	    if (prev_toymatrix.Get_Cell_Charge(prev_mcell)>recon_threshold){
	      int temp_val = mcell->Overlap1(*prev_mcell);
	      if (temp_val){
		cell_rank[mcell] += temp_val;
		flag = 1;
	      	break;
	      }
	    }
	  }
	  //see next time slice
	  for (int j=0;j!=next_mergetiling.get_allcell().size();j++){
	    MergeGeomCell *next_mcell = (MergeGeomCell*) next_mergetiling.get_allcell().at(j);
	    if (next_toymatrix.Get_Cell_Charge(next_mcell)>recon_threshold){
	      int temp_val = mcell->Overlap1(*next_mcell);
	      if (temp_val){
		cell_rank[mcell] += temp_val;
		flag = 1;
		break;
	      }
	    }
	  }
	  if (flag==1){
	    corner_smcells[nsimple_blob].push_back(mcell);
	  }
	}
	for (auto it = corner_smcells[nsimple_blob].begin(); it!=corner_smcells[nsimple_blob].end();it++){
	  MergeGeomCell *mcell = (MergeGeomCell*) (*it);
	  auto itq = find( corner_mcells[nsimple_blob].begin(),  corner_mcells[nsimple_blob].end(), mcell);
	  if (itq != corner_mcells[nsimple_blob].end()){
	    corner_mcells[nsimple_blob].erase(itq);
	  }
	}
	
	//save stuff into the first, second and third ... 
	Organize(nsimple_blob);
	
	nsimple_blob ++;
	// if (nsimple_blob >= 10) {
	//   break;
	// }
      }
    }

   

    //form the hypothesis and do the test ... 
    ncount = 0;

    //std::cout << "xin1" << std::endl;
    FormHypo();
    //std::cout << "xin1" << std::endl;
    DoTiling();
    //std::cout << "xin1" << std::endl;
    cur_chi2 = CalChi2();
    //std::cout << "xin1" << std::endl;
    SaveResult();
    //std::cout << "xin1" << std::endl;
    while(cur_chi2 > 5 && ncount < 10){
      //std::cout << cur_chi2 << std::endl;
      
      FormHypo();
      //std::cout << "xin1" << std::endl;
      DoTiling();
      //std::cout << "xin1" << std::endl;
      cur_chi2 = CalChi2();
      //std::cout << "xin2" << std::endl;
      //std::cout << cur_chi2 << std::endl;
      if (cur_chi2 < chi2_save)
	SaveResult();
    }

    std::cout << "DeBlob Chi2: " << chi2_save << " " << cell_all.size() << std::endl;
    
    
    
    // std::cout << "SimpleBlobTiling: "<< nsimple_blob << " " << corner_smcells[0].size() << " " << corner_mcells[0].size() << " " << hypo_ccells.at(0).size() << std::endl;
    // for (int j=0;j!=hypo_ccells.at(0).size();j++){
    //   std::cout << hypo_ccells.at(0).at(j).size() << " ";
    //   for (int k = 0;k!= hypo_ccells.at(0).at(j).size();k++){
    // 	std::cout << cell_rank[hypo_ccells.at(0).at(j).at(k)] << " ";
    //   }
    //   std::cout << std::endl;
    // }
   

  }
}

double WCP2dToy::SimpleBlobToyTiling::CalChi2(){
  //using namespace boost::numeric::ublas;

  double chi2=1e9;
  Buildup_index();
  
  if (ncount == 1)
    std::cout << "Dimensions: " << swindex << " " << " " << scindex << " " << wire_all.size() << std::endl;

  if (swindex !=0&& scindex !=0){

    if (scindex > 2000){
      toymatrix->SetSimpleBlobReduction(false);
    }else{
      if (swindex==wire_all.size()&&scindex < 500 || ncount==1){
	TMatrixD MA(swindex,scindex);
	TMatrixD MAT(scindex,swindex);
	
	TMatrixD Vy(swindex,swindex);
	TMatrixD Vy_inv(swindex,swindex);
	
	TVectorD Wy(swindex);
	TVectorD Wy_pred(swindex);
	
	TMatrixD MC(scindex,scindex);
	TMatrixD MC_inv(scindex,scindex);
	
	TMatrixD Vx(scindex,scindex);
	TMatrixD Vx_inv(scindex,scindex);
	
	
	TVectorD VCx(scindex);
	
	//std::cout << ncount << " " << swindex << " " << scindex << " " << cell_all.size() << " " << wire_all.size() << std::endl;
	
	// fill wire charge
	WireChargeMap wcmap = toytiling->wcmap();
	WireChargeMap wcemap = toytiling->wcemap();
	for (int i=0; i!=wire_all.size();i++){
	  int index = swimap[wire_all.at(i)];
	  float charge = wcmap[wire_all.at(i)];
	  float charge_err = wcemap[wire_all.at(i)];
	  Wy[index] =charge;
	  Vy(index,index) = charge_err*charge_err/1e6;
	  //WirePlaneType_t plane = wire_all.at(i)->plane();
	  //double 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;
	}
	
	std::cout << "finish fill in charge" << std::endl;
	
	//fill MA
	for (int i=0;i!=wire_all.size();i++){
	  int index = swimap[wire_all.at(i)];
	  for (int j=0;j!=wiremap[wire_all.at(i)].size();j++){
	    int index1 = scimap[wiremap[wire_all.at(i)].at(j)];
	    MA(index,index1) = 1;
	  }
	}
	
	std::cout << "finish fill in MA" << std::endl;
	
	MAT.Transpose(MA);
	Vy_inv = Vy;
	Vy_inv.Invert();
	
	MC = MAT * Vy_inv * MA;
	
	std::cout << "finish Invert Vy_inv" << std::endl;
	
	// //Now try NNLS  need to improve
	// int m = scindex, n = swindex;
	
	// std::cout << m << " " << n << std::endl;
	
	// matrix < double > NNLS_A(n,m);
	// vector < double > NNLS_b(n);
	
	// //fill value ...
	// for (int i = 0;i!=n;i++){
	//   NNLS_b(i) = Wy[i];
	//   //std::cout << i << " " << Wy[i] << std::endl;
	//   for (int j=0;j!=m;j++){
	//     NNLS_A(i,j) = MA(i,j);
	//     //std::cout << i << " " << j << " " << MA(i,j) << std::endl;
	//   }
	// }
	
	// lsp::nnls< matrix< double >, vector< double >  > nnls( NNLS_A, NNLS_b );
	// vector< double > NNLS_x(m);
	// matrix< double > NNLS_cov(m,m);
	// nnls.solve( NNLS_x , NNLS_cov );
	
	// // save results and calculate chi2 
	// for (int i=0;i!=scindex;i++){
	//   (*Cx)[i] = NNLS_x(i);
	//   (*dCx)[i] = sqrt(NNLS_cov(i,i));
	// }
	
	// MC.Print();
	
	//SVD solution
	TDecompSVD svd(MC);
	MC_inv = svd.Invert();
	
	std::cout << "Finish Invert MC" << std::endl;
	
	VCx = MC_inv * MAT * Vy_inv * Wy;
	Vx_inv = MAT * Vy_inv * MA;
	TDecompSVD svd1(Vx_inv);
	Vx = svd1.Invert();
	
	std::cout << "Finish Invert Vx " << std::endl; 
	
	Cx.clear();
	dCx.clear();
	for (int i = 0; i!=scindex; i++){
	  if (Vx(i,i)>=0){
	    dCx.push_back(sqrt( Vx(i,i)) * 1000.);
	  }else{
	    dCx.push_back(0);
	  }
	  Cx.push_back(VCx[i]);
	  //std::cout << i << " " << (*Cx)[i] << " " << (*dCx)[i] << std::endl;
	}
	//  std::cout << std::endl;
	
	
	Wy_pred = MA * VCx;
	TVectorD r1 = Vy_inv * (Wy - Wy_pred);
	TVectorD r2 = Wy - Wy_pred;
	chi2 = r1 * r2 / 1e6;
	
	std::cout << "Calculate Chi2 " << std::endl;
	
	//std::cout << chi2 << std::endl;
      }
    }
  }
    
  return chi2;
}
  
void WCP2dToy::SimpleBlobToyTiling::Buildup_index(){
  scindex = 0;
  swindex = 0;
  
  scimap.clear();
  swimap.clear();
  

  
  for (int j=0;j!=cell_all.size();j++){
    //construct merged cell index
    const GeomCell *cell = cell_all[j];
    if (scimap.find(cell) == scimap.end()){
      scimap[cell] = scindex;
      scindex ++;
      
      const GeomWireSelection wires = cellmap[cell];
      for (int k=0;k!=wires.size();k++){
	//construct merged wire index
	const GeomWire *wire = wires[k];
	if (swimap.find(wire) == swimap.end()){
	  swimap[wire] = swindex;
	  swindex ++;
	}
      }
    }
  }

  // for (int j=0;j!=wire_all.size();j++){
  //   const GeomWire *wire = wire_all.at(j);
  //   if (swimap.find(wire) == swimap.end()){
  //     swimap[wire] = swindex;
  //     swindex ++;
  //   }
  // }
  
  
}


void WCP2dToy::SimpleBlobToyTiling::SaveResult(){
  chi2_save = cur_chi2;
  //  std::cout << "abc1 " <<std::endl;
  // if (hypo_save.size() !=0){
  //   for (int i = 0;i!=hypo_save.size();i++){
  //     for (int j=0;j!=hypo_save.at(i).size();j++){
  // 	delete hypo_save.at(i).at(j);
  //     }
  //     hypo_save.at(i).clear();
  //   }
  //   hypo_save.clear();
  // }
  // hypo_save = cur_hypo;
  //std::cout << "abc2 " <<std::endl;

  wire_u_save.clear();
  wire_v_save.clear();
  wire_w_save.clear();
  wire_all_save.clear();
  cell_all_save.clear();

  wire_u_save = wire_u;
  wire_v_save = wire_v;
  wire_w_save = wire_w;
  wire_all_save = wire_all;
  cell_all_save = cell_all;

   Cx_save.clear();
  dCx_save.clear();
  Cx_save = Cx;
  dCx_save = dCx;

  //  std::cout << "abc3 " <<std::endl;

  cellmap_save.clear();
  wiremap_save.clear();

  cellmap_save = cellmap;
  wiremap_save = wiremap;
  
  //  std::cout << "abc4 " <<std::endl;

  scimap_save.clear();
  swimap_save.clear();
  
  scindex_save = scindex;
  swindex_save = swindex;
  swimap_save = swimap;
  scimap_save = scimap;

  //  std::cout << "abc6 " <<std::endl;

 
}


void WCP2dToy::SimpleBlobToyTiling::DoTiling(){
  
  GeomWireSelection mwires; //save all the merged wires
  GeomCellSelection mcells; // save other cells, and any associated cells
  GeomCellSelection mcells_no;
  //use merge tiling to do stuff

  //save the first pass merge wires
  for (int i=0;i!=sbcells.size();i++){
    MergeGeomCell* mcell = (MergeGeomCell*) sbcells.at(i);
    if (flag_cell.at(i)!=0){
      GeomWireSelection wires = mergetiling->wires(*mcell);
      for (int j=0;j!=wires.size();j++){
	mwires.push_back(wires.at(j));
      }
    }
  }

  //std::cout << "xin1: " << mcells.size() << " " << mwires.size() << std::endl; 

  //save the first pass merged wires;
  for (int i=0;i!=mwires.size();i++){
    MergeGeomWire* mwire = (MergeGeomWire*) mwires.at(i);
    GeomCellSelection cells = mergetiling->cells(*mwire);
    for (int j=0;j!=cells.size();j++){
      if (toymatrix->Get_Cell_Charge(cells.at(j))>recon_threshold){
	auto it = find(sbcells.begin(),sbcells.end(),cells.at(j));
	if (it==sbcells.end()){
	  mcells.push_back(cells.at(j));
	}
      }else{
	mcells_no.push_back(cells.at(j));
      }
    }
  }
  // std::cout << "xin2: " << mcells.size() << " " << mwires.size() << std::endl; 

  
  //save all ... 
  int flag = 1;
  while(flag){
    flag = 0;
    //do wires
    for (int i=0;i!=mcells.size();i++){
      MergeGeomCell* mcell = (MergeGeomCell*)mcells.at(i);
      GeomWireSelection wires = mergetiling->wires(*mcell);
      for (int j=0;j!=wires.size();j++){
	MergeGeomWire* mwire = (MergeGeomWire*)wires.at(j);
	auto it = find(mwires.begin(),mwires.end(),mwire);
	if (it == mwires.end()){
	  flag = 1;
	  mwires.push_back(mwire);
	}
      }
    }
    //do cells
    for (int i=0;i!=mwires.size();i++){
      MergeGeomWire* mwire = (MergeGeomWire*)mwires.at(i);
      GeomCellSelection cells = mergetiling->cells(*mwire);
      for (int j=0;j!=cells.size();j++){
	MergeGeomCell* mcell = (MergeGeomCell*)cells.at(j);
	auto it = find(mcells.begin(),mcells.end(),mcell);
	auto it1 = find(sbcells.begin(),sbcells.end(),mcell);
	auto it2 = find(mcells_no.begin(),mcells_no.end(),mcell);
	if (it==mcells.end() && it1 == sbcells.end() && it2 == mcells_no.end()){
	  flag = 1;
	  mcells.push_back(mcell);
	}
      }
    }
  }
  //std::cout << "xin3: " << mcells.size() << " " << mwires.size() << std::endl; 



  //use hypothesis to save stuff
  //save all the cells;
  cell_all.clear();
  for (int i = 0; i!=sbcells.size();i++){
    WCP2dToy::HypoSelection hypos = cur_hypo.at(i);
    MergeGeomCell* mcell = (MergeGeomCell*) sbcells.at(i);
    for (int j=0;j!=mcell->get_allcell().size();j++){
      const GeomCell* cell = mcell->get_allcell().at(j);
      for (int k=0;k!=hypos.size();k++){
	WCP2dToy::ToyHypothesis *hypo = (WCP2dToy::ToyHypothesis*) hypos.at(k);

	//std::cout << i << " " << j << " " << k << " " << hypo->IsInside(*cell) << std::endl;

	if (hypo->IsInside(*cell)){
	  cell_all.push_back(cell);
	  break;
	}
      }
    }
  }
  
  for (int i=0;i!=mcells.size();i++){
    MergeGeomCell* mcell = (MergeGeomCell*) mcells.at(i);
    for (int j=0;j!=mcell->get_allcell().size();j++){
      const GeomCell* cell = mcell->get_allcell().at(j);
      cell_all.push_back(cell);
    }
  }
  
  wire_all.clear();
  wire_u.clear();
  wire_v.clear();
  wire_w.clear();

  for (int i=0;i!=mwires.size();i++){
    MergeGeomWire* mwire = (MergeGeomWire*)mwires.at(i);
    for (int j=0;j!=mwire->get_allwire().size();j++){
      const GeomWire* wire = mwire->get_allwire().at(j);
      wire_all.push_back(wire);
      if (wire->plane() == ((WirePlaneType_t)0)){
  	wire_u.push_back(wire);
      }else if (wire->plane() == ((WirePlaneType_t)1)){
  	wire_v.push_back(wire);
      }else{
  	wire_w.push_back(wire);
      }
    }
  }
    
  //use toy tiling to do association
  cellmap.clear();
  wiremap.clear();

  GeomCellMap cmap = toytiling->cmap();
  GeomWireMap wmap = toytiling->wmap();
  
  for (int i=0;i!=cell_all.size();i++){
    const GeomCell* cell = cell_all.at(i);
    cellmap[cell] = cmap[cell];
  }
  for (int i=0;i!=wire_all.size();i++){
    const GeomWire* wire = wire_all.at(i);
    GeomCellSelection cells = wmap[wire];
    GeomCellSelection ncells;
    for (int j=0;j!=cells.size();j++){
      const GeomCell* cell = cells.at(j);
      auto it = find(cell_all.begin(),cell_all.end(),cell);
      if (it != cell_all.end()){
  	ncells.push_back(cell);
      }
    }
    wiremap[wire] = ncells;
  }
  
  
  
}


void WCP2dToy::SimpleBlobToyTiling::FormHypo(){
  // std::cout << ncount << std::endl;
   ClearHypo();

   //std::cout << ncount << std::endl;
  
  if (ncount == 0){
    for (int i=0;i!=nsimple_blob;i++){
      WCP2dToy::HypoSelection hypos;
      if (flag_cell.at(i) == 1){
	// only one hypothsis to connect highest and second highest
	MergeGeomCell *mcell1 = (MergeGeomCell*) first_cell.at(i).at(0);
	MergeGeomCell *mcell2 = (MergeGeomCell*) second_cell.at(i).at(0);
	WCP2dToy::ToyHypothesis *hypo = new WCP2dToy::ToyHypothesis(*mcell1,*mcell2);
	hypos.push_back(hypo);
      }else if (flag_cell.at(i) == 2){
	// two hypothesis, highest and second highest, and then highest to the third
	MergeGeomCell *mcell1 = (MergeGeomCell*) first_cell.at(i).at(0);
	MergeGeomCell *mcell2 = (MergeGeomCell*) second_cell.at(i).at(0);
	MergeGeomCell *mcell3 = (MergeGeomCell*) other_cell.at(i).at(0);
	WCP2dToy::ToyHypothesis *hypo = new WCP2dToy::ToyHypothesis(*mcell1,*mcell2);
	hypos.push_back(hypo);
	
	//do the smallest distance
	double dis1 = pow(mcell1->center().y-mcell3->center().y,2) + pow(mcell1->center().z-mcell3->center().z,2);
	double dis2 = pow(mcell2->center().y-mcell3->center().y,2) + pow(mcell2->center().z-mcell3->center().z,2);
	
	if (dis1 < dis2){
	  hypo = new WCP2dToy::ToyHypothesis(*mcell1,*mcell3);
	}else{
	  hypo = new WCP2dToy::ToyHypothesis(*mcell2,*mcell3);
	}
	hypos.push_back(hypo);
      }else if (flag_cell.at(i) == 3){
	// two hypothesis, highest to third, and second highest to third
	MergeGeomCell *mcell1 = (MergeGeomCell*) first_cell.at(i).at(0);
	MergeGeomCell *mcell2 = (MergeGeomCell*) second_cell.at(i).at(0);
	MergeGeomCell *mcell3 = (MergeGeomCell*) other_cell.at(i).at(0);

	MergeGeomCell *mcell4;
	std::set<int> edge_wires;
      	for (auto it = mcell1->ewires.begin(); it!=mcell1->ewires.end(); it++){
	  edge_wires.insert(*it);
	}
	for (auto it = mcell2->ewires.begin(); it!=mcell2->ewires.end(); it++){
	  edge_wires.insert(*it);
	}
	for (auto it = mcell3->ewires.begin(); it!=mcell3->ewires.end(); it++){
	  edge_wires.insert(*it);
	}
	std::vector<int> not_found;
	for (int j=0;j!=6;j++){
	  if (edge_wires.find(j)==edge_wires.end()){
	    not_found.push_back(j);
	  }
	}
	for (int j=1;j<other_cell.at(i).size();j++){
	  mcell4 = (MergeGeomCell*) other_cell.at(i).at(j);
	  int flag = 1;
	  for (int k=0;k!=not_found.size();k++){
	    auto it = mcell4->ewires.find(not_found.at(k));
	    if (it == mcell4->ewires.end()){
	      flag = 0;
	      break;
	    }
	  }
	  if (flag==1) break;
	}
	WCP2dToy::ToyHypothesis *hypo = new WCP2dToy::ToyHypothesis(*mcell1,*mcell3);
	hypos.push_back(hypo);
	hypo = new WCP2dToy::ToyHypothesis(*mcell2,*mcell4);
	hypos.push_back(hypo);
      }
      cur_hypo.push_back(hypos);
    }
  }else{
    //start to randomize ... 
    for (int i=0;i!=nsimple_blob;i++){
      WCP2dToy::HypoSelection hypos;
      if (flag_cell.at(i) == 1){
	// single track (change to two tracks)
	int n = gRandom->Uniform(0,first_cell.at(i).size());
	MergeGeomCell *mcell1 = (MergeGeomCell*) first_cell.at(i).at(n);
	n = gRandom->Uniform(0,second_cell.at(i).size());
	MergeGeomCell *mcell2 = (MergeGeomCell*) second_cell.at(i).at(n);

	if (other_cell.at(i).size()!=0){
	  n = gRandom->Uniform(0,other_cell.at(i).size());
	  MergeGeomCell *mcell3 = (MergeGeomCell*) other_cell.at(i).at(n);	
	  WCP2dToy::ToyHypothesis *hypo = new WCP2dToy::ToyHypothesis(*mcell1,*mcell2);
	  hypos.push_back(hypo);
	  n = gRandom->Uniform(0.1,1.9);
	  if (n==0){
	    hypo = new WCP2dToy::ToyHypothesis(*mcell1,*mcell3);
	    hypos.push_back(hypo);
	  }else{
	    hypo = new WCP2dToy::ToyHypothesis(*mcell2,*mcell3);
	    hypos.push_back(hypo);
	  }
	}else{
	  WCP2dToy::ToyHypothesis *hypo = new WCP2dToy::ToyHypothesis(*mcell1,*mcell2);
	  hypos.push_back(hypo);
	}
      }else if (flag_cell.at(i) == 2){
	// two hypothesis, highest and second highest, and then highest to the third
	int n = gRandom->Uniform(0,first_cell.at(i).size());
	MergeGeomCell *mcell1 = (MergeGeomCell*) first_cell.at(i).at(n);
	n = gRandom->Uniform(0,second_cell.at(i).size());
	MergeGeomCell *mcell2 = (MergeGeomCell*) second_cell.at(i).at(n);
	
	if (other_cell.at(i).size()!=0){
	  n = gRandom->Uniform(0,other_cell.at(i).size());
	  MergeGeomCell *mcell3 = (MergeGeomCell*) other_cell.at(i).at(n);
	  int nnn = 0;
	  while(mcell3 == mcell2 && nnn<20){
	    n = gRandom->Uniform(0,second_cell.at(i).size());
	    MergeGeomCell *mcell2 = (MergeGeomCell*) second_cell.at(i).at(n);
	    n = gRandom->Uniform(0,other_cell.at(i).size());
	    MergeGeomCell *mcell3 = (MergeGeomCell*) other_cell.at(i).at(n);
	    nnn ++;
	  }
	  
	  WCP2dToy::ToyHypothesis *hypo = new WCP2dToy::ToyHypothesis(*mcell1,*mcell2);
	  hypos.push_back(hypo);
	  n = gRandom->Uniform(0.1,1.9);
	  if (n==0){
	    hypo = new WCP2dToy::ToyHypothesis(*mcell1,*mcell3);
	    hypos.push_back(hypo);
	  }else{
	    hypo = new WCP2dToy::ToyHypothesis(*mcell2,*mcell3);
	    hypos.push_back(hypo);
	  }
	}else{
	  WCP2dToy::ToyHypothesis *hypo = new WCP2dToy::ToyHypothesis(*mcell1,*mcell2);
	  hypos.push_back(hypo);
	}
      }else if (flag_cell.at(i) == 3){
	// two hypothesis, highest to third, and second highest to third
	int n = gRandom->Uniform(0,first_cell.at(i).size());
	MergeGeomCell *mcell1 = (MergeGeomCell*) first_cell.at(i).at(n);
	n = gRandom->Uniform(0,second_cell.at(i).size());
	MergeGeomCell *mcell2 = (MergeGeomCell*) second_cell.at(i).at(n);

	if (other_cell.at(i).size()!=0){
	  n = gRandom->Uniform(0,other_cell.at(i).size());
	  MergeGeomCell *mcell3 = (MergeGeomCell*) other_cell.at(i).at(n);
	  
	  int nnn = 0;
	  while(mcell3 == mcell2 && nnn<20){
	    n = gRandom->Uniform(0,second_cell.at(i).size());
	    MergeGeomCell *mcell2 = (MergeGeomCell*) second_cell.at(i).at(n);
	    n = gRandom->Uniform(0,other_cell.at(i).size());
	    MergeGeomCell *mcell3 = (MergeGeomCell*) other_cell.at(i).at(n);
	    nnn ++;
	  }
	
	  MergeGeomCell *mcell4;
	  std::set<int> edge_wires;
	  for (auto it = mcell1->ewires.begin(); it!=mcell1->ewires.end(); it++){
	    edge_wires.insert(*it);
	  }
	  for (auto it = mcell2->ewires.begin(); it!=mcell2->ewires.end(); it++){
	    edge_wires.insert(*it);
	  }
	  for (auto it = mcell3->ewires.begin(); it!=mcell3->ewires.end(); it++){
	    edge_wires.insert(*it);
	  }
	  std::vector<int> not_found;
	  for (int j=0;j!=6;j++){
	    if (edge_wires.find(j)==edge_wires.end()){
	      not_found.push_back(j);
	    }
	  }
	  
	  //need to improve later .. no random here
	  for (int j=1;j<other_cell.at(i).size();j++){
	    mcell4 = (MergeGeomCell*) other_cell.at(i).at(j);
	    int flag = 1;
	    for (int k=0;k!=not_found.size();k++){
	      auto it = mcell4->ewires.find(not_found.at(k));
	      if (it == mcell4->ewires.end()){
		flag = 0;
		break;
	      }
	    }
	    if (flag==1) break;
	  }
	  
	  n = gRandom->Uniform(0.1,2.9);
	  if (n==0){
	    WCP2dToy::ToyHypothesis *hypo = new WCP2dToy::ToyHypothesis(*mcell1,*mcell2);
	    hypos.push_back(hypo);
	    hypo = new WCP2dToy::ToyHypothesis(*mcell3,*mcell4);
	    hypos.push_back(hypo);
	  }else if (n==1){
	    WCP2dToy::ToyHypothesis *hypo = new WCP2dToy::ToyHypothesis(*mcell1,*mcell3);
	    hypos.push_back(hypo);
	    hypo = new WCP2dToy::ToyHypothesis(*mcell2,*mcell4);
	    hypos.push_back(hypo);
	  }else if (n==2){
	    WCP2dToy::ToyHypothesis *hypo = new WCP2dToy::ToyHypothesis(*mcell1,*mcell4);
	    hypos.push_back(hypo);
	    hypo = new WCP2dToy::ToyHypothesis(*mcell2,*mcell3);
	    hypos.push_back(hypo);
	  }
	}else{
	  WCP2dToy::ToyHypothesis *hypo = new WCP2dToy::ToyHypothesis(*mcell1,*mcell2);
	  hypos.push_back(hypo);
	}
	
      }
      cur_hypo.push_back(hypos);
    }
  }
  ncount ++;
}

void WCP2dToy::SimpleBlobToyTiling::ClearHypo(){
  if (cur_hypo.size() !=0){
    for (int i = 0;i!=cur_hypo.size();i++){
      for (int j=0;j!=cur_hypo.at(i).size();j++){
	delete cur_hypo.at(i).at(j);
      }
      cur_hypo.at(i).clear();
    }
    cur_hypo.clear();
  }
}


void WCP2dToy::SimpleBlobToyTiling::Organize(int nsimple_blob){
  //fill the first rank, second rank and third rank and flag ... 
  // loop through the smcells and find the highest rank, fill in first rank, 
  int max = 0 ;
  int max_bin;

  flag_cell.push_back(0);
  
  //find the maximum ranked cell
  for (int i=0;i!=corner_smcells.at(nsimple_blob).size();i++){
    MergeGeomCell *mcell = (MergeGeomCell*) corner_smcells.at(nsimple_blob).at(i);
    if (cell_rank[mcell] > max){
      max = cell_rank[mcell];
      max_bin = i;
    }
  }

  if (max !=0){
    //if found
    GeomCellSelection cells;
    cells.push_back(corner_smcells.at(nsimple_blob).at(max_bin));


    // loop through everything and fill in everything connected
    int flag = 1;
    while(flag){
      flag = 0;
      for (int i=0;i!=corner_smcells.at(nsimple_blob).size();i++){
	MergeGeomCell *mcell = (MergeGeomCell*) corner_smcells.at(nsimple_blob).at(i);
	auto it = find(cells.begin(),cells.end(),mcell);
	if (it==cells.end()){
	  for (int j=0;j!=cells.size();j++){
	    MergeGeomCell *mcell1 = (MergeGeomCell*) cells.at(j);
	    if (mcell1->Overlap1(*mcell) ){
	      flag = 1;
	      cells.push_back(mcell);
	      break;
	    }
	  }
	}
      }
    }
    first_cell.push_back(cells);
    
    // loop through the smcells and find the second highest rank, fill in second rank
    max = 0 ;
    for (int i=0;i!=corner_smcells.at(nsimple_blob).size();i++){
      MergeGeomCell *mcell = (MergeGeomCell*) corner_smcells.at(nsimple_blob).at(i);
      auto it = find(first_cell.at(nsimple_blob).begin(),first_cell.at(nsimple_blob).end(),mcell);
      if (it == first_cell.at(nsimple_blob).end() && cell_rank[mcell] > max){
	max = cell_rank[mcell];
	max_bin = i;
      }
    }

    if (max!=0){ //if found
      GeomCellSelection cells1;
      cells1.push_back(corner_smcells.at(nsimple_blob).at(max_bin));
      
  

      // loop through everything and fill in everything connected
      flag = 1;
      while(flag){
	flag = 0;
	for (int i=0;i!=corner_smcells.at(nsimple_blob).size();i++){
	  MergeGeomCell *mcell = (MergeGeomCell*) corner_smcells.at(nsimple_blob).at(i);
	
	  auto it = find(cells1.begin(),cells1.end(),mcell);
	  auto it1 = find(first_cell.at(nsimple_blob).begin(),first_cell.at(nsimple_blob).end(),mcell);
	  if (it==cells1.end() && it1 == first_cell.at(nsimple_blob).end()){
	    for (int j=0;j!=cells1.size();j++){
	      MergeGeomCell *mcell1 = (MergeGeomCell*) cells1.at(j);
	      if (mcell1->Overlap1(*mcell)){
		flag = 1;
		cells1.push_back(mcell);
		break;
	      }
	    }
	  }
	}
      }
      second_cell.push_back(cells1);
      // do the third group and fill the flag ... 
      
      std::set<int> edge_wires;
      for (int i=0;i!=first_cell.at(nsimple_blob).size();i++){
	MergeGeomCell* mcell = (MergeGeomCell*) first_cell.at(nsimple_blob).at(i);
	for (auto it = mcell->ewires.begin(); it!=mcell->ewires.end(); it++){
	  edge_wires.insert(*it);
	}
      }
      for (int i=0;i!=second_cell.at(nsimple_blob).size();i++){
	MergeGeomCell* mcell = (MergeGeomCell*) second_cell.at(nsimple_blob).at(i);
	for (auto it = mcell->ewires.begin(); it!=mcell->ewires.end(); it++){
	  edge_wires.insert(*it);
	}
      }

      // for (auto it = edge_wires.begin();it!=edge_wires.end();it++){
      // 	std::cout << *it << " ";
      // }
      // std::cout << std::endl;
      if (edge_wires.size()==6){
	//put everything into the third group
	flag_cell.at(nsimple_blob) = 1;
	GeomCellSelection cells;
	for (int i=0;i!=corner_smcells.at(nsimple_blob).size();i++){
	  MergeGeomCell *mcell = (MergeGeomCell*) corner_smcells.at(nsimple_blob).at(i);
	  auto it = find(second_cell.at(nsimple_blob).begin(),second_cell.at(nsimple_blob).end(),mcell);
	  auto it1 = find(first_cell.at(nsimple_blob).begin(),first_cell.at(nsimple_blob).end(),mcell);
	  if (it==second_cell.at(nsimple_blob).end() && it1 == first_cell.at(nsimple_blob).end()){
	    cells.push_back(mcell);
	  }
	}
	for (int i=0;i!=corner_mcells.at(nsimple_blob).size();i++){
	  MergeGeomCell *mcell = (MergeGeomCell*) corner_mcells.at(nsimple_blob).at(i);
	  cells.push_back(mcell);
	}
	other_cell.push_back(cells);
      }else{
	//two cases
	GeomCellSelection cells;
	std::vector<int> not_found;
	for (int i=0;i!=6;i++){
	  if (edge_wires.find(i)==edge_wires.end()){
	    not_found.push_back(i);
	  }
	}
	int flag = 0;
	for (int i=0;i!=corner_smcells.at(nsimple_blob).size();i++){
	  MergeGeomCell *mcell = (MergeGeomCell*) corner_smcells.at(nsimple_blob).at(i);
	  int flag1 = 1;
	  for (int j=0;j!=not_found.size();j++){
	    if (mcell->ewires.find(not_found.at(j))==mcell->ewires.end()){
	      flag1 = 0;
	    }
	  }
	  if (flag1==1){
	    cells.push_back(mcell);
	    flag = 1;
	  }
	}
	for (int i=0;i!=corner_mcells.at(nsimple_blob).size();i++){
	  MergeGeomCell *mcell = (MergeGeomCell*) corner_mcells.at(nsimple_blob).at(i);
	  int flag1 = 1;
	  for (int j=0;j!=not_found.size();j++){
	    if (mcell->ewires.find(not_found.at(j))==mcell->ewires.end()){
	      flag1 = 0;
	    }
	  }
	  if (flag1==1){
	    cells.push_back(mcell);
	    flag = 1;
	  }
	}
		
	if (flag==0){
	  //find the ones containing any of them ... 
	  for (int i=0;i!=corner_smcells.at(nsimple_blob).size();i++){
	    MergeGeomCell *mcell = (MergeGeomCell*) corner_smcells.at(nsimple_blob).at(i);
	    int flag1 = 0;
	    for (int j=0;j!=not_found.size();j++){
	      if (mcell->ewires.find(not_found.at(j))!=mcell->ewires.end()){
		flag1 = 1;
	      }
	    }
	    if (flag1==1){
	      cells.push_back(mcell);
	    }
	  }
	  for (int i=0;i!=corner_mcells.at(nsimple_blob).size();i++){
	    MergeGeomCell *mcell = (MergeGeomCell*) corner_mcells.at(nsimple_blob).at(i);
	    int flag1 = 0;
	    for (int j=0;j!=not_found.size();j++){
	      if (mcell->ewires.find(not_found.at(j))!=mcell->ewires.end()){
		flag1 = 1;
	      }
	    }
	    if (flag1==1){
	      cells.push_back(mcell);
	    }
	  }
	  flag_cell.at(nsimple_blob) = 3;
	}else{
	  flag_cell.at(nsimple_blob) = 2;
	}
	other_cell.push_back(cells);

	// fill in all the stuff with rank > 8
	for (int i = 0; i!=corner_smcells.at(nsimple_blob).size();i++){
	  MergeGeomCell *mcell = (MergeGeomCell*) corner_smcells.at(nsimple_blob).at(i);
	  auto it = find(cells1.begin(),cells1.end(),mcell);
      	  auto it1 = find(first_cell.at(nsimple_blob).begin(),first_cell.at(nsimple_blob).end(),mcell);
      	  if (it==cells1.end() && it1 == first_cell.at(nsimple_blob).end()){
      	    //std::cout << cell_rank[mcell] << std::endl;
      	    if (cell_rank[mcell] >=3){
      	      second_cell.at(nsimple_blob).push_back(mcell);
      	    }
      	  }
	}
	
      }
    }else{

      // only find the first ... 
      GeomCellSelection cells;
      std::set<int> edge_wires;
      for (int i=0;i!=first_cell.at(nsimple_blob).size();i++){
	MergeGeomCell* mcell = (MergeGeomCell*) first_cell.at(nsimple_blob).at(i);
	for (auto it = mcell->ewires.begin(); it!=mcell->ewires.end(); it++){
	  edge_wires.insert(*it);
	}
      }
      std::vector<int> found;
	for (int i=0;i!=6;i++){
	  if (edge_wires.find(i)!=edge_wires.end()){
	    found.push_back(i);
	  }
	}
      

      //second first try to find the ones does not contain the same as the existing one
      for (int i=0;i!=corner_smcells.at(nsimple_blob).size();i++){
	MergeGeomCell *mcell = (MergeGeomCell*) corner_smcells.at(nsimple_blob).at(i);
	int flag1 = 1;
	for (int j=0;j!=found.size();j++){
	  if (mcell->ewires.find(found.at(j))!=mcell->ewires.end()){
	    flag1 = 0;
	  }
	}
	if (flag1==1){
	  cells.push_back(mcell);
	}
      }
      for (int i=0;i!=corner_mcells.at(nsimple_blob).size();i++){
	MergeGeomCell *mcell = (MergeGeomCell*) corner_mcells.at(nsimple_blob).at(i);
	int flag1 = 1;
	for (int j=0;j!=found.size();j++){
	  if (mcell->ewires.find(found.at(j))!=mcell->ewires.end()){
	    flag1 = 0;
	  }
	}
	if (flag1==1){
	  cells.push_back(mcell);
	}
      }
      second_cell.push_back(cells);
      //put the rest to other 
      cells.clear();
      for (int i=0;i!=corner_smcells.at(nsimple_blob).size();i++){
	MergeGeomCell *mcell = (MergeGeomCell*) corner_smcells.at(nsimple_blob).at(i);
	auto it = find(second_cell.at(nsimple_blob).begin(),second_cell.at(nsimple_blob).end(),mcell);
	auto it1 = find(first_cell.at(nsimple_blob).begin(),first_cell.at(nsimple_blob).end(),mcell);
	if (it==second_cell.at(nsimple_blob).end() && it1 == first_cell.at(nsimple_blob).end()){
	  cells.push_back(mcell);
	}
      }
      for (int i=0;i!=corner_mcells.at(nsimple_blob).size();i++){
	MergeGeomCell *mcell = (MergeGeomCell*) corner_mcells.at(nsimple_blob).at(i);
	auto it = find(second_cell.at(nsimple_blob).begin(),second_cell.at(nsimple_blob).end(),mcell);
	auto it1 = find(first_cell.at(nsimple_blob).begin(),first_cell.at(nsimple_blob).end(),mcell);
	if (it==second_cell.at(nsimple_blob).end() && it1 == first_cell.at(nsimple_blob).end()){
	  cells.push_back(mcell);
	}
      }
      other_cell.push_back(cells);
      flag_cell.at(nsimple_blob) = 1;
    }
  }else{
    GeomCellSelection cells;
    first_cell.push_back(cells);
    second_cell.push_back(cells);
    other_cell.push_back(cells);
  }
  

  std::cout << first_cell.at(0).size() << " " << second_cell.at(0).size() << " " << other_cell.at(0).size() << " " << flag_cell.at(0) << std::endl;

  // //now put everything into hypo_ccells
  // if (hypo_ccells.size() < nsimple_blob + 1){
  //   GeomCellSelectionV qcv; // vector of vector
  
  //   // first deal with smcells
  //   // do a loop, and insert the first element, 
  //   // do a loop, and merge anything that can be merged, erase, if none
  //   // insert the second element until nothing in the smcells;
  //   int flag_new = 1; //insert?
  //   int flag_merge = 0; //merge?
  //   while(corner_smcells[nsimple_blob].size()){
  //     if (flag_merge == 1 && corner_smcells[nsimple_blob].size()!=0){
  //       int temp_flag = 0;
  //       //do the merge part
  //       for (int j=0;j!=corner_smcells[nsimple_blob].size();j++){
  //         MergeGeomCell* mcell1 = (MergeGeomCell*) corner_smcells[nsimple_blob].at(j);
  // 	for (int k = 0;k!=qcv.size();k++){
  // 	  for (int kk=0;kk!=qcv.at(k).size();kk++){
  // 	    MergeGeomCell* mcell2 = (MergeGeomCell*) qcv.at(k).at(kk);
  // 	    if (mcell1->Overlap1(*mcell2)){
  // 	      qcv.at(k).push_back(mcell1);
  // 	      corner_smcells[nsimple_blob].erase(corner_smcells[nsimple_blob].begin() + j);
  // 	      temp_flag = 1;
  // 	      break;
  // 	    }
  // 	  }
  // 	  if (temp_flag==1) break;
  // 	}
  // 	if (temp_flag==1) break;
  //       }
  //       if (temp_flag == 0){
  // 	flag_merge = 0;
  // 	flag_new = 1;
  //       }
  //     }
  
  
  //     if (flag_new == 1 && corner_smcells[nsimple_blob].size()!=0){
  //       //insert an element and delete one ... 
  //       GeomCellSelection qc;
  //       qc.push_back(corner_smcells[nsimple_blob].at(corner_smcells[nsimple_blob].size()-1));
  //       qcv.push_back(qc);
  //       corner_smcells[nsimple_blob].pop_back();
  //       flag_new = 0;
  //       flag_merge = 1;
  //     }
  //   }
  
  //   // then deal with mcells
  //   if (corner_mcells[nsimple_blob].size()!=0)
  //     qcv.push_back(corner_mcells[nsimple_blob]);
  
  //   hypo_ccells.push_back(qcv); //save things in ... 
  // }
  
}


WCP2dToy::SimpleBlobToyTiling::~SimpleBlobToyTiling(){
  for (int i=0;i!=nsimple_blob;i++){
    for (int j=0;j!=corner_mcells[i].size();j++){
      delete corner_mcells[i].at(j);
    }
    for (int j=0;j!=corner_smcells[i].size();j++){
      delete corner_smcells[i].at(j);
    }
  }

  // for (int i = 0 ;i!=hypo_ccells.size(); i++){
  //   for (int j = 0; j!=hypo_ccells.at(i).size();j++){
  //     for (int k=0;k!=hypo_ccells.at(i).at(j).size();k++){
  // 	delete hypo_ccells.at(i).at(j).at(k);
  //     }
  //   }
  // }
}

ClassImp(WCP2dToy::SimpleBlobToyTiling);
