//-----------------------------------------------------------
// File and Version Information:
// $Id$
//
// Description:
//      Implementation of class PndTpcRiemannTrack
//      see PndTpcRiemannTrack.hh for details
//
// Environment:
//      Software developed for the PANDA Detector at FAIR.
//
// Author List:
//      Sebastian Neubert    TUM            (original author)
//
//
//-----------------------------------------------------------

// Panda Headers ----------------------

// This Class' Header ------------------
#include "PndTpcRiemannTrack.h"

// C/C++ Headers ----------------------
#include <iostream>
#include <iterator>
#include <assert.h>
#include <cmath>


// Collaborating Class Headers --------
#include "PndTpcRiemannHit.h"
#include "PndTpcCluster.h"
#include "TMatrixD.h"
#include "TVectorD.h"
#include "TGraph.h"
#include "TMath.h"
// Class Member definitions -----------


ClassImp(PndTpcRiemannTrack)

PndTpcRiemannTrack::PndTpcRiemannTrack()
  : _n(3),_av(3), _c(0),_m(0), _t(0), _isFitted(false), _nit(0), _doSort(true)
{}


void
PndTpcRiemannTrack::init(double x0_, double y0_, double R_, 
			 double dip, double z0){
  double x0=x0_/100;
  double y0=y0_/100;
  double R=R_/100;

  double R2=R*R;
  double lambda=R2-x0*x0-y0*y0;
  if(lambda==0)lambda=0.0001;
  double D=lambda/sqrt(4*R2+(1-lambda)*(1-lambda));
  double DC=-D/lambda;
  double C=DC-D;
  double A=-2*x0*DC;
  double B=-2*y0*DC;
  
  _n[0]=A;
  _n[1]=B;
  _n[2]=C;
  _c=D;
  _m=dip;
  _t=z0;
}

PndTpcRiemannHit*
PndTpcRiemannTrack::getHit(unsigned int i) const {
  std::list<PndTpcRiemannHit*>::const_iterator it=_hits.begin();
  std::advance(it, i);
  return *it;
}

PndTpcRiemannHit*
PndTpcRiemannTrack::getLastHit() const {
  std::list<PndTpcRiemannHit*>::const_iterator it=_hits.end();
  --it;
  return *it;
}



void
PndTpcRiemannTrack::addHit(PndTpcRiemannHit* hit){
  int nbefore=_hits.size();
  if(nbefore<2 || !_doSort){// first two hits hit
    _hits.push_back(hit);
    return;
  }
  // check if we start at end or at beginning:
  TVector3 posX=hit->cluster()->pos();
  std::list<PndTpcRiemannHit*>::iterator it2=_hits.end();
  --it2;
  TVector3 posend=(*it2)->cluster()->pos();
  std::list<PndTpcRiemannHit*>::iterator it1=_hits.begin();
  TVector3 posstart=(*it1)->cluster()->pos();
  double s1x=(posX-posstart).Mag();
  double s2x=(posX-posend).Mag();
  _nit=0; // reset iteration counter
  if(s1x>=s2x){// hit closer to end of list
    this->sortHit(hit,it2,_hits,-1); // direction -1 --> start backwards search
  }
  else { // hit closer to start of list
    this->sortHit(hit,it1,_hits,1); // direction 1 --> search from front to end
  }
  
  // update average
  _av*=(double)nbefore;
  _av[0]+=hit->x().X();
  _av[1]+=hit->x().Y();
  _av[2]+=hit->x().Z();
  _av*=1./(double)(nbefore+1);
}




void
PndTpcRiemannTrack::sortHit(PndTpcRiemannHit* hitX, 
			    hitIt& it2, hitList& hL, int dir){
  if(it2==hL.begin() || it2==hL.end()){
    // insert hit before it2 when we have reached one end of the list
    hL.insert(it2,hitX); // list::insert(pos,x) inserts IN FRONT OF pos!
    std::cout<<"inserting hit at start/end"<<std::endl;
    hitX->cluster()->pos().Print();
    return;
  }
  if(_nit>hL.size()+1){
    std::cout<<"Too many iterations! "<<_nit<<"/"<<hL.size()<<std::endl;
    return;
  }
  ++_nit;
  
  hitIt it1=it2;
  --it1;
  // always it1<it2
  
  TVector3 pos1=(*it1)->cluster()->pos(); //next point
  TVector3 pos2=(*it2)->cluster()->pos();
  TVector3 posX=hitX->cluster()->pos();
  // project the distances onto 12
  //TVector3 dir=pos2-pos1;
  //dir.SetMag(1);

  double s12=(pos2-pos1).Mag();
  double gamma=1; // scale factor 0.5<gamma<=1
  double cut12=s12*gamma;

  double s1x=(posX-pos1).Mag();
  double s2x=(posX-pos2).Mag();

  // most simple if we ly between the two points:
  if(s1x < cut12 && s2x < cut12){
    // insert third hit in between
    hL.insert(it2,hitX);
    return;
  }

  if(hL.size()==2 || (it2==--(hL.end())) ){
    if(s1x>=s2x){// closer to s2 --> add at end
      hL.push_back(hitX);
      return;
    }
    else {// closer to s1 --> add at start
      hL.insert(it1,hitX);
      return;
    }
  }

  //////// if we get here we have more than 2 hits in track 
  //////// so we can use three points:

  hitIt it3=it2;
  ++it3;
  TVector3 pos3=(*it3)->cluster()->pos();
  double s23=(pos3-pos2).Mag();
  double s3x=(posX-pos3).Mag();
  double cut23=s23*gamma;

  // check if we ly inside second interval
  if(s2x < cut23 && s3x < cut23){
    // insert third hit in between
    hL.insert(it2,hitX);
    return;
  }


  // if we come here X lies neither in between 12 nor between 23
  // check if it lies in 13:

  double s13=(pos3-pos1).Mag();
  double cut13=s13*gamma;
  if(s1x < cut13 && s3x < cut13){
    // insert third hit in between
    // we have two possibilities:
    if(s1x<s3x)hL.insert(it2,hitX); // insert between 12
    else hL.insert(it3,hitX); // insert between 13
    return;
  }

  // now we are left with the alternative that the hit lies outside 13
  // depending on our choice of the original search direction 
  // there is only one way to proceed now:

  // dir>0 --> we have to increase it2
  if(dir>0){
    ++it2;
    sortHit(hitX,it2,hL,dir);
  }
  else {
    --it2;
    sortHit(hitX,it2,hL,dir);
  }
  return;
}

