#include <iostream>
#include <map>
#include <vector>
#include <fstream>


//#include "runinfo.h"
#include "PndSdsChargeWeightingAlgorithms.h"
#include "PndSdsDigi.h"
#include "PndSdsCluster.h"
#include "PndSdsCalcStrip.h"

#include "TArray.h"
//using namespace std;
ClassImp(PndSdsChargeWeightingAlgorithms);

PndSdsChargeWeightingAlgorithms::PndSdsChargeWeightingAlgorithms(TClonesArray* arr) : TObject()
{
	fDigiArray = arr;
  fNoise=0.;
  fVerbose=0;
}
PndSdsChargeWeightingAlgorithms::~PndSdsChargeWeightingAlgorithms()
{
}

/*
 returns the (modified) error function
 this is a local (private) function
 */
Double_t PndSdsChargeWeightingAlgorithms::Erfmod(Double_t x, Double_t p0, Double_t p1, Double_t p2, Double_t p3)
{
  return ((TMath::Erf(p0*(x-p1)))*p2+p3);
}

std::pair<Double_t,Double_t> PndSdsChargeWeightingAlgorithms::CenterOfGravity(const PndSdsCluster* Cluster)
{
  std::pair<Double_t,Double_t> result;
  Int_t nrHits = Cluster->GetClusterSize();
  if(nrHits>1)							// minimum of hits in cluster
  {
    Double_t x_g=0., chargesum=0, charge=0, noise=0, stripno=0;
    Double_t xerror=0.,xerrtmp=0.,cherr=0.;
    noise = fCalcStrip->GetNoise();
    for(Int_t l=0;l<nrHits;++l)     // loop over all hits
    {
      // we work in cannel numbers, so no pitch used
      //
      //           ( q_i*chan_i )
      // x_g= SUM ( ------------ )
      //           (   Q_sum    )
      //     
      //
      
      charge = DigiCharge(Cluster->GetDigiIndex(l));
      stripno = DigiStripno(Cluster->GetDigiIndex(l));
      chargesum+=charge;
      x_g += charge * stripno ;
      if(fVerbose>2) Info("CenterOfGravity","Adding digi values (stripno,charge) = (%f,%f)",stripno,charge);
    }
    x_g = x_g/chargesum;
    result.first=x_g;
    
    // error estimation propagate dq:
    //      Sqrt (SUM{ ((x_i - x)dq_i)^2 })
    // dx=  -------------------------------
    //                    Q
    //
    for(Int_t l=0;l<nrHits;++l)     // loop over all hits again (errors)
    {
      cherr = DigiChargeError(Cluster->GetDigiIndex(l));
      cherr = sqrt(noise*noise+cherr*cherr);
      xerrtmp = ( x_g - DigiStripno(Cluster->GetDigiIndex(l)) ) * cherr;
      xerror += xerrtmp*xerrtmp;
    }
    xerror = sqrt(xerror)/(chargesum);
    
    if (xerror < 1e-15) 
      Warning("CenterOfGravity","Got bad error value: Cluster with %i digis. Position %f +-%f chn.",nrHits,x_g,xerror);
    result.second = xerror;
    
  }else{
    result=Binary(Cluster);
  }
  if(fVerbose>1) Info("CenterOfGravity","Got a cluster with %i digis. Position %f +- %f chn.",nrHits,result.first,result.second);
  return result;
}


