/* The solution the differential equation system by Adams method. */
/* This code will be used for nonlinear or linear practical system */
/* simulation under DOS with intarface via computer port LPT (printer port) */
/* It will be usable with PIC microcontroler to see the whole controling */
/* proces. The aim is to create software for microcontroler without necessity */
/* to go into terraine. */

#include <stdio.h>
#include <math.h>


#define NEQU 2

typedef double (*function)(double, double *);
function Vector[21];

double TNTargetF1(double T, double *V)
/*--------------------------------------------------*/
/*- This is the first differential equation.       -*/
/*-                                                -*/
/*-  dx[1]                                         -*/
/*-  -----  = TNTargetF1(T, X[1], X[2], ... X[m])  -*/
/*-    dt                                          -*/
/*-                                                -*/
/*- The vector V is defined:                       -*/
/*-      T    = T                                  -*/
/*-      V[1] = X[1]                               -*/
/*-      V[2] = X[2]                               -*/
/*-           .                                    -*/
/*-           .                                    -*/
/*-           .                                    -*/
/*-      V[m] = X[m]                               -*/
/*-                                                -*/
/*- where m is the number of coupled equations.    -*/
/*--------------------------------------------------*/

{
  return -V[2];
} /* function TNTargetF1 */

double TNTargetF2(double T, double *V)
/*--------------------------------------------------*/
/*- This is the 2nd differential equation.         -*/
/*-                                                -*/
/*-  dx[2]                                         -*/
/*-  -----  = TNTargetF1(T, X[1], X[2], ... X[m])  -*/
/*-    dt                                          -*/
/*-                                                -*/
/*- The vector V is defined:                       -*/
/*-      V[1] = X[1]                               -*/
/*-      V[2] = X[2]                               -*/
/*-           .                                    -*/
/*-           .                                    -*/
/*-           .                                    -*/
/*-      V[m] = X[m]                               -*/
/*-                                                -*/
/*- where m is the number of coupled equations.    -*/
/*--------------------------------------------------*/
{
  return V[1];
} /* function TNTargetF2 */

double TNTargetF3(double T, double *V) { return 0; }
double TNTargetF4(double T, double *V) { return 0; }
double TNTargetF5(double T, double *V) { return 0; }
double TNTargetF6(double T, double *V) { return 0; }
double TNTargetF7(double T, double *V) { return 0; }
double TNTargetF8(double T, double *V) { return 0; }
double TNTargetF9(double T, double *V) { return 0; }
double TNTargetF10(double T, double *V) { return 0; }
double TNTargetF11(double T, double *V) { return 0; }
double TNTargetF12(double T, double *V) { return 0; }
double TNTargetF13(double T, double *V) { return 0; }
double TNTargetF14(double T, double *V) { return 0; }
double TNTargetF15(double T, double *V) { return 0; }
double TNTargetF16(double T, double *V) { return 0; }
double TNTargetF17(double T, double *V) { return 0; }
double TNTargetF18(double T, double *V) { return 0; }
double TNTargetF19(double T, double *V) { return 0; }
double TNTargetF20(double T, double *V) { return 0; }

void AssignFunc() {
  Vector[0] = NULL;
  Vector[1] = &TNTargetF1;
  Vector[2] = &TNTargetF2;
  Vector[3] = &TNTargetF3;
  Vector[4] = &TNTargetF4;
  Vector[5] = &TNTargetF5;
  Vector[6] = &TNTargetF6;
  Vector[7] = &TNTargetF7;
  Vector[8] = &TNTargetF8;
  Vector[9] = &TNTargetF9;
  Vector[10] = &TNTargetF10;
  Vector[11] = &TNTargetF11;
  Vector[12] = &TNTargetF12;
  Vector[13] = &TNTargetF13;
  Vector[14] = &TNTargetF14;
  Vector[15] = &TNTargetF15;
  Vector[16] = &TNTargetF16;
  Vector[17] = &TNTargetF17;
  Vector[18] = &TNTargetF18;
  Vector[19] = &TNTargetF19;
  Vector[20] = &TNTargetF20;
}

