// -------------------------------------------------------------------------
//        CbmRichLightSpot source file
//        Created 16/09/04 by Y.Kharlov@gsi.de
// Light spot is a collection of fired PMTs (hits) produced by one track
// -------------------------------------------------------------------------


#include "TArrayI.h"

#include "CbmRichLightSpot.h"
#include "CbmRichHit.h"
#include "CbmRichPoint.h"
#include "CbmMCTrack.h"
#include "CbmRootManager.h"
#include <map.h>

// -----   Default constructor   -------------------------------------------
CbmRichLightSpot::CbmRichLightSpot()
  : CbmRichRing()
{
  const Int_t kMaxHits = 500;
  fHitIndex = new TArrayI(kMaxHits);
  fNhits = 0;
  fIflag =-1;

}
// -------------------------------------------------------------------------
CbmRichLightSpot::CbmRichLightSpot(const Double_t &x,const Double_t &y,const Double_t &r)
  : CbmRichRing(x,y,r)
{
  const Int_t kMaxHits = 500;
  fHitIndex = new TArrayI(kMaxHits);
  fNhits = 0;
  fIflag =-1;

}

//Destructor by Simeon Lebedev
CbmRichLightSpot::~CbmRichLightSpot(){
	//delete fHitIndex;

}

Int_t CbmRichLightSpot::GetHitIndex (Int_t i)
{
  return fHitIndex->At(i);

}
// -------------------------------------------------------------------------
void CbmRichLightSpot::AddHitIndex (Int_t &hit)
{
  // Add hit index to the array of indices
  fHitIndex->AddAt(hit,fNhits);
  fNhits++;
}

// -------------------------------------------------------------------------

void CbmRichLightSpot::Print(const Option_t *opt) const
{
  // Print light spot data members
  printf("RICH light spot: ");
  printf("track id =%6d, N hits =%3d, center=(%.2f,%.2f) cm, R=%.2f cm\n",
	 fTrackID,fNhits,fCenterX,fCenterY,fRadius);
}

// -------------------------------------------------------------------------

void CbmRichLightSpot::SetRecFlag ( Int_t iflag )
{
  fIflag = iflag;
}

//--------------------------------------------------------------------------

const Int_t CbmRichLightSpot::GetRecFlag()
{
  return fIflag;
}

//--------------------------------------------------------------------------

void CbmRichLightSpot::RingFit()
{
  // Fit the light spot by equation of circle
  // to find the circle radius and center
  //
  // Translated from F77 subroutine of S.Sadovsky

  if (fNhits < 3) {
    fRadius  = 0;
    fCenterX = 0;
    fCenterY = 0;
    return;
  }


  CbmRootManager *manager= CbmRootManager::Instance();
  TClonesArray *hitList = (TClonesArray*)manager->GetObject("RichHit");
  CbmRichHit *hit = 0;

  Float_t c[3], a[3][3];

  Float_t b1 = 0;
  Float_t b2 = 0;
  Float_t b3 = 0;

  Float_t b12 = 0;
  Float_t b22 = 0;
  Float_t b32 = fNhits;

  Float_t a11 = 0;
  Float_t a12 = 0;
  Float_t a13;
  Float_t a21;
  Float_t a22 = 0;
  Float_t a23;
  Float_t a31;
  Float_t a32;
  Float_t a33;

  for (Int_t i=0; i<fNhits; i++) {
    hit = (CbmRichHit*)hitList->At(fHitIndex->At(i));
    b1 += (hit->X()*hit->X() + hit->Y()*hit->Y()) * hit->X();
    b2 += (hit->X()*hit->X() + hit->Y()*hit->Y()) * hit->Y();
    b3 += (hit->X()*hit->X() + hit->Y()*hit->Y());

    b12 += hit->X();
    b22 += hit->Y();

    a11 += 2*hit->X()*hit->X();
    a12 += 2*hit->X()*hit->Y();
    a22 += 2*hit->Y()*hit->Y();
  }

  a21 = a12;

  a13 = b12;
  a23 = b22;

  a31 = 2*b12;
  a32 = 2*b22;
  a33 = b32;

  c[0] = b1*b22 - b2*b12;
  c[1] = b1*b32 - b3*b12;
  c[2] = b2*b32 - b3*b22;

  a[0][0] = a11*b22 - a21*b12;
  a[1][0] = a12*b22 - a22*b12;
  a[2][0] = a13*b22 - a23*b12;

  a[0][1] = a11*b32 - a31*b12;
  a[1][1] = a12*b32 - a32*b12;
  a[2][1] = a13*b32 - a33*b12;

  a[0][2] = a21*b32-a31*b22;
  a[1][2] = a22*b32-a32*b22;
  a[2][2] = a23*b32-a33*b22;

  Float_t det1 = a[0][0]*a[1][1] - a[0][1]*a[1][0];

  Float_t x11 = (c[0]*a[1][1] - c[1]*a[1][0])/det1;
  Float_t x21 = (a[0][0]*c[1] - a[0][1]*c[0])/det1;

//   Float_t det2 = a[0][1]*a[1][2] - a[0][2]*a[1][1];
//   Float_t det3 = a[0][0]*a[1][2] - a[0][2]*a[1][0];
//   Float_t x12 = (c[1]*a[1][2] - c[2]*a[1][1])/det2;
//   Float_t x22 = (a[0][1]*c[2] - a[0][2]*c[1])/det2;

  fRadius  = TMath::Sqrt((b3 + b32*(x11*x11 + x21*x21) -
			  a31*x11 - a32*x21)/a33);
  fCenterX = x11;
  fCenterY = x21;

}

