#include <TClonesArray.h>
#include "AliITSURecoLayer.h"
#include "AliITSsegmentation.h"
#include "AliITSUAux.h"
#include "AliITSUClusterPix.h"
#include "AliITSUGeomTGeo.h"
#include "AliLog.h"

using namespace AliITSUAux;
using namespace TMath;

ClassImp(AliITSURecoLayer)


//______________________________________________________
AliITSURecoLayer::AliITSURecoLayer(const char* name)
  :fActiveID(-1)
  ,fNSensors(0)
  ,fNSensorRows(0)
  ,fNSensorsPerRow(0)
  ,fSensVIDtoMatrixID(0)
  ,fR(0)
  ,fRMax(0)
  ,fRMin(0)
  ,fZMax(0)
  ,fZMin(0)
  ,fPhiOffs(0)
  ,fSensDZInv(0)
  ,fSensDPhiInv(0)
  ,fMaxStep(0.5)
  ,fSensors(0)
  ,fITSGeom(0)
  ,fClusters(0)
{
  // def. c-tor
  SetNameTitle(name,name);
}

//______________________________________________________
AliITSURecoLayer::AliITSURecoLayer(const char* name, Int_t activeID, AliITSUGeomTGeo* gm)
  :fActiveID(activeID)
  ,fNSensors(0)
  ,fNSensorRows(0)
  ,fNSensorsPerRow(0)
  ,fSensVIDtoMatrixID(0)
  ,fR(0)
  ,fRMax(0)
  ,fRMin(0)
  ,fZMax(0)
  ,fZMin(0)
  ,fPhiOffs(0)
  ,fSensDZInv(0)
  ,fSensDPhiInv(0)
  ,fMaxStep(0.5)
  ,fSensors(0)
  ,fITSGeom(gm)
  ,fClusters(0)
{
  // def. c-tor
  SetNameTitle(name,name);
  Build();
}

//______________________________________________________
AliITSURecoLayer::~AliITSURecoLayer()
{
  // def. d-tor
  delete fSensors;
  delete[] fSensVIDtoMatrixID;
  if (GetOwnsClusterArray()) delete fClusters;
}

//______________________________________________________
void AliITSURecoLayer::Print(Option_t* opt) const			      
{
  //print 
  printf("Lr %-15s %d (act:%+d), NSens: %4d in %d rows| MaxStep:%.2f ",
	 GetName(),GetID(),GetActiveID(),GetNSensors(),GetNSensorRows(),fMaxStep);
  printf("%6.3f<R<%6.3f | %+8.3f<Z<%+8.3f dZ:%6.3f dPhi:%6.3f\n",fRMin,fRMax,fZMin,fZMax,
	 fSensDZInv>0 ? 1/fSensDZInv : 0, fSensDPhiInv>0 ? 1/fSensDPhiInv : 0);
  TString opts = opt; opts.ToLower();
  if (opts.Contains("sn")) for (int i=0;i<GetNSensors();i++) GetSensor(i)->Print(opt);
}