std::pair<Double_t,Double_t> PndSdsChargeWeightingAlgorithms::HeadTail(const PndSdsCluster* Cluster)
{
  // We assume that the digis in each cluster are sorted.
  std::pair<Double_t,Double_t> result;
  Int_t nrHits = Cluster->GetClusterSize();
  
  if(nrHits<2) return Binary(Cluster);
  
  Double_t x_h=DigiStripno(Cluster->GetDigiIndex(0));
  Double_t x_t=DigiStripno(Cluster->GetDigiIndex(nrHits-1));
  Double_t q_h=DigiCharge(Cluster->GetDigiIndex(0));
  Double_t q_t=DigiCharge(Cluster->GetDigiIndex(nrHits-1));
  Double_t err=0;
  Double_t temmperr=0;
  
  Double_t q_inner=0.;
  if(nrHits==2) { // this will result in the same result as CoG for nrHits==2
    Double_t xm=(q_h*x_h + q_t*x_t)/(q_h+q_t);
    result.first=xm;
    err = (x_h-xm)*(x_h-xm)+(x_t-xm)*(x_t-xm);
    err=sqrt(err);
    err*=fNoise/(q_h+q_t);
    result.second=err;
  } else {
    for(Int_t l=1;l<(nrHits-1);l++) q_inner += DigiCharge(Cluster->GetDigiIndex(l));
    q_inner = q_inner/(nrHits-2);
    Double_t wert=(q_t-q_h)/(2.*q_inner);
    if(wert<0) wert=-wert;
    result.first=(0.5*(x_h+x_t)+wert);
    
    // error caclculation: dx^2 = noise^2*{n*w^2/Q^2+1/(2q^2)}
    err=wert/(q_h+q_t+q_inner); err*=err; err*=nrHits;
    temmperr=1/q_inner; temmperr*=temmperr; temmperr*=0.5;
    err+=temmperr;  
    err=sqrt(err);
    err*=fNoise;
    
    result.second=err;
  }
  return result;
}


std::pair<Double_t,Double_t> PndSdsChargeWeightingAlgorithms::Binary(const PndSdsCluster* Cluster)
{
  std::pair<Double_t,Double_t> result;
  Int_t nrHits = Cluster->GetClusterSize();
  Double_t charge=0.,chargemax=0;
  Double_t channel=0;
  for(Int_t i=0;i<nrHits;++i)
  {
    charge=DigiCharge(Cluster->GetDigiIndex(i));
    if( charge > chargemax)
    { // choose highest signal
      chargemax=charge;
      channel=DigiStripno(Cluster->GetDigiIndex(i));
    }
  }
  result.first=channel;
  result.second=1./sqrt(12.);
  if(fVerbose>1) Info("Binary","Got a cluster with %i digis. Position %f +- %f chn.",nrHits,result.first,result.second);
  return result;
}

//std::pair<Double_t,Double_t> PndSdsChargeWeightingAlgorithms::Eta(const PndSdsCluster* Cluster)
//{
//  //TODO: Make a good MVD eta-par file. Eta Algo needed at all?
//  std::pair<Double_t,Double_t> result;				// first calc eta!
//  Int_t nrHits = Cluster->GetClusterSize();
//  if(nrHits>1)
//  {
//    Double_t ql=DigiCharge(Cluster->GetDigiIndex(0));
//    Double_t qr=DigiCharge(Cluster->GetDigiIndex(nrHits-1));
//
//    Int_t f=1, o=nrHits-2;
//    Double_t xl=DigiStripno(Cluster->GetDigiIndex(0))*1.;
//
//    Double_t p0,pe0,p1,pe1,p2,pe2,p3,pe3;				// f(eta) parameters calc before
//    std::ifstream InPar("/home/student/Desktop/repository/calibPar/Eta_para.par");
//    InPar >> p0>>pe0>>p1>>pe1>>p2>>pe2>>p3>>pe3;
//    InPar.close();
//    while(f<o)							// build groups nearly same charge value
//    {
//      if(ql<=qr) 						// who is smaller
//      {
//        ql=ql+DigiCharge(Cluster->GetDigiIndex(f));
//        ++f;
//      }
//      if(qr<ql)
//      {
//        qr=qr+DigiCharge(Cluster->GetDigiIndex(o));
//        //             xl=xl+Erfmod(sube, p0, p1, p2, p3);
//        --o;
//      }
//    }
//
//    Double_t e = qr/(qr+ql);					// total eta
//
//    result.first=xl+f-1+Erfmod(e, p0, p1, p2, p3);				// reconstruct
//  }else{
//    return Binary(Cluster);
//  }
//  result.second=0.;
//  return result;
//}