const Int_t CbmRichLightSpot::GetTrackID()
{
  if(fTrackID >= -1)
    return fTrackID;

  CbmRootManager *manager= CbmRootManager::Instance();
  TClonesArray *hitList = (TClonesArray*)manager->GetObject("RichHit");
  TClonesArray *pointList = (TClonesArray*)manager->GetObject("RichPoint");
  TClonesArray *mcList = (TClonesArray*)manager->GetObject("MCTrack");

  map< Int_t, Int_t > hitmap;
  for(Int_t i=0; i<fNhits; i++) {
//     printf("Hit index: %d\n", fHitIndex->At(i));
    CbmRichHit* hit = (CbmRichHit*)hitList->At(fHitIndex->At(i));
//     hit->Print();
    if (hit->GetRefIndex() < 0) continue; // fake hits
    CbmRichPoint* point = (CbmRichPoint*)pointList->At(hit->GetRefIndex());
    CbmMCTrack* photon = (CbmMCTrack*)mcList->At(point->GetTrackID());
    Int_t photon_mother_id = photon->GetMotherID();

    if( hitmap.find( photon_mother_id )==hitmap.end() ) {
      hitmap.insert( pair<Int_t,Int_t>( photon_mother_id, 0 ) );
    }
    hitmap[ photon_mother_id ]++;
  }

  Int_t pdg;
  for( map<Int_t,Int_t>::iterator j=hitmap.begin(); j!=hitmap.end(); ++j )
    {
      Double_t from_same_track = ((Double_t)j->second) / fNhits;
      pdg = ((CbmMCTrack*)mcList->At((Int_t)j->first))->GetPdgCode();
//       printf("from_same_track: (PDG %d) %.1f pct\n",pdg,from_same_track*100);
      if ( from_same_track < 0.7 ) continue;
      fTrackID = (Int_t)j->first;
      break;
    }

    /*
  if(fTrackID >= -1) fReconstructed =  kTRUE;
  else
    fReconstructed = kFALSE;
  return fTrackID;
  */
}

