#include<fstream>
#include<math.h>
#include<stdlib.h>
#include<bibli_fonctions.h>
using namespace std;

/* Pendule simple - Euler, RK4, petits angles */

const double K = 1.;

void systeme(double* q, double t, double* qp, int n){
  qp[0] = q[1];
  qp[1] = -K * sin(q[0]);
}

void euler(void (*syst_ed)(double*,double,double*,int),
	   double* q, double t, double dt, int n){
  int i;
  double* qp = (double*)malloc(n * sizeof(double));
  syst_ed(q,t,qp,n);
  for (i = 0; i < n; i++)
    q[i] = q[i] + dt*qp[i];
  free(qp);
}

int main(){
  int N = 2, ntot = 10000;
  fstream fich;
  double tdeb = 0., tfin = 100., dt, t, q0, qp0, qapp;
  double* q_eu = (double*)malloc(N * sizeof(double));
  double* q_rk = (double*)malloc(N * sizeof(double));

  dt = (tfin-tdeb) / (ntot-1);
  q0 = 1.; qp0 = 0.;
  q_eu[0] = q0; q_eu[1] = qp0;
  q_rk[0] = q0; q_rk[1] = qp0;
  fich.open("pendule.res", ios::out);
  for(t = tdeb; t <= tfin; t += dt){
    qapp = q0*cos(sqrt(K)*t) + qp0/sqrt(K)*sin(sqrt(K)*t);
    fich << t << " " << q_eu[0] << " " << q_rk[0] << " " << qapp << endl;
    euler(systeme,q_eu,t,dt,N);
    rk4(systeme,q_rk,t,dt,N);
  }
  fich.close();
  free(q_eu); free(q_rk);
  ostringstream pyth;
  pyth << "A = loadtxt('pendule.res')\n"
       << "plot(A[:,0], A[:,1], label='Euler')\n"
       << "plot(A[:,0], A[:,2], label='RK4')\n"
       << "plot(A[:,0], A[:,3], label='Petits angles')\n"
       << "legend()\n"
    ;
  make_plot_py(pyth);
  return 0;
}