#include "sopnamsp.h"
#include "machdefs.h"
#include "rk4cdifeq.h"
#include "ctimer.h"
#include <iostream>

inline double max_dbl(double a, double b) { return ((a < b) ? b : a); } 

//++
// Class	RK4CDiffEq
// Lib  	Outils++ 
// include	rk4cdifeq.h
//
//	Classe de résolution d'équadif par la méthode de
//	Runge-Kutta d'ordre 4 adaptatif.
//	Voir DiffEqSolver et R4KDiffEq pour les méthodes.
//
//	On peut demander une précision relative ou absolue
//	sur chacune des fonctions du système. Le pas d'intégration
//	est adapté automatiquement pour fournir au moins 
//	cette précision.
//--

//++
// Links	Parents
// RK4DiffEq
// DiffEqSolver
//--

//++
// Titre	Constructeurs
//	Voir RK4DiffEq et DiffEqSolver
//--

//++
RK4CDiffEq::RK4CDiffEq()
//
//--
: RK4DiffEq(), eps(1.e-4), relAccuracy(true), accScale(10), yTemp(10), ySave(10)
{}

//++
RK4CDiffEq::RK4CDiffEq(DiffEqFunction* f)
//
//--
: RK4DiffEq(f), eps(1.e-4), relAccuracy(true), accScale(f->NFuncReal()), yTemp(f->NFuncReal()), ySave(f->NFuncReal())
{}

//++
RK4CDiffEq::RK4CDiffEq(DIFEQFCN1 f)
//
//--
: RK4DiffEq(f), eps(1.e-4), relAccuracy(true), accScale(1), yTemp(1), ySave(1)
{}

//++
// Titre	Méthodes
//--

//++
// RK4CDiffEq& RK4CDiffEq::Accuracy(double eps)
//	Fixe la précision requise sur les fonctions. Cette précision
//	est par défaut relative. Elle peut être absolue, auquel cas
//	il faut fixer, pour chaque fonction, un facteur d'échelle, et
//	la précision sur chaque fonction est alors scale[i]*eps.
//--

RK4CDiffEq& 
RK4CDiffEq::Accuracy(double x)
{
  eps = x;
  return *this;
}

//++
RK4CDiffEq&  
RK4CDiffEq::AbsAccuracy(Vector const& vScal)
//
//	On souhaite une précision absolue, et le vecteur vScal contient
//	le facteur d'échelle pour chaque fonction (voir Accuracy).
//--
{
  accScale = vScal;
  relAccuracy = false;
  return *this;
}

//++
RK4CDiffEq&  
RK4CDiffEq::AbsAccuracy(double scal)
//
//	On souhaite une précision absolue, et le vecteur scal contient
//	le facteur d'échelle à appliquer à toutes les fonctions.
//	La précision absolue souhaitée est alors eps*scal.
//--
{
  for (int i=0; i<accScale.NElts(); i++)
    accScale(i) = scal;
  relAccuracy = false;
  return *this;
}

//++
RK4CDiffEq&  
RK4CDiffEq::RelAccuracy()
//
//	On souhaite une précision relative. En quelque sorte, le facteur d'échelle 
//	pour chaque fonction est alors la valeur de la fonction.
//--
{
  relAccuracy = true;
  return *this;
}

static const double pgrow   = -0.20;
static const double pshrink = -0.25;
static const double safety  = 0.9;
static const double errcon = pow((4/safety),1/pgrow);

void
RK4CDiffEq::RKCStep(Vector& newY, Vector const& y0, Vector const& yScale, 
	       double dttry, double& dtdone, double& dtnext)
{
  double err;
  ySave = y0;
  do {
    // Deux petits pas
    RKStep(yTemp, ySave, dttry/2.);
    RKStep(newY, yTemp, dttry/2.);
    
    // Un grand pas
    RKStep(yTemp, ySave, dttry);
    
    yTemp -= newY;   // l'erreur courante

    err = 0;
    for (int i=0; i<yTemp.NElts(); i++) 
      err = max_dbl(fabs(yScale(i) ? (yTemp(i)/yScale(i)) : yTemp(i)), err);
    
    err /= eps;
    if (err > 1) 
      dttry *= safety*pow(err,pshrink);
    else {
      dtdone = dttry;
      if (err > errcon)
	dtnext = safety*dttry*pow(err,pgrow);
      else
	dtnext = dttry*4;
    }
  }  while (err > 1);
  
  // Et on corrige a l'ordre 5

  newY += yTemp/15.;
}

void
RK4CDiffEq::SolveArr(Matrix& y, double* t, double tf, int n)
{
  //TIMEF;
  // Les intervalles a stocker dans la matrice des resultats

  double dxres = (tf - mXStart)/n;

  Vector yt(mYStart,false);
  
  k1.Realloc(mFunc->NFuncReal());
  k2.Realloc(mFunc->NFuncReal());
  k3.Realloc(mFunc->NFuncReal());
  k4.Realloc(mFunc->NFuncReal());
  yTemp.Realloc(mFunc->NFuncReal());
  ySave.Realloc(mFunc->NFuncReal());

  double x = mXStart;
  double step = mStep;
  double nstep;

  for (int i=0; i<n; i++) {
    double xEndStep = (i+1)*dxres + mXStart;
    do {
      if (relAccuracy) accScale = yt;
      if ((x+step-xEndStep)*(x+step-mXStart)>0) step = xEndStep-x;
      RKCStep(yt, yt, accScale, step, step, nstep);
      x += step;
      //cout << x << " " << step << endl;
      step = nstep;
    } while ((x-xEndStep)*(x-mXStart) < 0);
    for (int j=0; j<mFunc->NFunc(); j++)
      y(i,j) = yt(j);
    t[i] = xEndStep;
    //cout << "fin boucle "<<i<<"  "<<x<<"  "<<yt(0)<<endl;
  }
   
}