//______________________________________________________
void AliITSURecoLayer::Build()
{
  // build internal structures
  const double kSafeR = 0.05; // safety margin for Rmin,Rmax of the layer
  if (fActiveID<0) return;
  //
  int nStaves=fITSGeom->GetNStaves(fActiveID);
  // determine number of sensor rows (sensors aligned at same phi and spanning the Z range of the layer)
  fNSensorRows = nStaves;
  //
  // if the stave has susbtaves, each substave can have multiple rows of sensors (but just 1 row of modules)
  if (fITSGeom->GetNHalfStaves(fActiveID)>0) fNSensorRows *= fITSGeom->GetNHalfStaves(fActiveID);
  //
  // if there are modules defined, the module may have multiple rows of sensors (though not spanning full Z)
  if (fITSGeom->GetNModules(fActiveID)>0) fNSensorRows *= fITSGeom->GetNChipRowsPerModule(fActiveID);
  //
  fNSensors = fITSGeom->GetNChipsPerLayer(fActiveID);
  fNSensorsPerRow = fNSensors/fNSensorRows;
  //
  fSensors = new TObjArray(fNSensors);
  fSensVIDtoMatrixID = new Int_t[fNSensors];
  const AliITSsegmentation* kSegm = fITSGeom->GetSegmentation(fActiveID);
  //
  TGeoHMatrix mmod;
  const TGeoHMatrix* mt2l;
  double phiTF,rTF, loc[3]={0,0,0},glo[3];
  //
  int nSensPerStave = fITSGeom->GetNChipsPerStave(fActiveID);
  for (int staveI=0;staveI<nStaves;staveI++) {
    for (int sensI=0;sensI<nSensPerStave;sensI++) {
      int sID = fITSGeom->GetChipIndex(fActiveID,staveI,sensI);
      AliITSURecoSens* sens = new AliITSURecoSens( sID );
      fSensors->AddLast(sens);
      double phiMin=1e9,phiMax=-1e9,zMin=1e9,zMax=-1e9;
      // this is NOT the sensor matrix, just the ideal chip matrix to get neighbors correct
      fITSGeom->GetOrigMatrix(sID,mmod); 
      //
      for (int ix=0;ix<2;ix++) {       // determine sensor boundaries (ideal)
	loc[0] = (ix-0.5)*kSegm->Dx(); // +-DX/2
	for (int iy=0;iy<2;iy++) {
	  loc[1] = (iy-0.5)*kSegm->Dy(); // +-DY/2
	  for (int iz=0;iz<2;iz++) {
	    loc[2] = (iz-0.5)*kSegm->Dz(); // +-DZ/2
	    mmod.LocalToMaster(loc,glo);
	    double phi = ATan2(glo[1],glo[0]);
	    BringTo02Pi(phi);
	    if      (phiMin>1e8)  phiMin=phi;
	    else if (!OKforPhiMin(phiMin,phi)) phiMin=phi;
	    if      (phiMax<-1e8) phiMax=phi; 
	    else if (!OKforPhiMax(phiMax,phi)) phiMax=phi;	      
	    if (glo[2]>zMax) zMax=glo[2];
	    if (glo[2]<zMin) zMin=glo[2];
	  }
	}
      }
      sens->SetBoundaries(phiMin,phiMax,zMin,zMax);
    }
  }
  fSensors->Sort(); // sort sensors to get the neighborhood correct
  //
  // now fill real sensor angles, Z's, accounting for misalignment
  fRMin=fZMin=1e9;
  fRMax=fZMax=-1e9;
  //
  fPhiOffs = 0;
  int firstSensID = fITSGeom->GetFirstChipIndex(fActiveID);
  for (int sensI=0;sensI<fNSensors;sensI++) {
    AliITSURecoSens* sens = GetSensor(sensI);
    mmod = *fITSGeom->GetMatrixSens(sens->GetID());
    fSensVIDtoMatrixID[sens->GetID() - firstSensID] = sensI;
    double phiMin=1e9,phiMax=-1e9,zMin=1e9,zMax=-1e9;
    for (int ix=0;ix<2;ix++) {
      loc[0] = (ix-0.5)*kSegm->Dx(); // +-DX/2
      for (int iy=0;iy<2;iy++) {
	loc[1] = (iy-0.5)*kSegm->Dy(); // +-DY/2
	for (int iz=0;iz<2;iz++) {
	  loc[2] = (iz-0.5)*kSegm->Dz(); // +-DZ/2
	  //
	  mmod.LocalToMaster(loc,glo);
	  double phi = ATan2(glo[1],glo[0]);
	  double r   = glo[0]*glo[0] + glo[1]*glo[1];
	  if (fRMin>r) fRMin = r;
	  if (fRMax<r) fRMax = r;
	  BringTo02Pi(phi);
	  if      (phiMin>1e8) phiMin=phi; 
	  else if (!OKforPhiMin(phiMin,phi)) phiMin=phi;
	  if      (phiMax<-1e8) phiMax=phi;
	  else if (!OKforPhiMax(phiMax,phi)) phiMax=phi;	      
	  if (glo[2]>zMax) zMax=glo[2];
	  if (glo[2]<zMin) zMin=glo[2];
	}
      }
    }
    mt2l = fITSGeom->GetMatrixT2L( sens->GetID() );
    mmod.Multiply(mt2l);	
    loc[0]=loc[1]=loc[2]=0;
    mmod.LocalToMaster(loc,glo);
    rTF   = Sqrt(glo[0]*glo[0] + glo[1]*glo[1]);  //  tracking params (misaligned)
    phiTF = ATan2(glo[1],glo[0]);
    BringTo02Pi(phiTF);
    //
    sens->SetXTF(rTF);
    sens->SetPhiTF(phiTF);
    sens->SetBoundaries(phiMin,phiMax,zMin,zMax);
    if (fZMin>zMin) fZMin = zMin;
    if (fZMax<zMax) fZMax = zMax;
    //
    if (sensI<fNSensorsPerRow) fPhiOffs += MeanPhiSmall(phiMax,phiMin);
  }
  //
  fPhiOffs /= fNSensorsPerRow; // average phi of the 1st row
  fSensDZInv = fNSensorsPerRow/(fZMax-fZMin);
  fSensDPhiInv = fNSensorRows/(2*Pi());
  //
  fRMin = Sqrt(fRMin);
  fRMax = Sqrt(fRMax);
  fR = 0.5*(fRMin+fRMax);
  fRMin -= kSafeR;
  fRMax += kSafeR;
  //
}

