#include "WCP2dToy/ToyMatrixIterate_Only.h"

using namespace WCP;

WCP2dToy::ToyMatrixIterate_Only::~ToyMatrixIterate_Only(){

}

WCP2dToy::ToyMatrixIterate_Only::ToyMatrixIterate_Only(WCP2dToy::ToyMatrix &toymatrix, WCP2dToy::MergeToyTiling* mergetiling)
  : toymatrix(toymatrix)
  , mergetiling(mergetiling)
{
  wirechargemap = mergetiling->wcmap();
  cellmap = mergetiling->cmap();
  
  // get all the cells,
  all_cells = mergetiling->get_allcell();
 
  ncount = 0;
  nlevel = 0;

  WCP::GeomCellSelection good_cells;
  WCP::GeomWireSelection used_wires;
  WCP::GeomCellSelection bad_cells;
  WCP::GeomCellSelection tried_cells;

  std::cout << "Test: " << all_cells.size() << std::endl;

  Iterate(all_cells,good_cells,used_wires,bad_cells,tried_cells);
 
}

void WCP2dToy::ToyMatrixIterate_Only::Iterate(GeomCellSelection remaining_cells, GeomCellSelection good_cells, GeomWireSelection used_wires, GeomCellSelection bad_cells, GeomCellSelection& tried_cells){
  nlevel ++;

  for (int i=0;i!=remaining_cells.size();i++){
    GeomCellSelection remaining_cells_1 = remaining_cells;
    GeomCellSelection good_cells_1 = good_cells;
    GeomWireSelection used_wires_1 = used_wires;
    GeomCellSelection bad_cells_1 = bad_cells;

    MergeGeomCell *mcell = (MergeGeomCell*)remaining_cells.at(i);
    
    auto it1 = find(tried_cells.begin(),tried_cells.end(),mcell);
    if (it1 == tried_cells.end()){
      // put it into tried cells
      tried_cells.push_back(mcell);

      // put it into good_cells
      auto it2 = find(good_cells_1.begin(),good_cells_1.end(),mcell);
      if (it2 == good_cells_1.end()){
	good_cells_1.push_back(mcell);
	// put its wires into used_wires
	GeomWireSelection wires = cellmap[mcell];
	for (int j=0;j!=wires.size();j++){
	  if (wirechargemap[wires.at(j)] > 10){
	    auto it3 = find(used_wires_1.begin(),used_wires_1.end(),wires.at(j));
	    if (it3 == used_wires_1.end())
	      used_wires_1.push_back(wires.at(j));
	  }
	}
      }
      
      // remove it from remaining_cells
      auto it3 = find(remaining_cells_1.begin(),remaining_cells_1.end(),mcell);
      remaining_cells_1.erase(it3);

      GeomCellSelection to_be_removed;
      // fill bad_cells and further reduce remaining_cells
      for (int j=0;j!=remaining_cells_1.size();j++){
	GeomWireSelection wires = cellmap[remaining_cells_1.at(j)];
	for (int k=0;k!=wires.size();k++){
	  if (wirechargemap[wires.at(k)]>10){
	    auto it4 = find(used_wires.begin(),used_wires.end(),wires.at(k));
	    if (it4 != used_wires.end()){
	      to_be_removed.push_back(remaining_cells_1.at(j));
	      break;
	    }
	  }
	}
      }
      for (int j=0;j!=to_be_removed.size();j++){
	auto it4 = find(remaining_cells_1.begin(),remaining_cells_1.end(),to_be_removed.at(j));
	remaining_cells_1.erase(it4);
	auto it5 = find(bad_cells_1.begin(),bad_cells_1.end(),to_be_removed.at(j));
	if (it5 == bad_cells_1.end())
	  bad_cells_1.push_back(to_be_removed.at(j));
      }
      
      if (remaining_cells_1.size() == 0 ){
	// if bad_cells + good_cells == all_cells, solve
	std::vector<int> already_removed; 
	std::vector<int> no_need_remove; 
	for (int j=0;j!=good_cells_1.size();j++){
	  int index = toymatrix.Get_mcindex(good_cells_1.at(j));
	  no_need_remove.push_back(index);
	}
	for (int j=0; j!= bad_cells_1.size();j++){
	  int index = toymatrix.Get_mcindex(bad_cells_1.at(j));
	  already_removed.push_back(index);
	}
	 WCP2dToy::ToyMatrixKalman toykalman(already_removed, no_need_remove, toymatrix,0,0);
	 ncount ++;
	 
	 std::cout << "Test: " << all_cells.size() << " " << bad_cells_1.size() + good_cells_1.size() << " " << toykalman.Get_numz() << " " << nlevel << " " << ncount << std::endl;
      }else{
	// otherwise, go deep
	Iterate(remaining_cells_1, good_cells_1, used_wires_1, bad_cells_1, tried_cells);
      }
      

    }
  }
  
  
  nlevel -- ;

}