std::pair<Double_t,Double_t> PndSdsChargeWeightingAlgorithms::Eta(const PndSdsCluster* Cluster, const TH1F* PosVsEta)//, const TH1F* pitch3)
{
	Int_t nrHits = Cluster->GetClusterSize();
  
  if(nrHits < 2.){return Binary(Cluster);}
  if(nrHits > 2.){return CenterOfGravity(Cluster);}
  
  if(nrHits == 2. && DigiStripno(Cluster->GetDigiIndex(1))-DigiStripno(Cluster->GetDigiIndex(0))==1.)
  {
    
    std::pair<Double_t,Double_t> result;
    
    std::pair<Double_t,Double_t> eta_value;
    Double_t stripno=0.;
    Int_t 	 NmbOfStrips=0;
    
    eta_value = EtaValue(Cluster, stripno, NmbOfStrips);
    
    result.first=PosVsEta->GetBinContent(ceil(eta_value.first * 200.));           //etadist histogram contains 200 bins.
    
    result.second=(PosVsEta->GetBinContent(ceil((eta_value.first+eta_value.second) * 200.))
                   - PosVsEta->GetBinContent(ceil((eta_value.first-eta_value.second) * 200.)))/2.;
    
    return result;
    
  }
}

std::pair<Double_t,Double_t> PndSdsChargeWeightingAlgorithms::EtaValue(const PndSdsCluster* Cluster, Double_t &stripno, Int_t &NmbOfStrips)
{
  
	Int_t nrHits = Cluster->GetClusterSize();
  std::pair<Double_t,Double_t> result;
  
  if(nrHits==2. && DigiStripno(Cluster->GetDigiIndex(1)) - DigiStripno(Cluster->GetDigiIndex(0))==1.)							// 2 strips fired
  {
    NmbOfStrips=2;
		Double_t ql=0., qr=0., noise=0., cherrl=0., cherrr=0.;
    noise = fCalcStrip->GetNoise();
    
    ql = DigiCharge(Cluster->GetDigiIndex(0));
    qr = DigiCharge(Cluster->GetDigiIndex(1));
    cherrr = DigiChargeError(Cluster->GetDigiIndex(1));
    cherrr = sqrt(noise*noise+cherrr*cherrr);
    
    cherrl = DigiChargeError(Cluster->GetDigiIndex(0));
    cherrl = sqrt(noise*noise+cherrl*cherrl);
    
    stripno = DigiStripno(Cluster->GetDigiIndex(0));
    
    result.first=qr/(qr+ql);
    PndSdsDigiStrip* digil = (PndSdsDigiStrip*)(fDigiArray->At(Cluster->GetDigiIndex(0)));
    PndSdsDigiStrip* digir = (PndSdsDigiStrip*)(fDigiArray->At(Cluster->GetDigiIndex(1)));
    
    result.second=sqrt(((ql/(qr+ql))*(1./(qr+ql))*(ql/(qr+ql))*(1./(qr+ql))*cherrr*cherrr)+((qr/(qr+ql))*(1./(qr+ql))*(qr/(qr+ql))*(1./(qr+ql))*cherrl*cherrl));
    return result;
  }
  
  if(nrHits==3 || (DigiStripno(Cluster->GetDigiIndex(1)) - DigiStripno(Cluster->GetDigiIndex(0))==2. && nrHits==2))		// 3 strips fired, sometimes middle strip is empty
  {
    
    Double_t ql=0., qr=0., qm=0., noise=0., cherrl=0., cherrr=0.;
    noise = fCalcStrip->GetNoise();
    NmbOfStrips=3;
    
    if(nrHits==3){
      ql = DigiCharge(Cluster->GetDigiIndex(0));
      qm = DigiCharge(Cluster->GetDigiIndex(1));
      qr = DigiCharge(Cluster->GetDigiIndex(2));
      cherrr = sqrt(TMath::Power(DigiChargeError(Cluster->GetDigiIndex(2)),2.)+TMath::Power((DigiChargeError(Cluster->GetDigiIndex(1))/2.),2.));
      cherrl = sqrt(TMath::Power(DigiChargeError(Cluster->GetDigiIndex(0)),2.)+TMath::Power((DigiChargeError(Cluster->GetDigiIndex(1))/2.),2.));
      
    }else{
      ql = DigiCharge(Cluster->GetDigiIndex(0));
      qr = DigiCharge(Cluster->GetDigiIndex(1));
      qm=3000.;
      cherrr = sqrt(TMath::Power(DigiChargeError(Cluster->GetDigiIndex(1)),2.)+TMath::Power((noise/2.),2.));
      cherrl = sqrt(TMath::Power(DigiChargeError(Cluster->GetDigiIndex(0)),2.)+TMath::Power((noise/2.),2.));
      PndSdsDigiStrip* digil = (PndSdsDigiStrip*)(fDigiArray->At(Cluster->GetDigiIndex(0)));
      PndSdsDigiStrip* digir = (PndSdsDigiStrip*)(fDigiArray->At(Cluster->GetDigiIndex(1)));
      std::cout<<"strip no charge: UNBL:   "<<DigiStripno(Cluster->GetDigiIndex(0))<<"  "<<digil->GetCharge()<<"  "<<DigiStripno(Cluster->GetDigiIndex(1))<<"  "<<digir->GetCharge()<<std::endl;
      
    }
    
    ql+=qm/2.;
    qr+=qm/2.;
    
    cherrr = sqrt(noise*noise+cherrr*cherrr);
    cherrl = sqrt(noise*noise+cherrr*cherrr);
    stripno = DigiStripno(Cluster->GetDigiIndex(0));
    result.first=qr/(qr+ql);
    result.second=sqrt(((ql/(qr+ql))*(1./(qr+ql))*(ql/(qr+ql))*(1./(qr+ql))*cherrr*cherrr)+((qr/(qr+ql))*(1./(qr+ql))*(qr/(qr+ql))*(1./(qr+ql))*cherrl*cherrl));
    return result;
  }
  
  result.first=-1.;
  result.second=-1.;
  return result;
}