/*----------------------------------------------------------------------------*/
/*-                                                                          -*/
/*-    Input:  LowerLimit, UpperLimit, XInitial, NumReturn, NumIntervals     -*/
/*-   Output:  TValues, XValues, Error                                       -*/
/*-                                                                          -*/
/*-  Purpose:  This unit solves an initial value problem (a first order      -*/
/*-            differential equation with initial condition specified).      -*/
/*-            Given a function of the form                                  -*/
/*-                       dx/dt = TNTargetF(T, X)                            -*/
/*-            and an initial condition                                      -*/
/*-                       X[LowerLimit] = const                              -*/
/*-            the Adams-Bashforth/Adams-Moulton four step predictor/        -*/
/*-            corrector method is used to solve for X in  the interval      -*/
/*-            [LowerLimit, UpperLimit].  The interval is divided up into    -*/
/*-            NumIntervals equal sub-intervals.                             -*/
/*-                                                                          -*/
/*-  User-defined Functions: TNTargetF(T, X : real) : real;                  -*/
/*-                                                                          -*/
/*-      User-defined Types: TNvector = array[0..TNArraySize] of real;       -*/
/*-                                                                          -*/
/*-    Global Variables:  LowerLimit   : real;    Lower limit of integration -*/
/*-                       UpperLimit   : real;    Upper limit of integration -*/
/*-                       XInitial     : real;    Value of X at LowerLimit   -*/
/*-                       NumReturn    : integer; Number of values to return -*/
/*-                       NumIntervals : integer; Number of subintervals     -*/
/*-                       TValues      : TNvector; Value of T between limits -*/
/*-                       XValues      : TNvector; Value of X at TValues     -*/
/*-                       Error        : byte;    Flags if something goes    -*/
/*-                                               wrong                      -*/
/*-                                                                          -*/
/*-               Notes:  NumIntervals should be an integer                  -*/
/*-                       multiple of NumReturn if you want                  -*/
/*-                       evenly spaced values returned.                     -*/
/*-                                                                          -*/
/*-              Errors:  0: No errors                                       -*/
/*-                       1: NumReturn <= 0                                  -*/
/*-                       2: NumIntervals < NumReturn                        -*/
/*-                       3: LowerLimit = UpperLimit                         -*/
/*-                                                                          -*/
/*----------------------------------------------------------------------------*/

void GetFirstThreeValues(double Spacing,
			      double *T,
			      double *X,
			      double **Values,
			      double (*DerivValues)[NEQU+1][6]);  // Runge-Kutta method;

void Adams(double LowerLimit,
		double UpperLimit,
		double *XInitial,
		int NumReturn,
		int NumIntervals,
		unsigned char *Error)
{
  double Spacing;                 /* Size of each subinterval */
  double T;                       /* Value of T */
  double X[NEQU+1], NewX[NEQU+1];     /* Iteration variables */
  int Index,FuncIndex;            /* Counters */
  double DerivValues[NEQU+1][6];    /* Derivative at last 4 X values */
  double Values[NEQU+1][6];
  int i_print;                    /* A counter for printing output */

  *Error = 0;
  if ( NumReturn <= 0 )
    *Error = 1;
  if ( NumIntervals < NumReturn )
    *Error = 2;
  if ( LowerLimit == UpperLimit )
    *Error = 3;
  if ( *Error == 0 )
  {
    AssignFunc();         /* Assignment of function pointers to the array */
    for (FuncIndex = 1; FuncIndex <= NEQU; FuncIndex++)
    for (Index = 0; Index <= 5;Index++) {
      DerivValues[FuncIndex][Index] = 0;
    }
    for (FuncIndex = 1; FuncIndex <= NEQU; FuncIndex++)
    {
      DerivValues[FuncIndex][1] = Vector[FuncIndex](LowerLimit, XInitial);
    }
    Spacing = (UpperLimit - LowerLimit) / NumIntervals;
    T = LowerLimit;
    for (FuncIndex = 1; FuncIndex <= NEQU; FuncIndex++) {
      X[FuncIndex] = XInitial[FuncIndex];
    }
    printf("%e  ",T);
    for(i_print=1;i_print<=NEQU;i_print++) printf("%e  ",X[i_print]);
    printf("\n");
    GetFirstThreeValues(Spacing, &T, X, Values, &DerivValues);
    for (Index = 4; Index <= NumIntervals; Index++)
    {
      {
	for (FuncIndex = 1; FuncIndex <= NEQU; FuncIndex++)
	{
	  /* Apply predictor */
	  NewX[FuncIndex] = X[FuncIndex] +
	     Spacing / 24 * (55 * DerivValues[FuncIndex][4] - 59 * DerivValues[FuncIndex][3] +
			     37 * DerivValues[FuncIndex][2] - 9 * DerivValues[FuncIndex][1]);
	}
	T += Spacing;
	for (FuncIndex = 1; FuncIndex <= NEQU; FuncIndex++)
	{
	  DerivValues[FuncIndex][1] = DerivValues[FuncIndex][2];
	  DerivValues[FuncIndex][2] = DerivValues[FuncIndex][3];
	  DerivValues[FuncIndex][3] = DerivValues[FuncIndex][4];
	  DerivValues[FuncIndex][4] = Vector[FuncIndex](T, NewX);
	}
	for (FuncIndex = 1; FuncIndex <= NEQU; FuncIndex++)
	{
	  /* Apply corrector */
	  NewX[FuncIndex] = X[FuncIndex] +
		  Spacing / 24 * (9 * DerivValues[FuncIndex][4] + 19 * DerivValues[FuncIndex][3] -
		  5 * DerivValues[FuncIndex][2] + DerivValues[FuncIndex][1]);
	  X[FuncIndex] = NewX[FuncIndex];
	}
	T -= Spacing;
      }
      T += Spacing;
      printf("%e  ",T);
      for(i_print=1;i_print<=NEQU;i_print++) printf("%e  ",X[i_print]);
      printf("\n");
    }
  }
} /* procedure Adams */

