/* 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 <dos.h>
#include <process.h>
#include <stdio.h>
#include <math.h>


/*---------------------- OBRACENE KYVADLO -------------------------------*/
#define NEQU 4              // [-] pocet diferencialnich rovnic
#define TIMER 1             // [-] simulace v realnem case
#define TO_FILE 0           // [-] vypis na obrazovku nebo do souboru
#define OUT  "Reverze.txt"  // nazev vystupniho souboru
#define M 50.0              // [kg] hmotnost posuvne casti
#define m 15.0              // [kg] hmotnost matematickeho kyvadla
#define g 0.009806          // [mm.ms-2] tihove zrychleni
#define l 1000.0            // [mm] delka matematickeho kyvadla
#define r 0                 // [kN.ms-1] konstanta pro zadani funkce sily
#define s 0.15              // [kN] konstanta pro zadani funkce budici sily

double F(double T) { return r*T + s; }   // funkce siloveho pusobeni

/*- soustava ctyrech diferencialnich rovnic prvniho radu -----------------*/
double TNTargetF1(double T, double *V)
  { return V[2]; }
double TNTargetF2(double T, double *V)
  { return F(T)/M+g*m/M*sin(V[3]); }
double TNTargetF3(double T, double *V)
  { return V[4]; }
double TNTargetF4(double T, double *V)
  { return F(T)/M/l*cos(V[3])+g/l*(1+m/M)*sin(V[3]); }
/*----------------------- KONEC ZADANI -----------------------------------*/

/*---------------------------------------------------------*/
/*- These are the other  unused differential equations.   -*/
/*-                                                       -*/
/*-  dx[k]                                                -*/
/*-  -----  = TNTargetFk(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.           -*/
/*---------------------------------------------------------*/
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; }

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

unsigned long int oldtime = 0;   /* miliseconds */

void settiming()
{
   struct  time t;

   gettime(&t);
   oldtime = (t.ti_hund+(t.ti_sec+t.ti_min*60)*100)*10;
}

void timing(int deltatime)
{
   struct  time t;
   unsigned long int symtime;
   int isover = 0;

   do {
     gettime(&t);
     symtime = (t.ti_hund+(t.ti_sec+t.ti_min*60)*100)*10;
     isover++;
   } while ((oldtime + deltatime) > symtime);
   if (isover == 1) {
     fprintf(outfile,"The computer isn't fast enough for this task.\n");
     exit(0);
   }
   oldtime = symtime;
}

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 GetFirstFourValues(double Spacing,
			      double *T,
			      double *X,
			      double (*Values)[NEQU+1][6],
			      double (*DerivValues)[NEQU+1][6]);  // Runge-Kutta method;
void GetFirstFourValues38(double Spacing,
			      double *T,
			      double *X,
			      double (*Values)[NEQU+1][6],
			      double (*DerivValues)[NEQU+1][6]);  // Runge-Kutta method;
void GetFirstFourValuesGill(double Spacing,
			      double *T,
			      double *X,
			      double (*Values)[NEQU+1][6],
			      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];
    }
    if (TIMER) settiming();
    if (TO_FILE) {
      fprintf(outfile,"%e  ",T);
      for(i_print=1;i_print<=NEQU;i_print++) fprintf(outfile,"%e  ",X[i_print]);
      fprintf(outfile,"\n");
    } else
    {
      printf("%e  ",T);
      for(i_print=1;i_print<=NEQU;i_print++) printf("%e  ",X[i_print]);
      printf("\n");
    }  
    GetFirstFourValues(Spacing, &T, X, &Values, &DerivValues);
    for (Index = 5; Index <= NumIntervals; Index++)
    {
      {
	for (FuncIndex = 1; FuncIndex <= NEQU; FuncIndex++)
	{
	  /* Apply predictor */
	  NewX[FuncIndex] = X[FuncIndex] + Spacing / 720 *
			     (1901 * DerivValues[FuncIndex][5] -
			      2774 * DerivValues[FuncIndex][4] +
			      2616 * DerivValues[FuncIndex][3] -
			      1274 * DerivValues[FuncIndex][2] +
			       251 * 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] = DerivValues[FuncIndex][5];
	  DerivValues[FuncIndex][5] = Vector[FuncIndex](T, NewX);
	}
	for (FuncIndex = 1; FuncIndex <= NEQU; FuncIndex++)
	{
	  /* Apply corrector */
	  NewX[FuncIndex] = X[FuncIndex] + Spacing / 720 *
			    (251 * DerivValues[FuncIndex][5] +
			     646 * DerivValues[FuncIndex][4] -
			     264 * DerivValues[FuncIndex][3] +
			     106 * DerivValues[FuncIndex][2] -
			      19 * DerivValues[FuncIndex][1]);
	  X[FuncIndex] = NewX[FuncIndex];
	}
	T -= Spacing;
      }
      T += Spacing;
      if (TIMER) timing((int)Spacing);
      if (TO_FILE) {
        fprintf(outfile,"%e  ",T);
        for(i_print=1;i_print<=NEQU;i_print++) fprintf(outfile,"%e  ",X[i_print]);
        fprintf(outfile,"\n");
      } else
      {
        printf("%e  ",T);
        for(i_print=1;i_print<=NEQU;i_print++) printf("%e  ",X[i_print]);
        printf("\n");
      }  
    }
  }
} /* procedure Adams */

