//-----------------------------------------------------------
// File and Version Information:
// $Id$
//
// Description:
//      Implementation of class Nystrom
//      see Nystrom.hh for details
//
// Environment:
//      Software developed for the PANDA Detector at FAIR.
//
// Author List:
//      Sebastian Neubert    TUM            (original author)
//
//
//-----------------------------------------------------------

// Panda Headers ----------------------

// This Class' Header ------------------
#include "Nystrom.h"

// C/C++ Headers ----------------------
#include <iostream>
#include "assert.h"

// Collaborating Class Headers --------
#include "AbsNystromEQM.h"

// Class Member definitions -----------


Nystrom::Nystrom(AbsNystromEQM* eqm)
  : _eqm(eqm)
{}

void
Nystrom::step(const TVectorT<double>& u, const TVectorT<double>& uprime, 
	      const TVectorT<double>& par,
	      TVectorT<double>& newu, TVectorT<double>& newuprime,
	      double h)
{
  assert(h!=0);
  double sign= h > 0 ? 1. : -1.;
  double habs=sign*h;
  double hh=habs*0.5;
  double h2=h*h;
  TVectorT<double> k1=_eqm->eval(u,uprime,par);
  TVectorT<double> k2=_eqm->eval(u+hh*uprime+h2/8.*k1,uprime+hh*k1,par);
  TVectorT<double> k3=_eqm->eval(u+hh*uprime+h2/8.*k2,uprime+hh*k2,par);
  TVectorT<double> k4=_eqm->eval(u+habs*uprime+h2/2.*k3,uprime+habs*k3,par); 
  
  TVectorT<double> ksum=k1+k2+k3;
  newu=u+h*uprime+h2/6.*ksum;
  newuprime=uprime+(h/6.)*(ksum+k4+k2+k3);

}

void 
Nystrom::propagate(double start, double end,
		   const TVectorT<double>& u, 
		   const TVectorT<double>& uprime,
		   const TVectorT<double>& par,
		   TVectorT<double>& newu, TVectorT<double>& newuprime)
{
  if(fabs(start-end)<1E-8){
    newu=u;
    newuprime=uprime;
  }

  double sign= start < end ? 1. : -1.;
  double h=1.e-2; // TODO: This should be more dynamic!
  double s=start;
  TVectorT<double> utmp(u);
  TVectorT<double> uprimetmp(uprime);
  unsigned int steps=0;
  while(fabs(s-end)>=h/2. && steps<10000){
    ++steps;
    step(utmp,uprimetmp,par,
	 newu,newuprime,sign*h);
    s+=sign*h;
    utmp=newu;
    uprimetmp=newuprime;
  }
  if(steps>10000)std::cerr<<"Nystrom:: too many steps (>10000) aborting."<<std::endl;
  // do the rest step
  h=end-s;
  if(h>1E-8){
    step(utmp,uprimetmp,par,
	 newu,newuprime,h);
  }
  
}