/*
 * PndTrackCollection.cxx
 *
 *  Created on: Jul 28, 2016
 *      Author: kibellus
 */

#include <PndTrackCollection.h>

PndTrackCollection::PndTrackCollection() {
  fLines = new vector<PndLineApproximation>();
}

PndTrackCollection::~PndTrackCollection() {
}

void PndTrackCollection::add(PndLineApproximation l,Bool_t skewed){
  if(l.getLine().getRating()==-1)return;
  //copy hits
  vector<PndFtsHit*> newHits;
  vector<PndFtsHit*> oldHits = l.getHits();
  for(int i=0;i<oldHits.size();i++){
    newHits.push_back(copyHit(oldHits[i]));
  }
  l.setHits(newHits);
  //add new Aprrox
  fLines->push_back(l);
  //refit
  if(fLines->size()==1)fCurrLine=(*fLines)[0].getLine();
  if(fLines->size()>1 && skewed){
    refitAllHits();
  } else if(fLines->size()>1){
    vector<PndFtsHit*> hits = l.getHits();
    for(int i=0;i<hits.size();i++){
      refitHit(fCurrLine,hits[i]);
    }
  }
  refitAllTracks();
}

PndFtsHit* PndTrackCollection::copyHit(PndFtsHit *h){
  PndFtsHit* h2 = new PndFtsHit();
  h2->SetDetectorID(h->GetDetectorID());
  h2->SetTubeID(h->GetTubeID());
  h2->SetChamberID(h->GetChamberID());
  h2->SetLayerID(h->GetLayerID());
  h2->SetXYZ(h->GetX(),h->GetY(),h->GetZ());
  h2->SetIsochrone(h->GetIsochrone());
  h2->SetIsochroneError(h->GetIsochroneError());
  h2->SetRefIndex(h->GetRefIndex());
  h2->SetEntryNr(h->GetEntryNr());
  return h2;
}

void PndTrackCollection::refitAllHits(){
  //get all hits
  vector<PndFtsHit*> allHits;
  for(int i=0;i<fLines->size();i++){
    vector<PndFtsHit*> hits = (*fLines)[i].getHits();
    for(int j=0;j<hits.size();j++) allHits.push_back(hits[j]);
  }
  //refit with line
  PndLine zyLine = (*fLines)[0].linearRegressionZY(allHits);
  //set the z pos
  for(int i=0;i<allHits.size();i++){
    refitHit(zyLine,allHits[i]);
  }
  fCurrLine = zyLine;
}

void PndTrackCollection::refitAllTracks(){
  for(int i=0;i<fLines->size();i++){
    PndLineApproximation a = (*fLines)[i];
    PndPlane p(a.getLine(),0);
    PndPlane p2(fCurrLine.getP1(),fCurrLine.getDir(),TVector3(1,0,0));
    PndLine newLine = p.getIntersection(p2);
    (*fLines)[i].setLine(newLine);
  }
}

void PndTrackCollection::refitHit(PndLine &l,PndFtsHit* hit){
  TVector3 base = l.getP1();
  TVector3 dir = l.getDir();
  Double_t lamp = (hit->GetZ()-base[2])/dir[2];
  TVector3 p = base+lamp*dir;
  hit->SetY(p[1]);
}

PndTrack PndTrackCollection::getPndTrack(map<Int_t,PndFtsHit*> orgHits){
  std::cout<<"PndTrackCollection::getPndTrack(): lines array size = "<<fLines->size()<<std::endl;
  //if(fLines->size() < 2)
  PndLine first = (*fLines)[0].getLine();
  PndLine last = (*fLines)[fLines->size()-1].getLine();
  TVector3 v(1,1,1);  // for "variances"
  TVector3 di(1,0,0); // for "detector plane"
  TVector3 dj(0,1,0); // for "detector plane"
  //this is not right to put a momentum of "3" and charge "+1", I'll fix that [r.k.'16]
  //FairTrackParP tp1(first.getP1(),3 * first.getDir().Unit(), v, v, 1, v, v, v);
  //FairTrackParP tp2(last.getP1(),3 * last.getDir().Unit(), v, v, 1, v, v, v);
  TVector3 mom1=first.getDir().Unit();
  TVector3 mom2=last.getDir().Unit();
  int q = scaleToMomentum(first.getP1(),mom1,last.getP1(),mom2);
  // If q==0 we implicitly have unaltered "momentum" vectors.
   std::cout<<"PndTrackCollection::getPndTrack(): after: m1("<<mom1.X()<<","<<mom1.Y()<<","<<mom1.Z()<<") m2("<<mom2.X()<<","<<mom2.Y()<<","<<mom2.Z()<<")  and q = "<<q<<std::endl;
  FairTrackParP tp1(first.getP1(),mom1, v, v, q, first.getP1(), di, dj);
  FairTrackParP tp2(last.getP1(),mom2, v, v, q, last.getP1(), di, dj);
  PndTrackCand  trackCand;
  Int_t hitCount;
  for(int i=0;i<fLines->size();i++){
    PndLineApproximation a = (*fLines)[i];
    vector<PndFtsHit*> hits = a.getHits();
    hitCount = hits.size();
    for(int j=0;j<hits.size();j++){
      trackCand.AddHit(orgHits[hits[j]->GetTubeID()]->GetEntryNr(),j);
    }
  }
  for(int i=0;i<fHits.size();i++)
    trackCand.AddHit(fHits[i]->GetEntryNr(),i+hitCount);
  return PndTrack(tp1,tp2,trackCand);
}

