#define __DMETODY_N_C__

#include "dtypyn.h"
#include <alloc.h>
#include <math.h>

#include "dmetodyn.h"

/* Plik ten zawiera otwarte metody rozwiazywania     */
/* ukladu rownan rozniczkowych zwyczajnych           */
/* pierwszego rzedu                                  */

/* ======================================================================= */
/* ============================ Jednokrokowe ============================= */
/* ======================================================================= */


void Euler( real* res, int dim, p_dequation1 *f, real* x, real t, real h )
{
  int i;
  for(i = 0; i < dim; i++)
    res[i] = ( x[i] + h*(*f[i])(t,x) );
}

/* ---------------------------- Rungego Kutty ---------------------------- */

void Heun( real* res, int dim, p_dequation1 *f, real* x, real t, real h )
{
  int i;
  real *k1;
  k1 = (real *)calloc(dim,sizeof(real));

  for(i = 0;i < dim; i++)
    k1[i] = x[i] + h*(*f[i])(t,x);
  for(i = 0;i < dim; i++)
    res[i] = ( x[i] + 0.5*h*( (*f[i])(t,x) + (*f[i])(t+h,k1) ) );

  free(k1);
}

void ImprovedEuler( real* res, int dim, p_dequation1 *f, real* x, real t, real h )
{
  int i;
  real *k1;
  k1 = (real *)calloc(dim,sizeof(real));

  for(i = 0; i < dim; i++)
    k1[i] = x[i] + 0.5*h*(*f[i])(t,x);
  for(i = 0; i < dim; i++)
    res[i] = ( x[i] + h*(*f[i])(t+0.5*h,k1)  );

  free(k1);
}

void RungeKutta4_classic( real *res, int dim, p_dequation1 *f, real* x, real t, real h )
{
  int i;
  real *k1,*k2,*k3,*k4;
  real* tmp;

  k1  = (real *)calloc(dim,sizeof(real));
  k2  = (real *)calloc(dim,sizeof(real));
  k3  = (real *)calloc(dim,sizeof(real));
  k4  = (real *)calloc(dim,sizeof(real));
  tmp = (real *)calloc(dim,sizeof(real));

  for(i = 0;i < dim; i++)
    k1[i] = (*f[i])(t,x);
  for(i = 0;i < dim; i++)
    tmp[i] = x[i] + 0.5*h*k1[i];
  for(i = 0;i < dim; i++)
    k2[i] = (*f[i])(t + 0.5*h,tmp);
  for(i = 0;i < dim; i++)
    tmp[i] = x[i] + 0.5*h*k2[i];
  for(i = 0;i < dim; i++)
    k3[i] = (*f[i])(t + 0.5*h,tmp);
  for(i = 0;i < dim; i++)
    tmp[i] =  x[i] + h*k3[i];
  for(i = 0;i < dim; i++)
    k4[i] = (*f[i])(t + h    ,tmp);

  for(i = 0;i < dim; i++)
    res[i] = x[i] + h*(k1[i]+2*k2[i]+2*k3[i]+k4[i])/6.0;

  free(tmp);
  free(k4);
  free(k3);
  free(k2);
  free(k1);
}

void RungeKutta4_three_eighths( real* res, int dim, p_dequation1 *f, real* x, real t, real h )
{
  int i;
  real *k1,*k2,*k3,*k4;
  real *tmp;

  k1  = (real *)calloc(dim,sizeof(real));
  k2  = (real *)calloc(dim,sizeof(real));
  k3  = (real *)calloc(dim,sizeof(real));
  k4  = (real *)calloc(dim,sizeof(real));
  tmp = (real *)calloc(dim,sizeof(real));

  for(i = 0;i < dim; i++)
    k1[i] = (*f[i])(t,x);
  for(i = 0;i < dim; i++)
    tmp[i] = x[i] + h*k1[i]/3.0;
  for(i = 0;i < dim; i++)
    k2[i] = (*f[i])(t + h/3.0  ,tmp);
  for(i = 0;i < dim; i++)
    tmp[i] = x[i] - h*k1[i]/3.0 + h*k2[i];
  for(i = 0;i < dim; i++)
    k3[i] = (*f[i])(t + 2*h/3.0,tmp);
  for(i = 0;i < dim; i++)
    tmp[i] = x[i] + h*k1[i] - h*k2[i] + h*k3[i];
  for(i = 0;i < dim; i++)
    k4[i] = (*f[i])(t + h      ,tmp);

  for(i = 0;i < dim; i++)
    res[i] = ( x[i] + h*(k1[i]+3*k2[i]+3*k3[i]+k4[i])/8.0  );

  free(tmp);
  free(k4);
  free(k3);
  free(k2);
  free(k1);
}