//______________________________________________________
Int_t AliITSURecoLayer::FindSensors(const double* impPar, AliITSURecoSens *sensors[kNNeighbors])
{
  // find sensors having intersection with track
  // impPar contains: lab phi of track, dphi, labZ, dz
  //
  return 0;
  /*
  double z = impPar[2];
  if (z>fZMax+impPar[3]) return 0; // outside of Z coverage
  z -= fZMin;
  if (z<-impPar[3]) return 0; // outside of Z coverage
  int sensInRow = int(z*fSensDZInv);
  if      (sensInRow<0) sensInRow = 0;
  else if (sensInRow>=fNSensorsPerRow) sensInRow = fNSensorsPerRow-1;
  //
  double phi = impPar[0] - fPhiOffs;
  BringTo02Pi(phi);
  int rowID = int(phi*fSensDPhiInv);  // stave id
  //
  int sensID = rowID*fNSensorsPerRow + sensInRow; // most probable candidate
  int nsens = 0;
  //
  AliITSURecoSens* sensN,*sens = GetSensor(sesnID);
  //
  int res = sens->CheckCoverage(impPar);

  // make sure this is best matching sensor
  if (sens->GetZMin()<impPar[2] && sens->GetZMax()>impPar[2] && 
      OKforPhiMin(sens->GetPhiMin(),impPar[0]) && OKforPhiMax(sens->GetPhiMax(),impPar[0]) ) 

  sensors[nsens++] = sens;
  //
  // check neighbours
  double zMn=impPar[2]-impPar[3], zMx=impPar[2]+impPar[3], phiMn=impPar[0]-impPar[1], phiMx=impPar[0]+impPar[1];
  BringTo02Pi(phiMn);
  BringTo02Pi(phiMx);
  //
  sensN = GetSensor(sens->GetNeighborID(AliITSURecoSens::kNghbR)); // neighbor on the right (smaller phi)
  if (sensN && OKforPhiMin(phiMn,sensN->GetPhiMax())) sensors[nsens++] = sensN;
  //
  sensN = GetSensor(sens->GetNeighborID(AliITSURecoSens::kNghbTR)); // neighbor on the top right (smaller phi, larger Z)
  if (sensN && OKforPhiMin(phiMn,sensN->GetPhiMax()) && sensN->GetZMin()<zMx) sensors[nsens++] = sensN;
  //
  sensN = GetSensor(sens->GetNeighborID(AliITSURecoSens::kNghbT)); // neighbor on the top (larger Z)
  if (sensN && sensN->GetZMin()<zMx) sensors[nsens++] = sensN;
  //
  sensN = GetSensor(sens->GetNeighborID(AliITSURecoSens::kNghbTL)); // neighbor on the top left (larger Z, larger phi)
  if (sensN && OKforPhiMax(phiMx,sensN->GetPhiMin()) && sensN->GetZMin()<zMx) sensors[nsens++] = sensN;
  //
  sensN = GetSensor(sens->GetNeighborID(AliITSURecoSens::kNghbL)); // neighbor on the left (larger phi)
  if (sensN && OKforPhiMax(phiMx,sensN->GetPhiMin())) sensors[nsens++] = sensN;
  //
  sensN = GetSensor(sens->GetNeighborID(AliITSURecoSens::kNghbBL)); // neighbor on the bottom left (smaller Z, larger phi)
  if (sensN && OKforPhiMax(phiMx,sensN->GetPhiMin()) && sensN->GetZMax()>zMn) sensors[nsens++] = sensN;
  //
  sensN = GetSensor(sens->GetNeighborID(AliITSURecoSens::kNghbB));  // neighbor on the bottom (smaller Z)
  if (sensN && sensN->GetZMax()>zMn) sensors[nsens++] = sensN;
  //
  sensN = GetSensor(sens->GetNeighborID(AliITSURecoSens::kNghbBR)); // neighbor on the bottom right (smaller Z, smaller phi)
  if (sensN && OKforPhiMin(phiMn,sensN->GetPhiMax()) && sensN->GetZMax()>zMn) sensors[nsens++] = sensN;
  //
  return nsens;
  */
}

