//******************************************************************************
// Programme de simulation du comportement d'une boussole dans un champ
// magnétique tournant -  Illustration du chaos
//
// Dominique Lefebvre - TangenteX.com  Octobre 2009
//******************************************************************************

#include <cstdlib>
#include <iostream>
#include <math.h>
#include <dislin_d.h>

using namespace std;

//******************************************************************************
// Définition des constantes
//******************************************************************************
#define TMAX     50         // durée max de la manip
#define PAS      0.01       // pas temporel de calcul
#define VMAX     5.0        // vitesse angulaire
#define N        3          // nombre d'équations du système différentiel

#define PI       3.1415927
#define DEUXPI   6.2831853


//******************************************************************************
// Définition des variables globales
//******************************************************************************
double alpha, beta, gamma, dt;

//******************************************************************************
// Définition des subroutines de service
//******************************************************************************
void plot_d(double x, double y)
{
   rlstrt(x,y);
   rlconn(x,y); 
   
   return;     
}

void InitGraphDislin(void)
{
    metafl("XWIN");
    disini();
    pagera();
    pagfll(255);
    color("black");
    winfnt("arial");
    
    return;     
}

//******************************************************************************
// Définition du système différentiel
//******************************************************************************
void Derivee(double Y[], double dY[])
{
   dY[0] = Y[1];                                       // y = d(theta)/dt
   dY[1] = -alpha*Y[1]-beta*sin(Y[0])-gamma*sin(Y[2]); // d²(theta)/dt²
   dY[2] = -1;                                         // dz/dt = d(theta-t)/dt
   return;
}

//******************************************************************************
// Routine RK4 - tirée des Numerical Recipes
//******************************************************************************
void RK4(double yin[], double yout[], double h)
{
   // La constante N, qui indique le nombre d'équations du système différentiel 
   // est définie globalement pour tout le programme.
   int i;
   double dydx[N], dydxt[N], yt[N], k1[N], k2[N], k3[N], k4[N];
  
   Derivee(yin, dydx); /* first step */
   for (i = 0; i < N; i++) 
    {
      k1[i] = h*dydx[i];
      yt[i] = yin[i] + 0.5*k1[i];
    }

   Derivee(yt, dydxt); /* second step */ 
   for (i = 0; i < N; i++)
    {
      k2[i] = h*dydxt[i];
      yt[i] = yin[i] + 0.5*k2[i];
    }   

   Derivee(yt, dydxt); /* third step */
   for (i = 0; i < N; i++)
    {
      k3[i] = h*dydxt[i];
      yt[i] = yin[i] + k3[i];
    }

   Derivee(yt, dydxt); /* fourth step */
   for (i = 0; i < N; i++)
    {
      k4[i] = h*dydxt[i];
      yout[i] = yin[i] + k1[i]/6. + k2[i]/3. + k3[i]/3. + k4[i]/6.;
    }
  return;
}


//******************************************************************************
// Programme principal
//******************************************************************************
int main(int argc, char *argv[])
{
    double theta0, v0, t0, theta, v, t, dt;
    double Yin[N], Yout[N];
    char buffer[80];
    
    // Déclaration des conditions initiales statiques
    t0 = 0.0;
    theta0 = 0.0;    

    // Saisie des paramètres de calcul alpha, beta, gamma
    printf("Vitesse angulaire initiale: ");scanf("%lf",&v0);
    printf("Valeur de alpha: "); scanf("%lf",&alpha); 
    printf("Valeur de beta: ");scanf("%lf",&beta); 
    printf("Valeur de gamma: "); scanf("%lf",&gamma); 

    // Initialisation des variables
    t = t0;
    dt = PAS;
    theta = theta0;
    v = v0;
   
    // Initialisation de la librairie graphique
    InitGraphDislin();
    
    // Tracé des axes et des légendes
    wintit("Boussole - Espace de phases");
    axstyp("CROSS");
    name("d(theta)/dt","X"); namjus("RIGHT","X");
    name("theta","Y");namjus("RIGHT","Y");
    labels("NONE","X");
    labels("NONE","Y");
    graf(-VMAX,VMAX,-VMAX,1,-PI,PI,-PI,PI/10);
    titlin("Diagramme de l'espace de phases (v, theta)",1);
    title();

    // Affichage des paramètres
    sprintf(buffer," -  Parametres v0: %.2lf  alpha: %.2lf  beta: %.2lf gamma: %.2lf",
            v0,alpha,beta,gamma);
    paghdr("Dominique Lefebvre - ",buffer,2,0); 
    
    color("red");     // tracé des points en rouge

    rlstrt(v,theta);  // positionnement aux valeurs initiales
    
    // boucle de calcul et d'affichage de la trajectoire par RK4
    do
       {
          // Calcul par RK4
          Yin[0] = theta;
          Yin[1] = v;
          Yin[2] = theta - t;
          RK4(Yin, Yout, dt);
          theta = Yout[0];
          v = Yout[1];
          // increment du temps
          t += dt;
          // contraintes de limitation au domaine [-PI, PI]
          if (theta > PI) 
           theta = theta - DEUXPI;
          else
           if (theta < -PI) 
              theta = theta + DEUXPI;
          // plot du point (v,theta)
          plot_d(v,theta);
       }       
    while (t <= TMAX);
    
    // Fin du programme
    disfin();
    return EXIT_SUCCESS;
}