void RungeKutta4_Ralston( real* res, int dim, p_dequation1 *f, real* x, real t, real h )
{
  int i;
  real *k1,*k2,*k3,*k4;
  real *tmp;

  k1  = (real *)calloc(dim,sizeof(real));
  k2  = (real *)calloc(dim,sizeof(real));
  k3  = (real *)calloc(dim,sizeof(real));
  k4  = (real *)calloc(dim,sizeof(real));
  tmp = (real *)calloc(dim,sizeof(real));

  for(i = 0;i < dim; i++)
    k1[i] = (*f[i])(t,x);
  for(i = 0;i < dim; i++)
    tmp[i] = x[i] + 0.4*h*k1[i];
  for(i = 0;i < dim; i++)
    k2[i] = (*f[i])(t +        0.4*h,tmp);
  for(i = 0;i < dim; i++)
    tmp[i] = x[i] + 0.29697760*h*k1[i]
                  + 0.15875966*h*k2[i];
  for(i = 0;i < dim; i++)
    k3[i] = (*f[i])(t + 0.45573726*h,tmp);
  for(i = 0;i < dim; i++)
    tmp[i] = x[i] + 0.21810038*h*k1[i]
                  - 3.05096470*h*k2[i]
                  + 3.83286432*h*k3[i];
  for(i = 0;i < dim; i++)
    k4[i] = (*f[i])(t +            h,tmp);
  for(i = 0;i < dim; i++)
    res[i] = ( x[i] + h*(0.17476028*k1[i]
		       - 0.55148053*k2[i]
		       + 1.20553547*k3[i]
		       + 0.17118478*k4[i]) );

  free(tmp);
  free(k4);
  free(k3);
  free(k2);
  free(k1);
}


void RungeKutta5_Scraton( real* res, int dim, p_dequation1 *f, real* x, real t, real h )
{
  int i;
  real *k1,*k2,*k3,*k4,*k5;
  /* long double E; */
  real *tmp;

  k1  = (real *)calloc(dim,sizeof(real));
  k2  = (real *)calloc(dim,sizeof(real));
  k3  = (real *)calloc(dim,sizeof(real));
  k4  = (real *)calloc(dim,sizeof(real));
  k5  = (real *)calloc(dim,sizeof(real));
  tmp = (real *)calloc(dim,sizeof(real));

  for(i = 0;i < dim; i++)
    k1[i] = h*(*f[i])(t,x);
  for(i = 0;i < dim; i++)
    tmp[i] = x[i] + 2.0*k1[i]/9.0;
  for(i = 0;i < dim; i++)
    k2[i] = h*(*f[i])(t + 2.0*h/9.0,tmp);
  for(i = 0;i < dim; i++)
    tmp[i] = x[i] + (k1[i] + 3*k2[i])/12.0;
  for(i = 0;i < dim; i++)
    k3[i] = h*(*f[i])(t + 1.0*h/3.0,tmp);
  for(i = 0;i < dim; i++)
    tmp[i] = x[i] + (3.0*23.0/128.0*k1[i] - 3.0*81.0/128.0*k2[i] + 3.0*90.0/128.0*k3[i])/128.0;
  for(i = 0;i < dim; i++)
    k4[i] = h*(*f[i])(t + 3.0*h/4.0,tmp);
  for(i = 0;i < dim; i++)
    tmp[i] = x[i] + (9.0*-345.0/10000.0*k1[i] + 9.0*2025.0/10000.0*k2[i] - 9.0*1224.0/10000.0*k3[i] + 9.0*544.0/10000.0*k4[i]);
  for(i = 0;i < dim; i++)
    k5[i] = h*(*f[i])(t + 9.0*h/10.0,tmp);
  for(i = 0;i < dim; i++)
    res[i] = x[i] + (1445.0/13770.0*k1[i] + 6561.0/13770.0*k3[i] + 3264.0/13770.0*k4[i] + 2500.0/13770.0*k5[i]);

  /* wzor modyfikujacy */

  /*

  for(i = 0;i < dim; i++)
    if(fabs(k4[i] - k1[i]) > ZERO) {

      E = (85.0*k1[i] - 243.0*k3[i] + 408.0*k4[i] -250.0*k5[i]) *
	  (95.0*k1[i] - 405.0*k2[i] + 342.0*k3[i] - 32.0*k4[i]) /
	  183600.0*(k4[i] - k1[i]);

      res[i] = res[i] - E;
    }

  */

  free(tmp);
  free(k5);
  free(k4);
  free(k3);
  free(k2);
  free(k1);
}

/* -------------------- Rungego Kutty o zmiennym kroku ------------------- */

