#include "TpcPrelimCluster.h"
#include <iostream>
#include <algorithm>
#include "TpcDigiMapper.h"
#include "TpcPadPlane.h"
#include "TpcCluster.h"
#include "McId.h"
#include "McIdCollection.h"

TpcPrelimCluster::TpcPrelimCluster(TpcPadPlane* p, double t, int id, double G, double C) :
  famp(0.), fcogT(0.), fpadplane(p), ftimeslice(t), fid(id), fG(G), fC(C), fnUnsharedDigis(0)
{
}

TpcPrelimCluster::TpcPrelimCluster(TpcPadPlane* p, int id, double G, double C) :
  famp(0.), fcogT(0.), fpadplane(p), ftimeslice(0), fid(id), fG(G), fC(C), fnUnsharedDigis(0)
{
}

TpcPrelimCluster::~TpcPrelimCluster()
{
}


void TpcPrelimCluster::addHit(TpcDigi* digi, bool noXclust, double share, bool check) {

  // check if digi is already there
  if (check && std::find(fdigis.begin(), fdigis.end(), digi) != fdigis.end()){
    fdigiShares[digi] += share;
  }
  else {
    fdigis.push_back(digi);
    fdigiShares[digi] = share;
  }

  // update amp and cogT 
  double sharedAmp = digi->amp()*fdigiShares[digi];
  fcogT += sharedAmp * digi->t(); // not normalized!
	famp  += sharedAmp;
  //fcogT*=1./famp;

	if (fdigiShares[digi] > 0.9999) ++fnUnsharedDigis;

	if (check) return; // we are in the second cluster merging pass - no updating of neighbours necessary!

	// update neighbours
  fpossiblePads.insert(digi->padId());	
  TpcPad* pad = fpadplane->GetPad(digi->padId());
  // only add neighbors to possible pads if digi is not shared!
  for(unsigned int ineigh=0; ineigh<pad->nNeighbours(); ++ineigh){
    if(noXclust){
      TpcPad* neigh = fpadplane->GetPad( pad->getNeighbour(ineigh) );
      if(fabs(pad->x()-neigh->x())>0.01) continue; //gt 0.1mm
    }
    if (share > 0.9999) fpossiblePads.insert(pad->getNeighbour(ineigh));
    else fpossiblePadsShared.insert(pad->getNeighbour(ineigh));
  }

}


bool TpcPrelimCluster::isInTimeWindow(const TpcDigi* const digi){
  if( fabs(digi->t() - fcogT/famp) <= ftimeslice ) // fcogT has to be normalized with famp
    return true;
  return false;
}


int TpcPrelimCluster::isInCluster(const TpcDigi* const digi) {
  if(!isInTimeWindow(digi))
    return 0; // not a neighbour
  int padID = digi->padId();
  if(fpossiblePads.find(padID) == fpossiblePads.end()) {  // not a neighbour to any unshared digi
    if(fpossiblePadsShared.find(padID) == fpossiblePadsShared.end()) return 0;  // not a neighbour to any shared digi
    return 2;   // neighbour to a shared digi
  }
  return 1; // neighbour to an unshared digi
}


TpcCluster* TpcPrelimCluster::convTpcCluster(unsigned int sectorId, double zJitter, bool saveRaw) {
  cog(zJitter); // also error is calculated here
  TpcCluster* c = new TpcCluster(fpos,ferr,famp,fid);
  c->SetMcId(fmcidCol);
  //always store digis inside the cluster, only index and weight is copied
  for(unsigned int i=0;i<fdigis.size();++i){
    if(fdigis[i]->isSaturated()){
      c->setSaturation(true);
    }
    c->addDigi(fdigis[i], fdigiShares[fdigis[i]]);
  }
  
  c->SetSector(sectorId);
  
  return c;
}


