// dynamika podvozku diferencialnimi rovnicemi
// autor zpusobu reseni: Michal Martinu
// do vypoctu je mozno zahrnout nelinearni vlastnosti perovani
// vhodne jako zaklad pro algoritmus mikrokontroleru

#include <graphics.h>
#include <conio.h>
#include <alloc.h>
#include <stdlib.h>
#include <stdio.h>
#include <math.h>

float v = 1.; // rychlost
float X1=0.0,X2=0.0,X3=0.0,X4=0.0;
float X1old=0.0,X2old=0.0,X3old=0.0,X4old=0.0;
float Xi1=0.0,Xi2=0.0,Xi3=0.0,Xi4=0.0;
float ft=0.0,ft_old;  // sila od torze
float torzni_tuhost = 1.; // redukce na jedno kolo

// auto projede levymi koly pres vymol
float _y(float t)    // cesta - vertikalni vychylky
{
  if (t < 100/v) return 0;
  if (t < 150/v) return (-t+100/v)*v;
  if (t < 200/v) return -50;
  if (t < 250/v) return (t-250/v)*v;
  return 0;
}

float _yi(float t)   // cesta - rychlost vertikalnich vychylek
{
  if (t < 100/v) return 0;
  if (t < 150/v) return -v;
  if (t < 200/v) return 0;
  if (t < 250/v) return v;
  return 0;
}

float k1 = 1.5,b1 = 7,m1 = 250.; // tuhost, tlumeni, hmotnost
float c1=0.1,d1=1.0;
float q1=0.0,r1=0.0,s1=0.0,t1=0.0,u1=1.0,w1=0.0; // koeficienty polynomu

float wheel_front_left(float T, float X, float xPrime)
{
  float o,p;
  o = _yi(T)-xPrime;
  p = _y(T)-X;
  return -ft/m1+
	 b1/m1*(c1*o*o + d1*o)+
	 k1/m1*(q1*p*p*p*p*p + r1*p*p*p*p + s1*p*p*p + t1*p*p + u1*p + w1);
}

float k2 = 1.5,b2 = 7,m2 = 250.; // tuhost, tlumeni, hmotnost
float c2=0.1,d2=1.0;
float q2=0.0,r2=0.0,s2=0.0,t2=0.0,u2=1.0,w2=0.0; // koeficienty polynomu

float wheel_front_right(float T, float X, float xPrime)
{
  float o,p;
  o = -xPrime;
  p = -X;
  return ft/m2+
	 b2/m2*(c2*o*o + d2*o)+
	 k2/m2*(q2*p*p*p*p*p + r2*p*p*p*p + s2*p*p*p + t2*p*p + u2*p + w2);
}

float k3 = 1.5,b3 = 7,m3 = 250.; // stifness, damping, mass
float c3=0.1,d3=1.0; // koeficienty tlumeni (quadratic function)
float q3=0.0,r3=0.0,s3=0.0,t3=0.0,u3=1.0,w3=0.0;
// koeficienty polynomu perovani

float wheel_rear_left(float T, float X, float xPrime)
{
  float o,p;
  o = _yi(T-v*200)-xPrime;
  p = _y(T-v*200)-X;
  return ft/m3+
	 b3/m3*(c3*o*o + d3*o)+
	 k3/m3*(q3*p*p*p*p*p + r3*p*p*p*p + s3*p*p*p + t3*p*p + u3*p + w3);
}

float k4 = 1.5,b4 = 7,m4 = 250.; // tuhost, tlumeni, hmotnost
float c4=0.1,d4=1.0;  // koeficienty tlumeni
float q4=0.0,r4=0.0,s4=0.0,t4=0.0,u4=1.0,w4=0.0;
// koeficienty polynomu perovani

float wheel_rear_right(float T, float X, float xPrime)
{
  float o,p;
  o = -xPrime;
  p = -X;
  return -ft/m4+
	 b4/m4*(c4*o*o + d4*o)+
	 k4/m4*(q4*p*p*p*p*p + r4*p*p*p*p + s4*p*p*p + t4*p*p + u4*p + w4);
}

void SolveCarOnTheRoad(float LowerLimit,
			 float UpperLimit,
			 float *InitialValue,
			 float *InitialDeriv,
			   int NumIntervals,
			 float Function(float,float,float))

/*----------------------------------------------------------------------------*/
/*-                                                                          -*/
/*-    Input:  LowerLimit, UpperLimit, InitialValue, InitialDeriv,           -*/
/*-            NumIntervals, NumReturn                                       -*/
/*-                                                                          -*/
/*-  Purpose:  This unit solves a 2nd order ordinary differential            -*/
/*-            equation with specified initial conditions.                   -*/
/*-            Given a function of the form                                  -*/
/*-                       X" = TNTargetF(T, X, X')                           -*/
/*-            and initial conditions                                        -*/
/*-                 X[LowerLimit] = InitialValue,                            -*/
/*-                X'[LowerLimit] = InitialDeriv                             -*/
/*-            the Runge-Kutta method of two variables (X and X') is used to -*/
/*-            solve for X and X' in the interval [LowerLimit, UpperLimit].  -*/
/*-            The interval is divided up into NumIntervals sub-intervals.   -*/
/*-                                                                          -*/
/*-  User-defined Functions: TNTargetF(T, X, xPrime: real) : real;           -*/
/*-                                                                          -*/
/*-                                                                          -*/
/*-    Global Variables:  LowerLimit   : real;    Lower limit of integration -*/
/*-                       UpperLimit   : real;    Upper limit of integration -*/
/*-                       InitialValue : real;    Value of X at LowerLimit   -*/
/*-                       InitialDeriv : real;    Deriv. of X at LowerLimit  -*/
/*-                                               wrong                      -*/
/*-                                                                          -*/
/*-               Notes:  NumIntervals should be an integer                  -*/
/*-                       multiple of NumReturn if you want                  -*/
/*-                       evenly spaced values returned.                     -*/
/*-                                                                          -*/
/*-                                                                          -*/
/*----------------------------------------------------------------------------*/