v_info_t default_v_info =
  {
    1e-8,  /* wspolczynnik tolerancji bezwzglednej   */
    1e-8,  /* wspolczynnik tolerancji wzglednej      */
    0.8 ,  /* mnoznik (zabezpieczenie aby            **
	   **  zapewnic prog bezpieczenstwa dla err) */
    0.2 ,  /* minimalny mnoznik (h' = mnoznik*h)     */
    5.0 ,  /* maksymalny mnoznik (h' = mnoznik*h)    */
    1e-8,  /* minimalny dopuszczalny krok            */
    10.0   /* maksymalny dopuszczalny krok           */
  };

void set_max_error(v_info_t *dane_zm, real max_err)
{
  dane_zm->atol = max_err;
  dane_zm->rtol = max_err;

  dane_zm->fac = default_v_info.fac;
  dane_zm->fac_min = default_v_info.fac_min;
  dane_zm->fac_max = default_v_info.fac_max;
  dane_zm->h_min = default_v_info.h_min;
  dane_zm->h_max = default_v_info.h_max;
}


/* metoda Rungego-Kutty 4. rzedu ze zmiennym krokiem calkowania */
/* z uzyciem ekstrapolacji Richardsona do szacowania bledu      */
/* oraz poprawiania rzedu metody (z 4. na 5.)                   */
void RungeKutta4_5_classic( real *res, int dim, p_dequation1 *f,
			    real *x, real t, real *h, v_info_t *dane_zm )
{
  int koniec,oblicz_wynik;
  int i;
  double nor_bl=0.0,err,nor_y2=0.0;
  real *mid,*y1,*y2;
  double h2;

  mid = (real *)calloc(dim,sizeof(real));
  y1  = (real *)calloc(dim,sizeof(real));
  y2  = (real *)calloc(dim,sizeof(real));

  koniec = FALSE;
  oblicz_wynik = FALSE;

  do {
    h2=(*h)/2.0;
    /* dwa kroki z h' = h/2 */
    RungeKutta4_classic(mid,dim,f,x,t,h2);
    RungeKutta4_classic(y2,dim,f,mid,t+h2,h2);
    /* jeden krok z h' = h */
    RungeKutta4_classic(y1,dim,f,x,t,(*h));

    /* liczymy norme bledu */
    nor_bl=0.0; nor_y2=0.0;
    for(i=0;i<dim;i++) {
      nor_bl+=fabs(y1[i]-y2[i]);
      nor_y2+=fabs(y2[i]);
    }

    /* obliczamy znormalizowany blad */
    err=nor_bl/(8.0*(dane_zm->atol+dane_zm->rtol*nor_y2));

    if( oblicz_wynik || err <= 1.0 ) {

      /* liczymy wynik poprawiajac jednym krokiem ekstrapolacji Richardsona */
      for(i=0;i<dim;i++)
	res[i]=y2[i] + (y2[i]-y1[i])/15;

      if(err==0.0)
	(*h) = dane_zm->h_max;
      else if( !oblicz_wynik )
        /* nie weszlismy tutaj z powodu ograniczenia dolnego na h */
        /* z h = dane_zm->h_min                                   */
	(*h) = (*h)*min(dane_zm->fac/pow(err,0.2),dane_zm->fac_max);

      /* obliczylismy wynik wiec mozemy wyjsc */
      koniec = TRUE;
    }
    else
    {
      (*h)=min( (*h)*max(dane_zm->fac/pow(err,0.2),dane_zm->fac_min) , dane_zm->h_max);
      if( (*h) <= dane_zm->h_min)
      {
        /* dalej nie mozemy zmniejszac h wiec w nastepnej iteracji */
        /* musimy juz obliczyc wynik i wyjsc z petli               */
	(*h) = dane_zm->h_min;
        oblicz_wynik = TRUE;
      }
    }
  }while(!koniec);

  free(mid);
  free(y1);
  free(y2);
}


/* ======================================================================= */
/* ============================ Wielokrokowe ============================= */
/* ======================================================================= */

#define I(arr,i,j) arr[i*dim + j]

void Midpoint( real* res, int dim, real* f, real* x, real h )
{
  int i;
  for(i = 0;i < dim; i++)
    res[i] = ( I(x,1,i) + 2*h*I(f,0,i) );
}

/* ----------------------------------------------------------------------- */


void open_1step_1( real* res, int dim, real* f, real* x, real h )
{
  int i;
  for(i = 0;i < dim; i++)
    res[i] = ( I(x,0,i) + h*I(f,0,i) );
}