void TpcPrelimCluster::cog(double zJitter){
  bool DEBUG=false;

  fpos.SetXYZ(0,0,0);
  famp=0;
  McIdCollection mcid;
  unsigned int ndigis=fdigis.size();

  // loop over digis to calculate cog
  TpcDigi* adigi;
  TVector3 thispos;
  for(unsigned int id=0;id<ndigis;++id){
    adigi = fdigis[id];
    double a = (double)adigi->amp()*fdigiShares[adigi];
    mcid.AddIDCollection(adigi->mcId(),a);
    try{
      TpcDigiMapper::getInstance()->map(adigi,thispos);
    }
    catch(const std::exception& ex) {
      std::cout<<ex.what()<<std::endl;
    }
    fpos += a*thispos;
    famp += a;
  }
  fpos *= 1./famp;


  // calculate errors: ------------------------------------------
  double Dl = TpcDigiMapper::getInstance()->getGas()->Dl();
  double Dt = TpcDigiMapper::getInstance()->getGas()->Dt();
  double driftl = fabs(fpos.z() - TpcDigiMapper::getInstance()->zGem());

  //if(DEBUG) {
  //  std::cout<<"TpcSectorProcessor: Gas DiffL: "<<Dl
  //     <<", Gas DiffT: "<<Dt<<std::endl;
  //}
  /*if(DEBUG) {
    std::cout<<"TpcSectorProcessor: zGem is "
       <<TpcDigiMapper::getInstance()->zGem()<<std::endl
       <<", drift length: "<<driftl<<std::endl;
  }*/

  double diffSigmaT = Dt * Dt * driftl;
  double diffSigmaL = Dl * Dl * driftl;

  double dx, dy;
  TpcDigiMapper::getInstance()->padsize(fdigis[0]->padId(),dx,dy);

	//std::cout<<"dx, dy = "<<dx<<"   "<<dy<<"\n";

  /*ferr=(0.,0.,0.);
  TVector3 df, thissig;
  for(unsigned int id=0; id<ndigis; ++id){
    adigi = fdigis[id];
    double a = (double)adigi->amp()*fdigiShares[adigi];
    try {
      TpcDigiMapper::getInstance()->map(adigi,thispos);
    }
    catch(const std::exception& ex) {
      std::cout<<ex.what()<<std::endl;
    }
      
    df = thispos-fpos;
    double sigmaX_sq = a*df.X()*df.X();
    double sigmaY_sq = a*df.Y()*df.Y();
    double sigmaZ_sq = a*df.Z()*df.Z();
    
    thissig.SetXYZ(sigmaX_sq, sigmaY_sq, sigmaZ_sq);
    ferr += thissig;
  } // end second loop over digis

  // normalize ferr
  ferr *= 1./famp;*/

  // if single pad cluster
  /*if(ferr.X()<1E-5) ferr.SetX(sqrt(dx*dx/12+diffSigmaT));
  if(ferr.Y()<1E-5) ferr.SetY(sqrt(dy*dy/12+diffSigmaT));
  if(ferr.Z()<1E-5) ferr.SetZ(sqrt(zJitter*zJitter/12+diffSigmaL));*/

  // calculate number of ionization electrons
  double nEff = famp / fG; // = amp/gain * (electrons/ADCChannel)

  // scale with 1/sqrt(amplitude), diffusion term and finite pad-pitch term
  ferr.SetX( fC * pow(nEff, -0.5) /** (diffSigmaT + dx/sqrt(12)*exp(-driftl*Dl)     )*/ );
  ferr.SetY( fC * pow(nEff, -0.5) /** (diffSigmaT + dy/sqrt(12)*exp(-driftl*Dl)     )*/ );
  ferr.SetZ( fC * pow(nEff, -0.5) /** (diffSigmaL + zJitter/sqrt(12)*exp(-driftl*Dt))*/ );
  
  //std::cout<<"famp + fG*diffSigmaT * fC = "<<famp << " + " << fG << " * " << diffSigmaT << " * " << fC << " = " <<famp << " + " << fG*diffSigmaT * fC << "\n";
  //if (driftl < 10) std::cout<<"dx = "<< dx << "; Dl = " << Dl << "; driftl = " << driftl << ";    dx/sqrt(12)*exp(-driftl*Dl) = " << dx/sqrt(12)*exp(-driftl*Dl) << ";    fC*fG*diffSigmaT = " << fC*fG*diffSigmaT <<"\n";

  /*if(sig.X()<1E-5)sig.SetX(sqrt(dx*dx/12+diffSigmaT));
  else sig.SetX(sqrt((sig.X()+fG*diffSigmaT)/amp)*fC/amp);
  if(sig.Y()<1E-5)sig.SetY(sqrt(dy*dy/12+diffSigmaT));
  else sig.SetY(sqrt((sig.Y()+fG*diffSigmaT)/amp)*fC/amp);
  if(sig.Z()<1E-5)sig.SetZ(sqrt(zDiff*zDiff/12+diffSigmaL));
  else sig.SetZ(sqrt((sig.Z()+fG*diffSigmaL)/amp)*fC/amp);*/

  fmcidCol = mcid;
}


TpcPrelimCluster* TpcPrelimCluster::getSharedCluster(TpcDigi* d, unsigned int i){
  std::set<TpcPrelimCluster*>::iterator it = fSharedClusters[d].begin();
  for (unsigned int j=0; j<i; ++j){
    ++it;
  }
  return *it;
}