//*****************************************************
// Class : TrackConversion.cpp
// 
// Author: M. Narain - F. Stichelbaut
//
// Date  : 16-APR-1998
//         20-APR-1998 : Use 5x5 error matrices.
//         23-APR-1998 : update conversion method.
//         24-APR-1998 : Use ColumnVector instead of array everywhere.
//         19-MAY-1998 : Use declareSymmetric for eitrack and eetrack
//         22-MAY-1998 : return phi_pos with ITrack
//         12-AUG-1998 : Conversion done in the constructor
//         22-NOV-1999 : Fixed bug in parametrization - SG/AS 
//*****************************************************

#include <iostream>
#include "LinearAlgebra/Matrix.h"
#include "LinearAlgebra/ColumnVector.h"
#include "trfutil/TRFMath.h"
#include "trfbase/ETrack.h"
#include "vertexutil/DebugOutput.hpp"  
#include "vertexutil/Vtrack.hpp"
#include "vertexutil/TrackConversion.hpp"

namespace vertex {

//----------------------------------------------------------
// Constructor
//----------------------------------------------------------

TrackConversion::TrackConversion(const ETrack& my_etrack, ColumnVector& itrack,
                                 Matrix& eitrack, double& phi_pos)
{
  if (DebugOutput::get_debug() >= 1){
    cout << " +++++ TrackConversion: constructor " << endl;
  }

  // Convert track parameters + err. matrix from External to Internal rep. 

  // External representation (perigee parameters):
  //   etrack(0) = r_signed = dca in rphi plane * sign of alpha
  //   etrack(1) = z = z coordinate of pca in rphi
  //   etrack(2) = phi_direction = phi_pos + alpha
  //   etrack(3) = tan(lambda) = cotg(theta)
  //   etrack(4) = q/pT = 1/R * 1/CONSB
  //   eetrack(5,5) = error matrix

  // Internal representation:
  //   itrack(0) = x0 } 
  //   itrack(1) = y0 } = center of circle in rphi plane
  //   itrack(2) = z0 }
  //   itrack(3) = R  // R>0 is clockwise !
  //   itrack(4) = C = cotg(theta)=tan(lambda)
  //   eitrack(5,5) = error matrix

  // ITrack is such that:
  // x = x0+R*sin(phi) 
  // y = y0-R*cos(phi) 
  // z = z0+R*cotg(theta)*phi

  //
  // Convert parameters
  //
  ColumnVector etrack(5);
  for (int i=0; i<5; i++)
  {
    etrack(i) = my_etrack.get_vector(i);
  }
  if (DebugOutput::get_debug() >= 3){
    cout << " +++ TrackConversion: etrack = " << etrack << endl;
  }
  // Compute phi_pos
  double sign = 0.0;
  if(etrack(0) != 0.0) {
    sign = etrack(0)/abs(etrack(0));
    phi_pos = etrack(2) - sign*PI2;
    phi_pos = fmod1(phi_pos,TWOPI);
  }
  double xp = abs(etrack(0)) * cos(phi_pos);
  double yp = abs(etrack(0)) * sin(phi_pos);
  double zp = etrack(1);
  double BMAG = 2.0;          // Magnetic field in Tesla
  double CONSB = BFAC * BMAG; // Conversion into GeV/cm (BFAC in TRFMath.h)
  double r = 1./etrack(4)/CONSB;
  double c = etrack(3);       // tan(lamb)=cotg(theta) w/ theta=pi/2-lamb

  double x0 = xp - r*abs(cos(phi_pos))*etrack(2)/abs(etrack(2));

  double phiy;
  if (etrack(2)<-PI2) { 
    phiy = -3*PI2-etrack(2); 
  } else {
    phiy = PI2-etrack(2);
  }
  double y0 = yp + r*abs(sin(phi_pos))*phiy/abs(phiy);
  double z0 = zp - r*c*(phi_pos-PI2);

  itrack(0) = x0;
  itrack(1) = y0;
  itrack(2) = z0;
  itrack(3) = -r; // R>0 is clockwise 
  itrack(4) = c;

  if (DebugOutput::get_debug() >= 3){
    cout << " +++ TrackConversion: itrack = " << itrack << endl;
  }
  //
  // Convert error matrix
  //

  double eps  = etrack(0);
  double k = 1./r;
  double kk =  pow(etrack(4),2)*CONSB;
  double sphi = sin(phi_pos);
  double cphi = cos(phi_pos);
  Matrix xjac(5,5);

  xjac(0,0) = cphi;
  xjac(1,0) = 0.0;
  xjac(2,0) = (1/k - eps) * sphi;
  xjac(3,0) = 0.0;
  xjac(4,0) = cphi/kk;

  xjac(0,1) = sphi;
  xjac(1,1) = 0.0;
  xjac(2,1) = (eps - 1/k) * cphi;
  xjac(3,1) = 0.0;
  xjac(4,1) = sphi/kk;

  xjac(0,2) = 0.0;
  xjac(1,2) = 1.0;
  xjac(2,2) = -c/k;
  xjac(3,2) = phi_pos * pow(c,2) / k;
  xjac(4,2) = c * phi_pos / kk;

  xjac(0,3) = 0.0;
  xjac(1,3) = 0.0;
  xjac(2,3) = 0.0;
  xjac(3,3) = 0.0;
  xjac(4,3) = -1./kk;

  xjac(0,4) = 0.0;
  xjac(1,4) = 0.0;
  xjac(2,4) = 0.0;
  xjac(3,4) = -pow(c,2);
  xjac(4,4) = 0.0;

  const TrackError tetrack = my_etrack.get_error();

  Matrix eetrack(5,5);
  eetrack.declareSymmetric();
  for( int j=0; j<5; j++) 
  {
    for( int k=j; k<5; k++) 
    {
      eetrack(j,k) = tetrack(j,k);
    }
  }

  eitrack.declareSymmetric();
  eitrack = xjac.transpose() * eetrack * xjac; 

  if (DebugOutput::get_debug() >= 3){
    cout << " +++ TrackConversion: eitrack = " << eitrack << endl;
  }

}

//----------------------------------------------------------
// Destructor
//----------------------------------------------------------

TrackConversion::~TrackConversion() {}

}