{
  float Spacing, HalfSpacing;           /* Size of each subinterval  */
  int Index;
  float F1x, F2x, F3x, F4x,
  F1xPrime, F2xPrime,
  F3xPrime, F4xPrime;                   /* Iteration variables  */
  float T, X, xPrime;                   /* Values pushed and pulled  */
					/* from the stack.           */

  /* procedure InitialCond2ndOrder */
  T = LowerLimit;
  X = *InitialValue;
  xPrime = *InitialDeriv;
  Spacing = (UpperLimit-LowerLimit)/NumIntervals;
  HalfSpacing = Spacing / 2;
  for (Index = 1;Index <= NumIntervals;Index++)
  {
    F1x = Spacing * xPrime;
    F1xPrime = Spacing * Function(T, X, xPrime);
    F2x = Spacing * (xPrime + 0.5 * F1xPrime);
    F2xPrime = Spacing * Function(T + HalfSpacing, X + 0.5 * F1x,
		xPrime + 0.5 * F1xPrime);
    F3x = Spacing * (xPrime + 0.5 * F2xPrime);
    F3xPrime = Spacing * Function(T + HalfSpacing, X + 0.5 * F2x,
		xPrime + 0.5 * F2xPrime);
    F4x = Spacing * (xPrime + F3xPrime);
    F4xPrime = Spacing * Function(T + Spacing, X + F3x, xPrime +
					F3xPrime);
    X = X + (F1x + 2 * F2x + 2 * F3x + F4x) / 6;
    xPrime = xPrime + (F1xPrime + 2 * F2xPrime + 2 * F3xPrime + F4xPrime) / 6;
    T = T + Spacing;
    *InitialValue = X;
    *InitialDeriv = xPrime;
  }
} /* procedure SolveCarOnTheRoad */



int main()
{
    float t;
    int color,rychl_zobr;
     /* request auto detection */
    int gdriver = DETECT, gmode, errorcode,i;

    printf("Zadej rychlost zobrazovani (asi 40): ");scanf("%d",&rychl_zobr);
    /* initialize graphics mode */
    initgraph(&gdriver, &gmode, "");

    /* read result of initialization */
    errorcode = graphresult();

    if (errorcode != grOk)  /* an error occurred */
    {
       printf("Graphics error: %s\n", grapherrormsg(errorcode));
       printf("Press any key to halt:");
       getch();
       exit(1);             /* return with error code */
    }
    for (;;) {
      gotoxy(1,1);printf("Prejezd auta levymi koly pres vymol.\n");
      printf("Torzni namahani pri ruznych torznich tuhostech.");

      moveto(0,400);
      setcolor(LIGHTGRAY);
      for(i=1;i<640;i++) lineto(i,400-_y(i));
      setfillstyle(HATCH_FILL, DARKGRAY);

      floodfill(0,479,LIGHTGRAY);
      gotoxy(34,25);printf("Special kind of solution by Ing.Michal Martinu");
      for(i = 1;i <= 5; i++) {
	switch(i) {
	  case 1:torzni_tuhost = .02;color=RED;break;
	  case 2:torzni_tuhost = .09;color=CYAN;break;
	  case 3:torzni_tuhost = 0.4;color=YELLOW;break;
	  case 4:torzni_tuhost = 1.7;color=GREEN;break;
	  case 5:torzni_tuhost = 3.0;color=BLUE;break;
	}
	ft = 0.0;
	X1 = 0.0;X2 = 0.0;X3 = 0.0;X4 = 0.0;
	Xi1 = 0.0;Xi2 = 0.0;Xi3 = 0.0;Xi4 = 0.0;
	for (t=0.0;t<640;t++) {
	  setcolor(WHITE);
	  line(400,250-X2/2,500,250-X4/2);line(400,250-X2/2,450,300-X1/2);
	  line(450,300-X1/2,550,300-X3/2);line(500,250-X4/2,550,300-X3/2);
	  X1old=X1;X2old=X2;X3old=X3;X4old=X4;ft_old=ft;moveto(t,200-10*ft);
	  SolveCarOnTheRoad(t,t+1,&X1,&Xi1,1,wheel_front_left);
	  SolveCarOnTheRoad(t,t+1,&X2,&Xi2,1,wheel_front_right);
	  SolveCarOnTheRoad(t,t+1,&X3,&Xi3,1,wheel_rear_left);
	  SolveCarOnTheRoad(t,t+1,&X4,&Xi4,1,wheel_rear_right);
	  ft = (X1-X2-X3+X4)*torzni_tuhost;
	  setcolor(color);
	  lineto(t,-10*ft+200);
	  delay(rychl_zobr);
	  if (kbhit()) break;
	  setcolor(BLACK);
	  line(400,250-X2old/2,500,250-X4old/2);line(400,250-X2old/2,450,300-X1old/2);
	  line(450,300-X1old/2,550,300-X3old/2);line(500,250-X4old/2,550,300-X3old/2);
	}
	if (kbhit()) break;
      }
      if (kbhit()) break;
      delay(400*rychl_zobr);
      cleardevice();
    }
    closegraph();
    return 0;
}