void CbmRichLightSpot::RingFitCOP()
{
  // Fit the light spot by equation of circle
  // to find the circle radius and center
  // By Alexander Ayriyan 10.08.2005

  if (fNhits < 3) {
      fRadius  = 0;
      fCenterX = 0;
      fCenterY = 0;
      return;
    }

  CbmRootManager *manager= CbmRootManager::Instance();
  TClonesArray *hitList = (TClonesArray*)manager->GetObject("RichHit");
  CbmRichHit *hit = 0;

	int i,iter,iterMax=20;

    double Xi,Yi,Zi;
    double M0,Mx,My,Mz,Mxy,Mxx,Myy,Mxz,Myz,Mzz,Mxz2,Myz2,Cov_xy;
//    double temp;
    double A0,A1,A2,A22,epsilon=0.000000000001;
    double Dy,xnew,xold,ynew,yold=100000000000.;
    double GAM,DET;
    double Xcenter,Ycenter,Radius;

    M0 = fNhits;
    Mx=My=0.;


  for(i = 0; i < fNhits; i++) {
  	    hit = (CbmRichHit*)hitList->At(fHitIndex->At(i));
     Mx += hit->X();
     My += hit->Y();
  }

    Mx /= M0;
    My /= M0;

//computing moments (note: all moments are normed, i.e. divided by N)
    Mxx=Myy=Mxy=Mxz=Myz=Mzz=0.;

    for (i = 0; i < fNhits; i++) {
    	hit = (CbmRichHit*)hitList->At(fHitIndex->At(i));
        Xi = hit->X() - Mx;
        Yi = hit->Y() - My;
        Zi = Xi*Xi + Yi*Yi;

        Mxy += Xi*Yi;
        Mxx += Xi*Xi;
        Myy += Yi*Yi;
        Mxz += Xi*Zi;
        Myz += Yi*Zi;
        Mzz += Zi*Zi;
    }
    Mxx /= M0;
    Myy /= M0;
    Mxy /= M0;
    Mxz /= M0;
    Myz /= M0;
    Mzz /= M0;

	//computing the coefficients of the characteristic polynomial
    Mz = Mxx + Myy;
    Cov_xy = Mxx*Myy - Mxy*Mxy;
    Mxz2 = Mxz*Mxz;
    Myz2 = Myz*Myz;

    A2 = 4.*Cov_xy - 3.*Mz*Mz - Mzz;
    A1 = Mzz*Mz + 4.*Cov_xy*Mz - Mxz2 - Myz2 - Mz*Mz*Mz;
    A0 = Mxz2*Myy + Myz2*Mxx - Mzz*Cov_xy - 2.*Mxz*Myz*Mxy + Mz*Mz*Cov_xy;

    A22 = A2 + A2;
    iter = 0;
    xnew = 0.;

	//Newton's method starting at x=0
    for(iter = 0; iter < iterMax; iter++) {
        ynew = A0 + xnew*(A1 + xnew*(A2 + 4.*xnew*xnew));

        if (fabs(ynew)>fabs(yold))
        {
		//  printf("Newton2 goes wrong direction: ynew=%f  yold=%f\n",ynew,yold);
            xnew = 0.;
            break;
        }

        Dy = A1 + xnew*(A22 + 16.*xnew*xnew);
        xold = xnew;
        xnew = xold - ynew/Dy;

        if (fabs((xnew-xold)/xnew) < epsilon) break;
    }

    if (iter == iterMax-1) {
	//  printf("Newton2 does not converge in %d iterations\n",iterMax);
        xnew = 0.;
    }
    if (xnew<0.)
    {
        iter=30;
//      printf("Negative root:  x=%f\n",xnew);
    }

//    computing the circle parameters
    GAM = - Mz - xnew - xnew;
    DET = xnew*xnew - xnew*Mz + Cov_xy;
    Xcenter = (Mxz*(Myy-xnew) - Myz*Mxy)/DET/2.;
    Ycenter = (Myz*(Mxx-xnew) - Mxz*Mxy)/DET/2.;
    Radius = TMath::Sqrt(Xcenter*Xcenter+Ycenter*Ycenter-GAM);

    fCenterX = Xcenter + Mx;
    fCenterY = Ycenter + My;
    fRadius  = Radius;

}


