// ETrack.cpp

#include "ETrack.h"
#include "trfutil/trfstream.h"
#include "Surface.h"

using trf::TrackVector;
using trf::TrackError;
using trf::SurfacePtr;
using trf::VTrack;
using trf::ETrack;

//**********************************************************************

// default constructor

ETrack::ETrack() { }

//**********************************************************************

// constructor from surface alone or nothing
ETrack::ETrack(const SurfacePtr& psrf) 
: VTrack(psrf) { }

//**********************************************************************

// constructor from surface, vector and error
ETrack::ETrack(const SurfacePtr& psrf, const TrackVector& vec,
               const TrackError& err )
: VTrack(psrf,vec), _err(err) { }

//**********************************************************************

// constructor from surface, vector, error and direction
ETrack::ETrack(const SurfacePtr& psrf, const TrackVector& vec,
               const TrackError& err, TrackSurfaceDirection dir )
: VTrack(psrf,vec,dir), _err(err) { }

//**********************************************************************

// Constructor from a VTrack and an error matrix.
ETrack::ETrack(const VTrack& trv, const TrackError& err)
: VTrack(trv), _err(err) { }

//**********************************************************************

// copy constructor
ETrack::ETrack(const ETrack& tre)
: VTrack(tre), _err(tre._err) { }

//**********************************************************************

// destructor
ETrack::~ETrack() { }

//**********************************************************************

// assignment operator
ETrack& ETrack::operator=(const ETrack& tre) {
  if ( &tre == this ) return *this;
  ( (VTrack&) *this ) = tre;
  _err = tre._err;
  return *this;
}

//**********************************************************************

// output
// We use VTrack::ostr to display the surface and vector and then
// display the error matrix as the square root of the diagonal
// elements followed by correlation matrix.

void ETrack::ostr(ostream& stream) const {
  stream << begin_object;
  stream << "Etrack ";
  VTrack::ostr(stream,true);
  if ( is_valid() ) {
    //stream << new_line << _err;
    TrackVector derr;
    for ( int i=0; i<5; ++i ) {
      derr(i) = sqrt(_err(i,i));
    }
    stream << new_line << derr;
    TrackError nerr = _err;
    normalize(nerr);
    stream << new_line << nerr;
  }
  stream << end_object;
}

//**********************************************************************
// External functions.
//**********************************************************************

// output stream
ostream& operator<<( ostream& stream, const ETrack& rhs )
{
  rhs.ostr( stream );
  return stream;
}

//**********************************************************************

// equality
bool operator==(const ETrack& trv1, const ETrack& trv2) {
  if ( *trv1.get_surface() != *trv2.get_surface() ) return false;
  if ( trv1.get_vector() != trv2.get_vector() ) return false;
  if ( trv1.get_error() != trv2.get_error() ) return false;
  return true;
}

//**********************************************************************

// inequality
bool operator!=(const ETrack& trv1, const ETrack& trv2) {
  return ! ( trv1 == trv2 );
}

//**********************************************************************

// Get the difference between two track vectors with errors
// weighted by their combined error matrix.
double chisq_diff(const ETrack& trv1, const ETrack& trv2 ) {
  // If surfaces are not the same, return -1.0.
  if ( *trv1.get_surface() != *trv2.get_surface() ) return -1.0;
  TrackVector vecdif = 
    trv1.get_surface()->vec_diff( trv1.get_vector(), trv2.get_vector() );
  TrackError errsum = trv1.get_error() + trv2.get_error();
  if ( invert(errsum) ) return -2.0;
  return chisq_diff(vecdif,errsum);
}

//**********************************************************************

// Get the difference between track vectors with and without errors
// weighted by the error matrix.
double chisq_diff(const ETrack& trv1, const VTrack& trv2 ) {
  // If surfaces are not the same, return -1.0.
  if ( *trv1.get_surface() != *trv2.get_surface() ) return -1.0;
  TrackVector vecdif = 
    trv1.get_surface()->vec_diff( trv1.get_vector(), trv2.get_vector() );
  TrackError errsum = trv1.get_error();
  if ( invert(errsum) ) return -2.0;
  return chisq_diff(vecdif,errsum);
}

//**********************************************************************

// Get the difference between track vectors without and with errors
// weighted by the error matrix.
double chisq_diff(const VTrack& trv1, const ETrack& trv2 ) {
  return chisq_diff(trv2,trv1);
}

//**********************************************************************