//double
//PndTpcRiemannTrack::s_hit(PndTpcRiemannHit* hit){
//  hitIt it=_hits.begin();
//  double s=0;
//  while()
//
//
//}

double
PndTpcRiemannTrack::dist(PndTpcRiemannHit* hit){
  double d2=_c;
  d2+=hit->x().X()*_n[0];
  d2+=hit->x().Y()*_n[1];
  d2+=hit->x().Z()*_n[2];
  return d2;
}


void
PndTpcRiemannTrack::refit()
{
  
  TMatrixT<double> av(3,1);
  av[0][0]=_av[0];
  av[1][0]=_av[1];
  av[2][0]=_av[2];

  TMatrixD sampleCov(3,3);
  
  std::list<PndTpcRiemannHit*>::iterator it=_hits.begin();
  while(it!=_hits.end()){
    TMatrixD h(3,1);
    h[0][0]=(*it)->x().X();
    h[1][0]=(*it)->x().Y();
    h[2][0]=(*it)->x().Z();
    TMatrixD d(3,1);
    d=h-av;
    TMatrixD dt(TMatrixD::kTransposed,d);
    TMatrixD ddt(d,TMatrixD::kMult,dt);
    sampleCov+=ddt;  
    ++it;
  }
  double nh=_hits.size();
  sampleCov*=1./nh;
  
  TVectorD eigenValues(3);
  TMatrixD eigenVec=sampleCov.EigenVectors(eigenValues);
    
  // find smallest eigenvalue
  double val=eigenValues[0]; int g=0;
  for(int k=1;k<3;++k){
    if(eigenValues[k]<val){
      val=eigenValues[k];
      g=k;
    }
  }
  _n=TMatrixDColumn(eigenVec,g);
 
  double norm=1./TMath::Sqrt(_n.Norm2Sqr());
  _n*=norm;
  _c=-1.*_n*_av;

}


TVectorD
PndTpcRiemannTrack::orig() const {
  TVectorD o(2);
  double den=0.5/(_c+_n[2]);
  o[0]=-_n[0]*den;
  o[1]=-_n[1]*den;
  return o;
}


double 
PndTpcRiemannTrack::r() const {
  if(_c==0)return 0;
  if(fabs(_c)>1000)return 0;
  double a=2.*(_c+_n[2]);
  //if(a==0){
  //  std::cout<<"PndTpcRiemannTrack:: a==0 cannot calc r! set r=1E4"<<std::endl;
  //  return 1.E-4;
  //}
  //std::cout<<_n[0]<<"   "<<_n[1]<<"   "<<_c<<"   "<<a<<std::endl;
  double nom=_n[0]*_n[0]+_n[1]*_n[1]-2.0*_c*a;
  if(nom<0.0){
    nom=1.E-4;
    std::cout<<"PndTpcRiemannTrack::nom<0! set r=1E-4!"<<std::endl;
  }
  return sqrt(nom)/TMath::Abs(a);
}

void
PndTpcRiemannTrack::szFit(){
  std::list<PndTpcRiemannHit*>::iterator it=_hits.begin();
  if(!_isFitted){
    TGraph g(4);
    for(unsigned int i=0;i<4;++i){
    (*it)->calcPosOnTrk(this,false);
    g.SetPoint(i,(*it)->s(),(*it)->z());
    //std::cout<<_hits[i]->z()<<"|"<<_hits[i]->s()<<std::endl;
    ++it;
    }
    int errorcode;
    g.LeastSquareLinearFit(4,_t,_m,errorcode,-999,999);
    _isFitted=true;
  }
  
  TGraph g(getNumHits());
  // get s'es and zs
  it=_hits.begin();
  unsigned int n=getNumHits();
  for(unsigned int i=0;i<n;++i){
    (*it)->calcPosOnTrk(this,false);
    g.SetPoint(i,(*it)->s(),(*it)->z());
    ++it;
  }
  int errorcode;
  g.LeastSquareLinearFit(n,_t,_m,errorcode,-999,999);
  //std::cout<<"szFit Error Code:"<<errorcode<<std::endl;
  return;
}

double
PndTpcRiemannTrack::szDist(PndTpcRiemannHit* hit, bool calcPos){
  szFit();
  if(calcPos)hit->calcPosOnTrk(this);
  double hits=hit->s();
  double predz=hits*_m+_t;
  return predz-hit->z();
}

// only after szFit!
double
PndTpcRiemannTrack::dip() const {
  return cos(atan(_m));
}


// Todo: check for backward going tracks!!!
double
PndTpcRiemannTrack::sign() const {
  double s=getHit(3)->s(); 
  return s<0 ? -1. : 1;
}