// Propagator.cpp

#include "Propagator.h"
#include <iostream>
#include "PropStat.h"
#include "TrackVector.h"
#include "ETrack.h"

using std::cerr;
using std::endl;

using trf::TrackError;
using trf::TrackDerivative;
using trf::ETrack;
using trf::PropStat;
using trf::Propagator;

//**********************************************************************
// Free functions.
//**********************************************************************

namespace {

// Surface creator.

ObjPtr create(const ObjData& data) { return ObjPtr(0); }

}

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

// constructor

Propagator::Propagator() { }

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

// destructor

Propagator::~Propagator() { }

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

// Return the creator.

ObjCreator Propagator::get_creator() { return create; }

namespace {
 
// Return the number of bad entries in the error matrix.

int check_error(const ETrack& tre) {
  const TrackError& err = tre.get_error();
  int nbad = 0;
  for ( int i=0; i<5; ++i ) {
    double eii = err(i,i);
    if ( eii <= 0.0 ) ++nbad;
    for ( int j=0; j<i; ++j ) {
      double ejj = err(j,j);
      double eij = err(j,i);
      if ( fabs(eij*eij) >= eii*ejj ) {
        ++nbad;
      }
    }
  }
  return nbad;
}

}

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

// Reduce a propagation direction.
// This drops the _MOVE from a direction and returns true
// if the suffix was present.  
// It is used in subclasses to separate the true direction from
// the move status in a direction.
bool Propagator::reduce_direction(PropDir& dir) {
  switch (dir) {
  case Propagator::NEAREST:
    return false;
  case Propagator::FORWARD:
    return false;
  case Propagator::BACKWARD:
    return false;
  case Propagator::NEAREST_MOVE:
    dir = NEAREST;
    return true;
  case Propagator::FORWARD_MOVE:
    dir = FORWARD;
    return true;
  case Propagator::BACKWARD_MOVE:
    dir = BACKWARD;
    return true;
  default:
    cerr << "Propagator::reduce_direction: Unknown direction." << endl;
    abort();
  }
  return false;
}


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

// Default implementation of proagation with error.  We ask
// for the derivative matrix and use it to update the error
// matrix.

PropStat Propagator::err_prop(ETrack& tre0, const Surface& srf,
                              TrackDerivative* pder) const {
  ETrack tre = tre0;
  TrackDerivative deriv;
  TrackDerivative* pderiv = &deriv;
  if ( pder != 0 ) pderiv = pder;
  PropStat pstat = vec_prop(tre,srf,pderiv);
  if ( ! pstat.success() ) return pstat;
  TrackError err = tre.get_error();
  tre.set_error( err.Xform(*pderiv) );
  if ( check_error(tre) != 0 ) {
    return PropStat();
  }
  tre0 = tre;
  return pstat;
}

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

// Same as above with direction specified.

PropStat
Propagator::err_dir_prop(ETrack& tre0, const Surface& srf, PropDir dir,
                         TrackDerivative* pder) const {
  ETrack tre = tre0;
  TrackDerivative deriv;
  TrackDerivative* pderiv = &deriv;
  if ( pder != 0 ) pderiv = pder;
  PropStat pstat = vec_dir_prop(tre,srf,dir,pderiv);
  if ( ! pstat.success() ) return pstat;
  TrackError err = tre.get_error();
  tre.set_error( err.Xform(*pderiv) );
  if ( check_error(tre) != 0 ) {
    return PropStat();
  }
  tre0 = tre;
  return pstat;
}

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