/******************************************************
*             UNIT  Equa_dif.cpp                      *
* --------------------------------------------------- *
*  Integration procedure odeint1 used by program      *
*  planets.cpp                                        *
* --------------------------------------------------- *
* From a pascal version by Jean-Pierre Dumont, France *
*                                                     *
*                    C++ version by J-P Moreau, Paris.*
*                           (www.jpmoreau.fr)         *
******************************************************/
#include <stdio.h>
#include <math.h>


#define  Nvar_max   32          // Cf. Planets.cpp
#define  Ncorps     8
#define  DtSave     0.02


typedef double Vecteur_etat[Nvar_max+1]; 
typedef int    Pointers[Nvar_max];
typedef double Colonne[Nvar_max+1];

Pointers ipt;
Colonne  Masse,q1d,q2d,q1,q2;


double sqr(double x) {
  return x*x;
}

void Derivs (double t, double *XT, double *XF)  {
  double xi,xdi,xj,
         yi,ydi,yj,
         dxij,dyij,rij,
         xsm,ysm;
  int    i,ipti,iptj,j;

  for (i=1; i<=Ncorps; i++) {
	//Calculate intermediate scalar values
    ipti  = ipt[i];
    xdi   = XT[ipti+1];
    ydi   = XT[ipti+2];
    xi    = XT[ipti+3];
    yi    = XT[ipti+4];
	//Calculate second members
    XF[ipti+3] = xdi;
    XF[ipti+4] = ydi;
    xsm = 0;
    ysm = 0;
    if (i != 1) 
	  for (j=1; j<i; j++) {
        iptj = ipt[j];
        xj   = XT[iptj+3];
        yj   = XT[iptj+4];
        dxij = xj-xi;
        dyij = yj-yi;
        rij  = sqr(dxij)+sqr(dyij);
        rij  = Masse[j]/(rij*sqrt(rij));
        xsm  = xsm + dxij*rij;
        ysm  = ysm + dyij*rij;
      }
    if (i != Ncorps) 
	  for (j=i+1; j<=Ncorps; j++) {
        iptj = ipt[j];
        xj   = XT[iptj+3];
        yj   = XT[iptj+4];
        dxij = xj-xi;
        dyij = yj-yi;
        rij  = sqr(dxij)+sqr(dyij);
        rij  = Masse[j]/(rij*sqrt(rij));
        xsm  = xsm + dxij*rij;
        ysm  = ysm + dyij*rij;
      }
    XF[ipti+1] = xsm;
    XF[ipti+2] = ysm;
  } // i loop
} // Derivs()

//==========================================================================
void RK4(double *y, double *dydt, int n, double t, double h, double *yout) {
/*==========================================================================
| This procedure integrates the 1st order differential system:
|      dy/dt = F(y,t)
| by a fourth order Runge-Kutta method to advance the solution
| on interval h of the independant variable t:
 ------------------------------------------------------------------
| INPUTS:
| y     = State vector at begin of integration step
| dydt  = its derivative at the same point.
| n     = number of equations of system
| t     = i.s. value at begin of time step
| h     = time step
 -------------------------------------------------------------------
| OUTPUT:
| yout  = State vector at end of integration step.
 -------------------------------------------------------------------
| Programs using procedure RK4 must provide access to function:
|
| Derivs(double t, Vecteur_etat y, Vecteur_etat dydt);
| which returns the values of derivatives dydt at point t, knowing
| both t and the values of functions y.
|
| Also define the type:
|
| double Vecteur_etat[Nvar_max+1] in calling program, where Nvar_max 
| is the maximal number of equations of the system:  n <= Nvar_max.
 --------------------------------------------------------------------*/
   int    i;
   double th,hh,h6;
   Vecteur_etat dym,dyt,yt;

   hh = 0.5* h;
   h6 =h/6;
   th = t+ hh;
   for (i=1; i<=n; i++)  yt[i] = y[i] + hh * dydt[i];
   Derivs(th,yt,dyt);
   for (i=1; i<=n; i++)  yt[i] = y[i] + hh * dyt[i];
   Derivs(th,yt,dym);
   for (i=1; i<=n; i++) {
     yt[i] = y[i]+ h*dym[i];
     dym[i] = dyt[i] + dym[i];
   }
   Derivs(t+h,yt,dyt);
   for (i=1; i<=n; i++)  yout[i] = y[i]+h6*(dydt[i]+dyt[i]+2.0*dym[i]);
 } // RK4()