void GetFirstFourValues(double Spacing,
			      double *T,
			      double *X,
			      double (*Values)[NEQU+1][6],
			      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 < 5; 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;
    if (TIMER) timing((int)Spacing);
    if (TO_FILE) {
      fprintf(outfile,"%e  ",*T);
      for(i_print=1;i_print<=NEQU;i_print++) fprintf(outfile,"%e  ",X[i_print]);
      fprintf(outfile,"\n");
    } else
    {
      printf("%e  ",*T);
      for(i_print=1;i_print<=NEQU;i_print++) printf("%e  ",X[i_print]);
      printf("\n");
    }  
  }
} /* procedure GetFirstFourValues */


void GetFirstFourValues38(double Spacing,
			      double *T,
			      double *X,
			      double (*Values)[NEQU+1][6],
			      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 < 5; 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]/3;
    for (FuncIndex = 1; FuncIndex <= NEQU; FuncIndex++ )
      F2[FuncIndex] = Spacing * Vector[FuncIndex](*T + Spacing/3, Hx);
    for (FuncIndex = 1; FuncIndex <= NEQU; FuncIndex++)
      Hx[FuncIndex] = X[FuncIndex] - F1[FuncIndex]/3 + F2[FuncIndex];
    for (FuncIndex = 1; FuncIndex <= NEQU; FuncIndex++ )
      F3[FuncIndex] = Spacing * Vector[FuncIndex](*T + 2*Spacing/3, Hx);
    for (FuncIndex = 1; FuncIndex <= NEQU; FuncIndex++)
      Hx[FuncIndex] = X[FuncIndex] + F1[FuncIndex] - F2[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] + 3 * F2[FuncIndex] + 3 * F3[FuncIndex] + F4[FuncIndex]) / 8;
    *T += Spacing;
    for (FuncIndex = 1; FuncIndex <= NEQU; FuncIndex++ )
      (*DerivValues)[FuncIndex][Index + 1] = Vector[FuncIndex](*T, X);
    if (TO_FILE) {
      fprintf(outfile,"%e  ",*T);
      for(i_print=1;i_print<=NEQU;i_print++) fprintf(outfile,"%e  ",X[i_print]);
      fprintf(outfile,"\n");
    } else
    {
      printf("%e  ",*T);
      for(i_print=1;i_print<=NEQU;i_print++) printf("%e  ",X[i_print]);
      printf("\n");
    }
  }
} /* procedure GetFirstFourValues */

void GetFirstFourValuesGill(double Spacing,
			      double *T,
			      double *X,
			      double (*Values)[NEQU+1][6],
			      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 < 5; 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] + 1/2*(sqrt(2)-1)*F1[FuncIndex] + (1-1/sqrt(2))*F2[FuncIndex];
    for (FuncIndex = 1; FuncIndex <= NEQU; FuncIndex++ )
      F3[FuncIndex] = Spacing * Vector[FuncIndex](*T + HalfSpacing, Hx);
    for (FuncIndex = 1; FuncIndex <= NEQU; FuncIndex++)
      Hx[FuncIndex] = X[FuncIndex] - 1/sqrt(2)*F2[FuncIndex] + (1+1/sqrt(2))*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-sqrt(2))*F2[FuncIndex] +  (2+sqrt(2))*F3[FuncIndex] + F4[FuncIndex]) / 6;
    *T += Spacing;
    for (FuncIndex = 1; FuncIndex <= NEQU; FuncIndex++ )
      (*DerivValues)[FuncIndex][Index + 1] = Vector[FuncIndex](*T, X);
    if (TO_FILE) {
      fprintf(outfile,"%e  ",*T);
      for(i_print=1;i_print<=NEQU;i_print++) fprintf(outfile,"%e  ",X[i_print]);
      fprintf(outfile,"\n");
    } else
    {
      printf("%e  ",*T);
      for(i_print=1;i_print<=NEQU;i_print++) printf("%e  ",X[i_print]);
      printf("\n");
    }
  }
} /* procedure GetFirstFourValues */


void main() {
  unsigned char error = 0;
  double pocatek[NEQU+1]= {0,0,0,-0.20135792,0};

  if (TO_FILE) outfile = fopen(OUT,"wt");
  Adams(0,1000,pocatek,20,20,&error);
  fclose(outfile);
}