void open_2step_1( real* res, int dim, real* f, real* x, real h )
{
  int i;
  for(i = 0;i < dim; i++)
    res[i] = ( I(x,0,i) + h*( 3*I(f,0,i) + I(f,1,i) )/2.0 );
}

void open_3step_1( real* res, int dim, real* f, real* x,  real h )
{
  int i;
  for(i = 0;i < dim; i++)
    res[i] = ( I(x,0,i) + h*( 23*I(f,0,i) - 16*I(f,1,i) +  5*I(f,2,i) )/12.0 );
}


void open_4step_1( real* res, int dim, real* f, real* x, real h )
{
  int i;
  for(i = 0;i < dim; i++)
    res[i] = ( I(x,0,i) + h*( 55*I(f,0,i) - 59*I(f,1,i) + 37*I(f,2,i) -  9*I(f,3,i) )/24.0 );
}

void open_4step_2( real* res, int dim, real* f, real* x, real h )
{
  int i;
  for(i = 0;i < dim; i++)
    res[i] = ( I(x,3,i) + 4*h*( 2*I(f,0,i) -   I(f,1,i) + 2*I(f,2,i) )/3.0 );
}


/* ----------------------------------------------------------------------- */

static real* x0 = NULL;

void InitializeHamming( void )
{
  if(x0 != NULL) {
    free(x0);
    x0 = NULL;
  }
}

void Hamming( real* res, int dim, p_dequation1 *func, real* f[4], real* x[4], real t, real h )
{
  int i;
  real *x0new, *x0new_, *f0new, *xnew_;


  /* ewentualna inicjalizacja */
  if(x0 == NULL) {
    x0 = (real*)calloc(dim,sizeof(real));
    for(i = 0;i < dim; i++)
      x0[i] = f[0][i];
  }

  x0new  = (real *)calloc(dim,sizeof(real));
  x0new_ = (real *)calloc(dim,sizeof(real));
  f0new  = (real *)calloc(dim,sizeof(real));
  xnew_  = (real *)calloc(dim,sizeof(real));

  /* wzor wstepny */
  for(i = 0;i < dim; i++)
    x0new[i] = x[3][i] + (4.0/3.0)*h*( 2*f[0][i] - f[1][i] + 2*f[2][i] );

  /* wzor modyfikujacy */
  for(i = 0;i < dim; i++)
    x0new_[i]  = x0new[i] + (112.0/121.0)*(x[0][i] - x0[i]);

  for(i = 0;i < dim; i++)
    f0new[i] = func[i](t+h,x0new_);

  /* wzor korygujacy */
  /* ( jeden krok iteracji prostej metody zamknietej ) */
  for(i = 0;i < dim; i++)
    xnew_[i] = (1.0/8.0)*(9*x[0][i] - x[2][i]) + (3.0/8.0)*h*(f0new[i] + 2*f[0][i] - f[1][i]);

  /* wzor koncowy */
  for(i = 0;i < dim; i++)
    res[i] = xnew_[i] - (9.0/121.0)*(xnew_[i] - x0new[i]);

  for(i = 0;i < dim; i++)
    x0[i] = x0new[i];

  free(x0new);
  free(x0new_);
  free(f0new);
  free(xnew_);
}

/* ============================================================= */

struct
{
  char *Name;
  RK_method *pMethod;
} RK_methods_table[RK_METHODS_NO] =

  {
   {"Eulera - 1. rzedu",Euler},
   {"Heuna - 2. rzedu" ,Heun},
   {"Ulepszona Eulera - 2. rzedu",ImprovedEuler},
   {"Rungego-Kutty 4. rzedu - klasyczna",RungeKutta4_classic},
   {"Rungego-Kutty 4. rzedu - trzech osmych",RungeKutta4_three_eighths},
   {"Rungego-Kutty 4. rzedu - Ralstona",RungeKutta4_Ralston}/*,
   {"Scratona 5. rzedu",RungeKutta5_Scraton}                */
  };


struct
{
  char *Name;
  RK_varstep_method *pMethod;
} RK_varstep_methods_table[RK_VARSTEP_METHODS_NO] =
 {
  {"RK 4. rzedu (poprawka do 5.) z ekstrapolacja Richardsona",RungeKutta4_5_classic}
 };

/* -------------------------- */

struct
{
  char *Name;
  multistep_method *pMethod;
} mutistep_methods_table[MULTISTEP_METHODS_NO] =

  {{"Midpoint (dwukrokowa",Midpoint},
   {"jednokrokowa (Eulera)",open_1step_1},
   {"dwukrokowa",open_2step_1},
   {"trojkrokowa",open_3step_1},
   {"czterokrokowa pierwsza",open_4step_1},
   {"czterokrokowa druga",open_4step_2}};

#undef __DMETODY_N_C__