std::pair<Double_t,Double_t> PndSdsChargeWeightingAlgorithms::AutoSelect(const PndSdsCluster* Cluster)
{
  Warning("auto_select", "Do not use, this selection is still wrong!");
  switch(Cluster->GetClusterSize())
  {
    case 1:
      return Binary(Cluster);
      break;
    case 2:
      //return eta(Cluster); // switched off (no par file)
      return CenterOfGravity(Cluster);
      break;
    case 3:
      return CenterOfGravity(Cluster);
      break;
    case 4:
      return CenterOfGravity(Cluster);
      break;
    default:
      return HeadTail(Cluster);
      break;
  }
  return HeadTail(Cluster);
}
/*
 void PndSdsChargeWeightingAlgorithms::MakedNdEta(const std::vector<PndSdsCluster>& Cluster, RunInfo& info)
 {
 for(Int_t cID=0;cID<Cluster->size();cID++)
 {
 Int_t nrHits = Cluster[cID].GetClusterSize();
 if(nrHits>1)
 {
 Double_t ql=Cluster[cID].GetHit(0).GetCharge()*1.;
 Double_t qr=Cluster[cID].GetHit(nrHits-1).GetCharge()*1.;
 
 Int_t f=1, o=nrHits-2;
 Double_t xl=Cluster[cID].GetHit(0).GetChannel()*1.;
 while(f<o)							// build groups nearly same charge value
 {
 if(ql<=qr) 						// who is smaller
 {
 ql=ql+Cluster[cID].GetHit(f).GetCharge()*1.;
 ++f;
 }
 if(qr<ql)
 {
 qr=qr+Cluster[cID].GetHit(o).GetCharge()*1.;
 //             xl=xl+Erfmod(sube, p0, p1, p2, p3);
 --o;
 }
 }
 Double_t e = qr/(qr+ql);					// total eta
 info.FilldNdEta(e);
 }
 }
 return;
 }
 */

Double_t PndSdsChargeWeightingAlgorithms::DigiCharge(Int_t digiIndex)
{
  PndSdsDigiStrip* digi = (PndSdsDigiStrip*)(fDigiArray->At(digiIndex));
  Double_t charge = fChargeConverter->DigiValueToCharge(digi->GetCharge());
  //Info("DigiCharge","digi=%p chargedigi=%f charge=%f",digi,digi->GetCharge(), charge);
  //digi->Print();
  return charge;
}

Double_t PndSdsChargeWeightingAlgorithms::DigiChargeError(Int_t digiIndex)
{
  Double_t cherr = DigiCharge(digiIndex);
  cherr *= fChargeConverter->GetRelativeError(cherr); 
  return cherr;
}


Int_t PndSdsChargeWeightingAlgorithms::DigiStripno(Int_t digiIndex)
{
	Int_t strip;
	SensorSide side;
	PndSdsDigiStrip* myDigi = (PndSdsDigiStrip*)fDigiArray->At(digiIndex);
	fCalcStrip->CalcFeChToStrip(myDigi->GetFE(), myDigi->GetChannel(), strip, side);
  return strip;
}