//-----------------------------------------------------------
// 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 <assert.h>

// 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)
{}

void
PndTpcRiemannTrack::addHit(PndTpcRiemannHit* hit){
  int nbefore=_hits.size();
  _hits.push_back(hit);
  _av*=(double)nbefore;
  _av[0]+=hit->x().X();
  _av[1]+=hit->x().Y();
  _av[2]+=hit->x().Z();
  _av*=1./(double)(nbefore+1);
}

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);
  
  int nh=_hits.size();
  for(int i=0;i<nh;++i){
    TMatrixD h(3,1);
    h[0][0]=_hits[i]->x().X();
    h[1][0]=_hits[i]->x().Y();
    h[2][0]=_hits[i]->x().Z();
    TMatrixD d(3,1);
    d=h-av;
    TMatrixD dt(TMatrixD::kTransposed,d);
    TMatrixD ddt(d,TMatrixD::kMult,dt);
    sampleCov+=ddt;  
  }

  sampleCov*=1./(double)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(_hits.size()<3)return 0;
  if(fabs(_c)>100)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(){
  TGraph g(getNumHits());
  // get s'es and zs
  unsigned int n=getNumHits();
  for(unsigned int i=0;i<n;++i){
    _hits[i]->calcPosOnTrk(this);
    g.SetPoint(i,_hits[i]->s(),_hits[i]->z());
    //std::cout<<_hits[i]->z()<<"|"<<_hits[i]->s()<<std::endl;
  }
  int errorcode;
  g.LeastSquareLinearFit(n,_t,_m,errorcode,-999,999);
  //std::cout<<"szFit Error Code:"<<errorcode<<std::endl;
  return;
}

double
PndTpcRiemannTrack::szDist(PndTpcRiemannHit* hit){
  szFit();
  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=_hits[3]->s(); 
  return s<0 ? -1. : 1;
}