// HitVector.cpp

#include "HitVector.h"
#include "TrackVector.h"

using std::ostream;

using trf::TrfMatrix;
using trf::HitVector;
using trf::HitError;
using trf::HitDerivative;
using trf::TrackError;

//**********************************************************************
// HitVector
//**********************************************************************

// constructor from implementation
HitVector::HitVector( const TrfVector& hvec ) : _vec(hvec) { }

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

// constructor
HitVector::HitVector( int size ) : _vec(size) {
  _vec.fill(0.0);
}

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

// constructor from an array
HitVector::HitVector( int size, double* arr ) : _vec(size) {
  for (int i=0; i<size; ++i) _vec(i) = arr[i];
}

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

// constructor from one value
HitVector::HitVector( double x1 ) : _vec(1) {
  _vec(0) = x1;
}

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

// constructor from two values
HitVector::HitVector( double x1, double x2 ) : _vec(2) {
  _vec(0) = x1;
  _vec(1) = x2;
}

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

// constructor from three values
HitVector::HitVector( double x1, double x2, double x3 ) : _vec(3) {
  _vec(0) = x1;
  _vec(1) = x2;
  _vec(2) = x3;
}

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

// copy constructor
HitVector::HitVector( const HitVector& hvec ) : _vec(hvec._vec) { }

//**********************************************************************
// HitError
//**********************************************************************

// constructor from implementation
HitError::HitError( const TrfSMatrix& herr ) : _err(herr) { }

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

// constructor from a track error and a track derivative:
// E_hit = dhit_dtrack * E_track * dhit_dtrack_transpose

HitError::HitError(const HitDerivative& dhit_dtrack, 
                   const TrackError& trkerr)
  : _err(dhit_dtrack.get_matrix().nrow())
  {
    _err = trkerr.xform(dhit_dtrack.get_matrix());
  }

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

// constructor
HitError::HitError( int size ) : _err(size) {
  _err.fill(0.0);
}

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

// constructor from an array
// order is lower triangle (00, 10, 11, 20, ...)
HitError::HitError( int size, double* arr ) : _err(size) {
  int ij = 0;
  for (int i=0; i<size; ++i) 
    for (int j=0; j<=i; ++j) 
      _err(i,j) = arr[ij++];
}

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

// constructor for 1D from values

HitError::HitError( double e11 ) : _err(1) {
  _err(0,0) = e11;
}

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

// constructor for 2D from values

HitError::HitError( double e11, double e21, double e22 ) : _err(2) {
  _err(0,0) = e11;
  _err(1,0) = e21;
  _err(1,1) = e22;
}

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

// constructor for 3D from values

HitError::HitError( double e11, double e21, double e22,
                    double e31, double e32, double e33 ) : _err(3) {
  _err(0,0) = e11;
  _err(1,0) = e21;
  _err(1,1) = e22;
  _err(2,0) = e31;
  _err(2,1) = e32;
  _err(2,2) = e33;
}

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

// copy constructor

HitError::HitError( const HitError& herr ) : _err(herr._err) { }

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

// invert
int HitError::invert() {
#ifdef TRF_USING_static_la
  return static_la::invert(_err);
#else
  return ::invert(_err);
#endif
}

//**********************************************************************
// HitDerivative
//**********************************************************************

// constructor from implementation
HitDerivative::HitDerivative(const TrfMatrix& hder)
: _mtx(hder) { }

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

// constructor
HitDerivative::HitDerivative( int size ) : _mtx(size,5) {
  _mtx.fill(0.0);
}

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

// constructor from an array
// order is by completed rows (00, 01, 02, ..., 10, 12, ...)
HitDerivative::HitDerivative( int size, double* arr ) : _mtx(size,5) {
  int ij = 0;
  for (int i=0; i<size; ++i)
    for (int j=0; j<5; ++j)
      _mtx(i,j) = arr[ij++];
}

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

// copy constructor
HitDerivative::HitDerivative( const HitDerivative& hder )
: _mtx(hder._mtx) { }

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

//**********************************************************************
// Friends.
//**********************************************************************

// vector output stream
ostream& trf::operator<<(ostream& stream, const HitVector& rhs) {
  return stream << rhs._vec;
}

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

// vector addition
HitVector trf::operator+(const HitVector& lhs, const HitVector& rhs) {
  return HitVector(lhs._vec + rhs._vec);
}

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

// vector subtraction
HitVector trf::operator-(const HitVector& lhs, const HitVector& rhs) {
  return HitVector(lhs._vec - rhs._vec);
}

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

// vector equality
bool trf::operator==(const HitVector& lhs, const HitVector& rhs) {
  if ( lhs.size() != rhs.size() ) return false;
  return lhs._vec == rhs._vec;
}

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

// vector equality with tolerance
bool trf::is_equal(const HitVector& lhs, const HitVector& rhs) {
  if ( lhs.size() != rhs.size() ) return false;
  return is_equal(lhs._vec,rhs._vec);
}

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

// output stream
ostream& trf::operator<<( ostream& stream, const HitError& rhs ) {
  return stream << rhs._err;
}

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

// output stream
ostream& trf::operator<<( ostream& stream, const HitDerivative& rhs ) {
  return stream << rhs._mtx;
}

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