void CbmRichLightSpot::RingFitTAU()
{
  // Fit the light spot by equation of circle
  // to find the circle radius and center
  // By Alexander Ayriyan 10.08.2005

  if (fNhits < 3) {
      fRadius  = 0;
      fCenterX = 0;
      fCenterY = 0;
      return;
    }

  CbmRootManager *manager= CbmRootManager::Instance();
  TClonesArray *hitList = (TClonesArray*)manager->GetObject("RichHit");
  CbmRichHit *hit = 0;

   int i,iter,iterMax=20;

    double Xi,Yi,Zi;
    double M0,Mx,My,Mz,Mxy,Mxx,Myy,Mxz,Myz,Mzz,Mxz2,Myz2,Cov_xy;
//    double temp;
    double A0,A1,A2,A22,A3,A33,epsilon=0.000000000001;
    double Dy,xnew,xold,ynew,yold=100000000000.;
    double GAM,DET;
    double Xcenter,Ycenter,Radius;

    M0 = fNhits;
    Mx=My=0.;

    for (i = 0; i < fNhits; i++) {
	hit = (CbmRichHit*)hitList->At(fHitIndex->At(i));
     	Mx += hit->X();
     	My += hit->Y();
    }
    Mx /= M0;
    My /= M0;

//     computing moments (note: all moments are normed, i.e. divided by N)
    Mxx=Myy=Mxy=Mxz=Myz=Mzz=0.;

    for (i=0; i < fNhits; i++) {
	hit = (CbmRichHit*)hitList->At(fHitIndex->At(i));
        Xi = hit->X() - Mx;
        Yi = hit->Y() - My;
        Zi = Xi*Xi + Yi*Yi;

        Mxy += Xi*Yi;
        Mxx += Xi*Xi;
        Myy += Yi*Yi;
        Mxz += Xi*Zi;
        Myz += Yi*Zi;
        Mzz += Zi*Zi;
    }
    Mxx /= M0;
    Myy /= M0;
    Mxy /= M0;
    Mxz /= M0;
    Myz /= M0;
    Mzz /= M0;

//    computing the coefficients of the characteristic polynomial
    Mz = Mxx + Myy;
    Cov_xy = Mxx*Myy - Mxy*Mxy;
    Mxz2 = Mxz*Mxz;
    Myz2 = Myz*Myz;

    A3 = 4.*Mz;
    A2 = - 3.*Mz*Mz - Mzz;
    A1 = Mzz*Mz + 4.*Cov_xy*Mz - Mxz2 - Myz2 - Mz*Mz*Mz;
    A0 = Mxz2*Myy + Myz2*Mxx - Mzz*Cov_xy - 2.*Mxz*Myz*Mxy+Mz*Mz*Cov_xy;

    A22 = A2 + A2;
    A33 = A3 + A3 + A3;
    iter = 0;
    xnew = 0.;

//    Newton's method starting at x=0
    for (iter=0; iter<iterMax; iter++) {
        ynew = A0 + xnew*(A1 + xnew*(A2 + xnew*A3));

        if (fabs(ynew)>fabs(yold)) {
//            printf("Newton3 goes wrong direction: ynew=%f  yold=%f\n",ynew,yold);
            xnew = 0.;
            break;
        }

        Dy = A1 + xnew*(A22 + xnew*A33);
        xold = xnew;
        xnew = xold - ynew/Dy;

        if (fabs((xnew-xold)/xnew) < epsilon) break;
    }

    if (iter == iterMax-1) {
//        printf("Newton3 does not converge in %d iterations\n",iterMax);
        xnew = 0.;
    }
    if (xnew<0.) {
        iter=30;
//        printf("Negative root:  x=%f\n",xnew);
    }

//    computing the circle parameters
    GAM = - Mz;
    DET = xnew*xnew - xnew*Mz + Cov_xy;
    Xcenter = (Mxz*(Myy-xnew) - Myz*Mxy)/DET/2.;
    Ycenter = (Myz*(Mxx-xnew) - Mxz*Mxy)/DET/2.;
    Radius = TMath::Sqrt(Xcenter*Xcenter+Ycenter*Ycenter-GAM);


  fCenterX = Xcenter + Mx;
  fCenterY = Ycenter + My;
  fRadius  = Radius;


}
// -------------------------------------------------------------------------
void CbmRichLightSpot::RingRobustFitCOP(){
	if(fNhits < 3){
		fRadius  = 0.;
		fCenterX = 0.;
		fCenterY = 0.;
		return;
	}
	CbmRootManager *manager= CbmRootManager::Instance();
	TClonesArray *hitList = (TClonesArray*)manager->GetObject("RichHit");
	CbmRichHit *hit = NULL;
	int i,iter,iterMax=20;
	int i_iter, i_max_Robust = 4;
	const int MinNuberOfHits = 3;
	const int MaxNuberOfHits = 2000;
	double Xi,Yi,Zi;
	double M0,Mx,My,Mz,Mxy,Mxx,Myy,Mxz,Myz,Mzz,Mxz2,Myz2,Cov_xy;//,temp;
	double A0,A1,A2,A22,epsilon=0.000000000001;
	double Dy,xnew,xold,ynew,yold=100000000000.;
	double GAM,DET;
	double Xcenter,Ycenter,Radius;
	double SumS1 = 0., SumS2 = 0.;
	double sigma;
	double ctsigma;
	double sigma_min = 0.05;
	double dx, dy, radius;
	double d[MaxNuberOfHits];
	double w[MaxNuberOfHits];
	double ct = 7.;

    for(i = 0; i < MaxNuberOfHits; i++) w[i] = 1.;

    Mx=My=0.;
    for(i = 0; i < fNhits; i++) {
        hit = (CbmRichHit*)hitList->At(fHitIndex->At(i));
        Mx += hit->X();
        My += hit->Y();
    }

    M0 = fNhits;
    Mx /= M0;
    My /= M0;

	for(i_iter = 0; i_iter < i_max_Robust; i_iter++){
		sigma = sigma_min;
		if(i_iter != 0){
			for(i = 0; i < fNhits; i++){
				hit = (CbmRichHit*)hitList->At(fHitIndex->At(i));
				dx = Mx - hit->X();
				dy = My - hit->Y();
				d[i] = sqrt(dx*dx + dy*dy) - Radius;
				SumS1 += w[i]*d[i]*d[i];
				SumS2 += w[i];
			}
			if(SumS2 == 0.){ sigma_min; } else{ sigma = sqrt(SumS1/SumS2);}
			if(sigma < sigma_min) sigma = sigma_min;
			ctsigma = ct*sigma;
			SumS1 = 0.;
			SumS2 = 0.;
			for(i = 0; i < fNhits; i++){
				if(d[i] <= ctsigma){
					w[i] = (1 - (d[i]/(ctsigma))*(d[i]/(ctsigma)))*(1 - (d[i]/(ctsigma))*(d[i]/(ctsigma)));
				}else{w[i] = 0.;}
			}
		}
		//computing moments (note: all moments are normed, i.e. divided by N)
		M0 = 0;
		Mxx=Myy=Mxy=Mxz=Myz=Mzz=0.;
		for (i = 0; i < fNhits; i++) {
			hit = (CbmRichHit*)hitList->At(fHitIndex->At(i));
			if(w[i] != 0.){
				Xi = hit->X() - Mx;
				Yi = hit->Y() - My;
				Zi = Xi*Xi + Yi*Yi;
				Mxy += Xi*Yi;
				Mxx += Xi*Xi;
				Myy += Yi*Yi;
				Mxz += Xi*Zi;
				Myz += Yi*Zi;
				Mzz += Zi*Zi;
				M0++;
			}
		}
		if(M0 < MinNuberOfHits){
			M0 = 0;
			Mxx=Myy=Mxy=Mxz=Myz=Mzz=0.;
			M0 = 0;
			for (i = 0; i < fNhits; i++) {
				hit = (CbmRichHit*)hitList->At(fHitIndex->At(i));
				Xi = hit->X() - Mx;
				Yi = hit->Y() - My;
				Zi = Xi*Xi + Yi*Yi;
				Mxy += Xi*Yi;
				Mxx += Xi*Xi;
				Myy += Yi*Yi;
				Mxz += Xi*Zi;
				Myz += Yi*Zi;
				Mzz += Zi*Zi;
				M0++;
			}
		}
		Mxx /= M0;
		Myy /= M0;
		Mxy /= M0;
		Mxz /= M0;
		Myz /= M0;
		Mzz /= M0;

		//computing the coefficients of the characteristic polynomial
		Mz = Mxx + Myy;
		Cov_xy = Mxx*Myy - Mxy*Mxy;
		Mxz2 = Mxz*Mxz;
		Myz2 = Myz*Myz;

		A2 = 4.*Cov_xy - 3.*Mz*Mz - Mzz;
		A1 = Mzz*Mz + 4.*Cov_xy*Mz - Mxz2 - Myz2 - Mz*Mz*Mz;
		A0 = Mxz2*Myy + Myz2*Mxx - Mzz*Cov_xy - 2.*Mxz*Myz*Mxy + Mz*Mz*Cov_xy;

		A22 = A2 + A2;
		iter = 0;
		xnew = 0.;

		//Newton's method starting at x=0
		for(iter = 0; iter < iterMax; iter++) {
			ynew = A0 + xnew*(A1 + xnew*(A2 + 4.*xnew*xnew));

			if (fabs(ynew)>fabs(yold))
			{
			//  printf("Newton2 goes wrong direction: ynew=%f  yold=%f\n",ynew,yold);
				xnew = 0.;
				break;
			}

			Dy = A1 + xnew*(A22 + 16.*xnew*xnew);
			xold = xnew;
			xnew = xold - ynew/Dy;

			if (fabs((xnew-xold)/xnew) < epsilon) break;
		}

		if (iter == iterMax-1) {
		//  printf("Newton2 does not converge in %d iterations\n",iterMax);
			xnew = 0.;
		}
		if (xnew<0.)
		{
			iter=30;
	//      printf("Negative root:  x=%f\n",xnew);
		}

	//    computing the circle parameters
		GAM = - Mz - xnew - xnew;
		DET = xnew*xnew - xnew*Mz + Cov_xy;
		Xcenter = (Mxz*(Myy-xnew) - Myz*Mxy)/DET/2.;
		Ycenter = (Myz*(Mxx-xnew) - Mxz*Mxy)/DET/2.;
		//cout << " Radius = " << Radius << endl;
		Radius = sqrt(Xcenter*Xcenter+Ycenter*Ycenter-GAM);
		Xcenter = Xcenter + Mx;
		Ycenter = Ycenter + My;
		Mx = Xcenter;
		My = Ycenter;

		if (DET == 0.) {
			fRadius  = 0.;
			fCenterX = 0.;
			fCenterY = 0.;
			return;
		}

	}
	fCenterX = Xcenter;
	fCenterY = Ycenter;
	fRadius = Radius;
}
// -------------------------------------------------------------------------


ClassImp(CbmRichLightSpot)