//*****************************************************
// 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
//*****************************************************

#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) = tang(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
  //   itrack(4) = C = 1/tan(theta)
  //   eitrack(5,5) = error matrix

  //
  // 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 =  etrack(0) * sin(phi_pos);
  double yp = -etrack(0) * cos(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 = (1./etrack(3));
  double x0 = xp - r*sin(phi_pos);
  double y0 = yp + r*cos(phi_pos);
  double z0 = zp - r*c*phi_pos;

  itrack(0) = x0;
  itrack(1) = y0;
  itrack(2) = z0;
  itrack(3) = r;
  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) = sphi;
  xjac(1,0) = 0.0;
  xjac(2,0) = (eps - 1/k) * cphi;
  xjac(3,0) = 0.0;
  xjac(4,0) = sphi/kk;

  xjac(0,1) = -cphi;
  xjac(1,1) = 0.0;
  xjac(2,1) = (eps - 1/k) * sphi;
  xjac(3,1) = 0.0;
  xjac(4,1) = -cphi/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() {}

}