void GetFirstThreeValues(double Spacing,
			      double *T,
			      double *X,
			      double **Values,
			      double (*DerivValues)[NEQU+1][6])

/*---------------------------------------------------------------*/
/*- Input: Spacing, T, X, Values, DerivValues                   -*/
/*- Output: T, X, Values, DerivValues                           -*/
/*-                                                             -*/
/*- This procedure uses the Runge-Kutta one step method to      -*/
/*- solve for the first three (T + i-Spacing, 1<=i<=3) Values.  -*/
/*- The derivative at these points are also calculated          -*/
/*- (DerivValues).                                              -*/
/*---------------------------------------------------------------*/

{
  int Index,FuncIndex;

  double HalfSpacing;
  double F1[NEQU+1], F2[NEQU+1], F3[NEQU+1], F4[NEQU+1], Hx[NEQU+1];
  int i_print;

  HalfSpacing = Spacing / 2;

  for (Index = 1; Index < 4; Index++)
  {
    for (FuncIndex = 1; FuncIndex <= NEQU; FuncIndex++ )
      F1[FuncIndex] = Spacing * Vector[FuncIndex](*T, X);
    for (FuncIndex = 1; FuncIndex <= NEQU; FuncIndex++)
      Hx[FuncIndex] = X[FuncIndex] + F1[FuncIndex]/2;
    for (FuncIndex = 1; FuncIndex <= NEQU; FuncIndex++ )
      F2[FuncIndex] = Spacing * Vector[FuncIndex](*T + HalfSpacing, Hx);
    for (FuncIndex = 1; FuncIndex <= NEQU; FuncIndex++)
      Hx[FuncIndex] = X[FuncIndex] + F2[FuncIndex]/2;
    for (FuncIndex = 1; FuncIndex <= NEQU; FuncIndex++ )
      F3[FuncIndex] = Spacing * Vector[FuncIndex](*T + HalfSpacing, Hx);
    for (FuncIndex = 1; FuncIndex <= NEQU; FuncIndex++)
      Hx[FuncIndex] = X[FuncIndex] + F3[FuncIndex];
    for (FuncIndex = 1; FuncIndex <= NEQU; FuncIndex++ )
      F4[FuncIndex] = Spacing * Vector[FuncIndex](*T + Spacing, Hx);
    for (FuncIndex = 1; FuncIndex <= NEQU; FuncIndex++ )
      X[FuncIndex] = X[FuncIndex] +
      (   F1[FuncIndex] + 2 * F2[FuncIndex] +
      2 * F3[FuncIndex] +     F4[FuncIndex]) / 6;
    for (FuncIndex = 1; FuncIndex <= NEQU; FuncIndex++ )
      (*DerivValues)[FuncIndex][Index + 1] = Vector[FuncIndex](*T, X);
    *T += Spacing;
    printf("%e  ",*T);
    for(i_print=1;i_print<=NEQU;i_print++) printf("%e  ",X[i_print]);
    printf("\n");
  }
} /* procedure GetFirstThreeValues */


void main() {
  int error = 0;
  double pocatek[NEQU+1]= {0,0,1};

  printf("\n\n\n\n\n\n\n\n\n\n\n\n\n");
  Adams(0,M_PI,pocatek,20,20,&error);
}