//*/
/*
Int_t AliITSURecoLayer::FindSensors(const double* impPar, AliITSURecoSens *sensors[AliITSURecoSens::kNNeighbors],int mcLab)
{
  // find sensors having intersection with track
  // impPar contains: lab phi of track, dphi, labZ, dz

  //tmp>>>
  int nFnd = 0;
  int fndSens[50];
  if (mcLab>=0) { // find correct sensors from MC info
    int ncl = GetNClusters();
    for (int icl=ncl;icl--;) {
      AliCluster* cl = GetCluster(icl);
      for (int ilb=0;ilb<3;ilb++) {
	if (cl->GetLabel(ilb)<0) break;
	if (cl->GetLabel(ilb)==mcLab) {fndSens[nFnd++] = cl->GetVolumeId(); break;}
      }
    }
    if (nFnd>0) {
      Int_t layS,staS,sensS;
      for (int is=0;is<nFnd;is++) {
	fITSGeom->GetChipId(fndSens[is],layS,staS,sensS);
	printf("SNMC#%d(%d): %d %d %d | ",is,mcLab,layS,staS,sensS); GetSensorFromID(fndSens[is])->Print();
      }
    }
  }

  //tmp<<<

  double z = impPar[2];
  if (z>fZMax+impPar[3]) {
    if (nFnd>0) printf("MissedSens!!!\n");
    return 0; // outside of Z coverage
  }
  z -= fZMin;
  if (z<-impPar[3]) {
    if (nFnd>0) printf("MissedSens!!!\n");
    return 0; // outside of Z coverage
  }
  int sensInSta = int(z*fSensDZInv);
  if      (sensInSta<0) sensInSta = 0;
  else if (sensInSta>=fNSensInStave) sensInSta = fNSensInStave-1;
  //
  double phi = impPar[0] - fPhiOffs;
  BringTo02Pi(phi);
  int staID = int(phi*fSensDPhiInv);  // stave id
  int nsens = 0;
  //
  AliITSURecoSens* sensN,*sens = GetSensor(staID*fNSensInStave+sensInSta);
  sensors[nsens++] = sens;
  //
  // check neighbours
  double zMn=impPar[2]-impPar[3], zMx=impPar[2]+impPar[3], phiMn=impPar[0]-impPar[1], phiMx=impPar[0]+impPar[1];
  BringTo02Pi(phiMn);
  BringTo02Pi(phiMx);
  //
  sensN = GetSensor(sens->GetNeighborID(AliITSURecoSens::kNghbR)); // neighbor on the right (smaller phi)
  if (sensN && OKforPhiMin(phiMn,sensN->GetPhiMax())) sensors[nsens++] = sensN;
  //
  sensN = GetSensor(sens->GetNeighborID(AliITSURecoSens::kNghbTR)); // neighbor on the top right (smaller phi, larger Z)
  if (sensN && OKforPhiMin(phiMn,sensN->GetPhiMax()) && sensN->GetZMin()<zMx) sensors[nsens++] = sensN;
  //
  sensN = GetSensor(sens->GetNeighborID(AliITSURecoSens::kNghbT)); // neighbor on the top (larger Z)
  if (sensN && sensN->GetZMin()<zMx) sensors[nsens++] = sensN;
  //
  sensN = GetSensor(sens->GetNeighborID(AliITSURecoSens::kNghbTL)); // neighbor on the top left (larger Z, larger phi)
  if (sensN && OKforPhiMax(phiMx,sensN->GetPhiMin()) && sensN->GetZMin()<zMx) sensors[nsens++] = sensN;
  //
  sensN = GetSensor(sens->GetNeighborID(AliITSURecoSens::kNghbL)); // neighbor on the left (larger phi)
  if (sensN && OKforPhiMax(phiMx,sensN->GetPhiMin())) sensors[nsens++] = sensN;
  //
  sensN = GetSensor(sens->GetNeighborID(AliITSURecoSens::kNghbBL)); // neighbor on the bottom left (smaller Z, larger phi)
  if (sensN && OKforPhiMax(phiMx,sensN->GetPhiMin()) && sensN->GetZMax()>zMn) sensors[nsens++] = sensN;
  //
  sensN = GetSensor(sens->GetNeighborID(AliITSURecoSens::kNghbB));  // neighbor on the bottom (smaller Z)
  if (sensN && sensN->GetZMax()>zMn) sensors[nsens++] = sensN;
  //
  sensN = GetSensor(sens->GetNeighborID(AliITSURecoSens::kNghbBR)); // neighbor on the bottom right (smaller Z, smaller phi)
  if (sensN && OKforPhiMin(phiMn,sensN->GetPhiMax()) && sensN->GetZMax()>zMn) sensors[nsens++] = sensN;
  //
  if (mcLab>=0) {
    Int_t layS,staS,sensS;
    printf("Found %d sensors for phi %.3f : %.3f | Z %.4f %.4f\n", nsens,phiMn,phiMx,zMn,zMx); 
    for (int is=0;is<nsens;is++) {
      fITSGeom->GetChipId(sensors[is]->GetID()+fITSGeom->GetFirstModIndex(fActiveID),layS,staS,sensS);
      printf("*SNF#%d: %d %d %d | ",is,layS,staS,sensS); sensors[is]->Print();
    }
    for (int ism=0;ism<nFnd;ism++) {
      AliITSURecoSens* snMC = GetSensorFromID(fndSens[ism]);
      Bool_t ok=kFALSE;
      for (int isf=0;isf<nsens;isf++) {
	if (snMC==sensors[isf]) {ok=kTRUE;break;}
      }
      if (!ok) printf("MissedSens %d!!!\n",ism);
    }
  }
  return nsens;
}
*/
//______________________________________________________
void AliITSURecoLayer::ProcessClusters(Int_t mode)
{
  // register in each sensor of the layer its cluster.
  // the clusters of the layer must be sorted per sensor
  int ncl = fClusters->GetEntriesFast();
  int curSensID = -1;
  for (int i=fNSensors;i--;) GetSensor(i)->SetNClusters(0);
  AliITSURecoSens* curSens = 0;
  for (int icl=0;icl<ncl;icl++) {
    AliITSUClusterPix* cl = (AliITSUClusterPix*) fClusters->UncheckedAt(icl);
    cl->GoToFrameTrk();
    int vID = cl->GetVolumeId();
    if (vID<curSensID) {AliFatal("Clusters are not sorted in increasing sensorID");}
    if (vID>curSensID) {
      if (curSens) curSens->ProcessClusters(mode);    // prepare clusters for reconstruction
      curSens   = GetSensor(vID - fITSGeom->GetFirstChipIndex(fActiveID));
      curSensID = vID;
      curSens->SetFirstClusterId(icl);
    }
    curSens->IncNClusters();
  }
  if (curSens) curSens->ProcessClusters(mode); // last sensor was not processed yet
  //
}

//______________________________________________________
Bool_t AliITSURecoLayer::IsEqual(const TObject* obj) const
{
  // check if layers are equal in R
  const AliITSURecoLayer* lr = (const AliITSURecoLayer*)obj;
  return Abs(lr->GetR()-GetR())<1e-6 ? kTRUE : kFALSE;
}

//______________________________________________________
Int_t  AliITSURecoLayer::Compare(const TObject* obj) const
{
  // compare two layers
  const AliITSURecoLayer* lr = (const AliITSURecoLayer*)obj;
  double dr = GetR() - lr->GetR();
  if (Abs(dr)<1e-6) return 0;
  return dr>0 ? 1:-1;
  //      
}

//_________________________________________________________________
AliITSURecoSens* AliITSURecoLayer::GetSensorFromID(Int_t i) const 
{
  // get sensor from its global id
  i -= fITSGeom->GetFirstChipIndex(fActiveID);
  if (i<0||i>=fNSensors) AliFatal(Form("Sensor with id=%d is not in layer %d",i+fITSGeom->GetFirstChipIndex(fActiveID),fActiveID));
  return GetSensor(SensVIDtoMatrixID(i));
}