///////////////////////////////////////////////////////////////////////
void rkqc(double *y, double *dydt,
          int n, double *t,
          double htry, double eps,
          double *yscal,
          double *hdid, double *hnext)  {
/*====================================================================
| Runge-Kutta integration step with control of truncation local error 
| to obtain a required precision and adjust time step consequently
----------------------------------------------------------------------
| INPUTS:
| y      = State vector of size n
| dydt   = its derivative at begin value of independant variable, t
| n      = number of equations of system
| t      = begin value of independant variable
| htry   = time step proposed as a try
| eps    = precision requirement:
|          Max (ycalc[i] - yvrai[i])/yscal[i] < eps
| yscal  = normalization vector of solution.
----------------------------------------------------------------------
| OUTPUTS:
|  y     = end state vector
|  t     = i.s. end value
|  hdid  = actual time step  
|  hnext = time step advised for the next integration step
----------------------------------------------------------------------
| Programs using procedure RKQC must provide access to function:
|
| Derivs(double t, Vecteur_etat y, Vecteur_etat dydt)
| which returns the values of derivatives dydt at point t, knowing
| both t and the values of functions y.
|
| Also define the type:
|
| double Vecteur_etat[Nvar_max+1] in calling program, where Nvar_max 
| is the maximal number of equations of the system:  n <= Nvar_max.
=====================================================================*/
// LABEL: e1

double pgrow=-0.20,
       pshrnk=-0.25,
       fcor=0.06666666,   // 1/15
       un = 1.0,
       safety=0.9,
       errcon=6e-4,
       tiny= 1e-20;    

int    i;
double tsav,hh,h,temp,errmax;

Vecteur_etat  dysav,ysav,ytemp;

       tsav = *t;        // Save begin time
       for ( i=1; i<=n; i++) {
         ysav[i] = y[i];
         dysav[i] = dydt[i];
       }
       h = htry;         // define increment for a try value
e1:    hh = 0.5*h;       // take 2 half time steps
       RK4(ysav,dysav,n,tsav,hh,ytemp);
       *t = tsav + hh;
       Derivs(*t,ytemp,dydt);
       RK4(ytemp,dydt,n,*t,hh,y);
       *t = tsav + h;
       if (*t == tsav) {
         printf(" Pause in RKQC procedure\n");
         printf(" Increment too small of independant variable\n");
       }
       RK4(ysav,dysav,n,tsav,h,ytemp);
       errmax = 0;   // Evaluate error
       temp =0;
       for (i=1; i<=n; i++) {
         ytemp[i] = y[i] - ytemp[i];   // ytemp = estimated error
         if (yscal[i]>tiny)  temp = fabs(ytemp[i]/yscal[i]);
         if (errmax < temp)  errmax = temp;
       } // i loop
       errmax = errmax/eps;    // real error / requirement
       if (errmax > un)  {     // Error too big, reduce h
         h = safety*h*exp(pshrnk*log(errmax));
         goto e1;  // start again
       }
       else {                  // the step has been a success!
         *hdid = h;             // Calculate next time step
         if (errmax > errcon)
           *hnext = safety*h*exp(pgrow*log(errmax));
         else
           *hnext = 4.0*h;
       }
       for (i=1; i<=n; i++)  y[i] += ytemp[i]*fcor;
} // rkqc()

///////////////////////////////////////////////////////////////////////
void odeint1(double *ystart, int nvar,
             double t1, double t2, double eps, double h1, double hmin,
             int *nok, int *nbad) {
/*=================================================================
|  This procedure integrates the 1st order differential system:
|       dy/dt = F(y,t)
|  where y and F are vectors of size nvar, between times
|  t1 and t2.
|
|  INPUTS:
|  ystart= begin coordinates vector and speeds
|  nvar  = number of equations
|  t1    = begin integration time
|  t2    = end integration time
|          t2 may be > or < t1
|  eps   = absolute required precision for solution
|  h1    = time increment proposed at beginning (try value)
|  hmin  = minimum time increment
|
|  OUTPUTS:
|  ystart= end coordinates vector and speeds
|  nok   = number of unchanged time steps
|  nbad  = number of modified time steps
====================================================================
| Programs using procedure RKQC must provide access to function:
|
| Derivs(double t, Vecteur_etat y, Vecteur_etat dydt)
| which returns the values of derivatives dydt at point t, knowing
| both t and the values of functions y.
|
| Also define the type:
|
| double Vecteur_etat[Nvar_max+1] in calling program, where Nvar_max 
| is the maximal number of equations of the system:  n <= Nvar_max.
|
| Also define:        double dtsave
|
| Initialize:         dtsave
/
===================================================================*/

int    maxstp = 10000;

double two = 2,
       zero = 0,
       tiny = 1e-20;  

int    nstp,i;
double tsav,t,hnext,hdid,h;

Vecteur_etat  yscal,y,dydt;

extern double dtsave;

     t = t1;
     if (t2 > t1) h = fabs(h1);
     else h = -fabs(h1);
     *nok  = 0;
     *nbad = 0;
     for (i=1; i<=nvar; i++)  y[i] = ystart[i];
     tsav = t - dtsave*two;
     for (nstp=1; nstp<=maxstp; nstp++) {
       Derivs(t,y,dydt);
       for (i=1; i<=nvar; i++)  yscal[i] = fabs(y[i])+fabs(dydt[i]*h); //+tiny
       if (((t+h-t2)*(t+h-t1)) > zero)  h = t2 - t;
       rkqc(y,dydt,nvar,&t,h,eps,yscal,&hdid,&hnext);
       if (hdid == h) *nok  = *nok + 1;
       else  *nbad = *nbad + 1;
       if (((t-t2)*(t2-t1)) >= zero) {
         for (i=1; i<=nvar; i++)  ystart[i] = y[i];
         return; 
       }
       if (fabs(hnext) < hmin) {
         printf(" Time step too small!\n");
         *nok =-1 ;  // error flag
         return;
       }
       h = hnext;
     } // nstp loop
	 printf(" Pause in ODEINT1 procedure: too many time steps!\n");
} // odeint1()

// end of file equa_dif.cpp