int PndTrackCollection::scaleToMomentum(TVector3 pos1, TVector3& mom1, TVector3 pos2, TVector3& mom2){
  // calculating momentum and scaling vectors mom1 and mom2 accordingly
  // returns integer 1 charge, 0 if errors occurred;
  // We approximate the field to a constant value in a defined region. We assume circular tracks
  std::cout<<"PndTrackCollection::scaleToMomentum(): positions: p1("<<pos1.X()<<","<<pos1.Y()<<","<<pos1.Z()<<") p2("<<pos2.X()<<","<<pos2.Y()<<","<<pos2.Z()<<")"<<std::endl;
  double fZBegin = 345.;//cm
  double fZEnd   = 620.;//cm
  // FIXME we want many div/zero protections!
  // FIXME Which coordinate zx or zy?
  // Firstly get the points at the magnetic field border (Half maximum of B strength)
  TVector2 x1(pos1.Z(),pos1.X());
  TVector2 x2(pos2.Z(),pos2.X());
  // We correct for
  double sign1=( mom1.Z() >= 0 ) ? 1. : -1. ;
  double sign2=( mom2.Z() >= 0 ) ? 1. : -1. ;
  TVector2 d1(sign1*mom1.Z(),sign1*mom1.X());
  TVector2 d2(sign2*mom2.Z(),sign2*mom2.X());
  std::cout<<"PndTrackCollection::scaleToMomentum(): before: m1("<<mom1.X()<<","<<mom1.Y()<<","<<mom1.Z()<<") m2("<<mom2.X()<<","<<mom2.Y()<<","<<mom2.Z()<<")"<<std::endl;
  double dphi=d1.DeltaPhi(d2);
  //int q = ( dphi > 0 ) ? 1 : -1 ; // FIXME right direction??
  int q = ( mom2.X() > mom1.X() ) ? 1 : -1 ;
  if (0.==mom1.Z()) return q;
  double t1 = (pos1.Z() - fZBegin) / mom1.Z();
  if (0.==mom2.Z()) return q;
  double t2 = (pos2.Z() - fZEnd) / mom2.Z();
  TVector2 T1=x1+t1*d1;
  TVector2 T2=x2+t2*d2;
  // Second is the intersection between the tangents
  if (0.==d2.X()) return q;
  double tmp1=d2.Y() - d1.Y()*d1.X()/d2.X();
  if (0.==tmp1) return q;
  double k2 = ( d1.Y()*(x2.X()-x1.X()) + x1.Y() - x2.Y() ) / tmp1;
  TVector2 A=x2+k2*d2;
  // Third we get an average AT distance as we don't trust the Begin/End positions
  double AT= 0.5*( (A-T1).Mod() + (A-T2).Mod() );
  double sph=sin(0.5*dphi);
  if (0.==sph) return q;
  double R=AT*sqrt(1+1/(sph*sph));
  // Fourth is getting the momentum magnitude
  double B=1.;
  //Double_t po[3], BB[3]; //po[0] = hitXLabSys; //po[1] = hitYLabSys; //po[2] = hitZLabSys; //fField->GetFieldValue(po, BB); //return value in KG (G3) //By = BB[1] / 10.; // By is y-component of magnetic field in Tesla
  double c=0.00299792458; //B[T], R[cm] and p[GeV/c]
  double pt=B*c*R;
  double tmpm1=mom1.X()*mom1.X()+mom1.Z()*mom1.Z();
  if (0.==tmpm1) return q;
  double scale1=pt/sqrt(tmpm1);
  double tmpm2=mom2.X()*mom2.X()+mom2.Z()*mom2.Z();
  if (0.==tmpm2) return q;
  double scale2=pt/sqrt(tmpm2);
  // We're fine now. Let's put the output.
  mom1=sign1*scale1*mom1;
  mom2=sign2*scale2*mom2;
  std::cout<<"PndTrackCollection::scaleToMomentum(): dphi="<<dphi<<"  R="<<R<<"  pt="<< pt << "  scale1=" <<scale1 << "  scale2=" << scale2 <<std::endl;
  std::cout<<"PndTrackCollection::scaleToMomentum(): after: m1("<<mom1.X()<<","<<mom1.Y()<<","<<mom1.Z()<<") m2("<<mom2.X()<<","<<mom2.Y()<<","<<mom2.Z()<<")  and q = "<<q<<std::endl;
  return q;
}

Double_t PndTrackCollection::getDistTo(PndLine l,Int_t layer){
  Double_t dist = 9999999999;
  for(int i=0;i<fLines->size();i++){
    PndLineApproximation appr = (*fLines)[i];
    Int_t apprLayer = appr.getHits()[0]->GetLayerID();
    if(layer<8 && apprLayer>=8)continue;
    if(layer>=16 && apprLayer<16)continue;
    if(layer>8 && layer < 16 && layer!=apprLayer)continue;
    PndLine l2 = appr.getLine();
    TVector3 p1 = l.getPerpendicular(l2);
    TVector3 p2 = l2.getPerpendicular(l);
    Double_t dist2 = (p1-p2).Mag();
    if(dist2<dist) dist = dist2;
  }
  return dist;
}