#include <stdio.h>
#include <math.h>
#include <stdlib.h>
#include <string.h>
#include <time.h>

#define BIFORCAZIONE

typedef unsigned long long int INT;
struct phasespace{
  double x;
  double v;
};

struct phasespace leap_frog(struct phasespace, double, double, double);
struct phasespace verlet(struct phasespace, double, double);
struct phasespace sumstruct(struct phasespace, struct phasespace, double, double);
struct phasespace RK2(struct phasespace, double, double, double, double, double, double);
struct phasespace RK4(struct phasespace, double, double, double, double, double, double);
struct phasespace RKn(struct phasespace, double, double, double, double, double, double, double, double);
double energy(struct phasespace, double);
double modulo(double, double);
FILE *control_fopen(char *, char *);

int main(){

  clock_t begin = clock();
  struct phasespace vec, vec0;
  double TMAX, w2, dt, t, E0=1., E=0., xnm1, temp, f0, we, gamma, Te, eps, VAR = M_PI/500.;
  INT i, j, N=2, method, STEPS, counter=0;
  FILE *input, *output, *error, *poincare, *attrazione;

  input = control_fopen("input.dat", "r");

  fprintf(stderr, "%lf\n", VAR);  //controllo

  do{
    printf("Inserire 1 per leap_frog, 2 per Verlet, 3 per RK2, 4 per RK4\n");
    scanf("%llu", &method);
  }while(method != 1 && method != 2 && method != 3 && method != 4);

  fscanf(input, "%lf %lf %lf %lf %lf %lf %lf %lf", &vec.x, &vec.v, &w2, &TMAX, &dt, &f0, &we, &gamma);
  fclose(input);
  Te = 2.*M_PI/we;

  #ifdef POINCARE
  dt = Te/1000.;
  TMAX = 30000*Te;
  eps = dt/2.;
  #endif

  poincare = control_fopen("poincare.dat", "w");
  #ifdef ERRORE
  error = control_fopen("error.dat", "w");
  #endif

  STEPS = (INT)TMAX/dt+1;

  fprintf(stderr, "%llu\n", STEPS);

  #ifdef INTEGRATORE
  output = control_fopen("pendolo.dat", "w");
  //leap-frog
  if(method == 1){
    #ifdef ENERGY
    E0 = energy(vec, w2);
    #endif
    fprintf(output, "%lf, %lf, %lf, %.10lf\n", 0., modulo(vec.x, 2*M_PI), vec.v, E0/E0-1);
    xnm1 = vec.x;
    vec = verlet(vec, w2, dt);
    for(j=0; j<STEPS; ++j){
      temp = vec.x;
      vec = leap_frog(vec, xnm1, w2, dt);
      xnm1 = temp;
      #ifdef ENERGY
      E = energy(vec, w2);
      #endif
      fprintf(output, "%lf, %lf, %lf, %lf\n", (j+1)*dt, modulo(vec.x, 2*M_PI), vec.v, E/E0-1);
    }
  }

  //Verlet
  if(method == 2){
    #ifdef ENERGY
    E0 = energy(vec, w2);
    #endif
    fprintf(output, "%lf, %lf, %lf, %.10lf\n", 0., modulo(vec.x, 2*M_PI), vec.v, E0/E0-1);
    for(j=0; j<STEPS; ++j){
      vec = verlet(vec, w2, dt);
      #ifdef ENERGY
      E = energy(vec, w2);
      #endif
      fprintf(output, "%lf, %lf, %lf, %lf\n", (j+1)*dt, modulo(vec.x, 2*M_PI), vec.v, E/E0-1);
    }
  }

  //RK2
  if(method == 3){
    #ifdef ERRORE
    for(i=0; i<6; i++){
      #endif
      #ifdef ENERGY
      E0 = energy(vec, w2);
      #endif
      fprintf(output, "%lf %lf %lf %lf %.10lf\n", 0., vec.x, modulo(vec.x, 2*M_PI), vec.v, E0/E0-1);
      for(j=0; j<STEPS; ++j){
        vec = RK2(vec, w2, dt, j*dt, gamma, f0, we);
        #ifdef ENERGY
        E = energy(vec, w2);
        #endif
        fprintf(output, "%lf %lf %lf %lf %.10lf\n", (j+1)*dt, vec.x, modulo(vec.x, 2*M_PI), vec.v, E/E0-1);
        #ifdef POINCARE
        if((N*Te-(j+1)*dt) < eps){
          fprintf(poincare, "%lf %lf\n", modulo(vec.x, 2*M_PI), vec.v);
          ++N;
        }
        #endif
      }
      #ifdef ERRORE
      fprintf(error, "%.10lf, %.10lf\n", log10(dt), log10(fabs(E/E0-1)));
      dt *= 1./sqrt(10.);
    }    // VERIFICA ORDINE DEL METODO --> OK
    #endif
  }

  //RK4
  if(method == 4){
    #ifdef ATTRAZIONE
    attrazione = control_fopen("attrazione.dat", "w");
    for(vec0.v = -M_PI; vec0.v <= M_PI; vec0.v += VAR){
      fprintf(stderr, "banana %llu\n", ++counter);  //controllo
      for(vec0.x = -M_PI; vec0.x <= M_PI; vec0.x += VAR){
        vec.x = vec0.x;
        vec.v = vec0.v;
        #endif
        #ifdef ENERGY
        E0 = energy(vec, w2);
        #endif
        #ifdef TRAIETTORIA
        //fprintf(output, "%lf %lf %lf %lf %.10lf\n", 0., vec.x, modulo(vec.x, 2*M_PI), vec.v, E0/E0-1);
        #endif
        for(j=0; j<STEPS; ++j){
          vec = RK4(vec, w2, dt, j*dt, gamma, f0, we);
          #ifdef ENERGY
          E = energy(vec, w2);
          #endif
          #ifdef TRAIETTORIA
          if(j>10000){
            fprintf(output, "%lf %lf %lf %lf %.10lf\n", (j+1)*dt, vec.x, modulo(vec.x, 2*M_PI), vec.v, E/E0-1);
          }
          #endif
          #ifdef POINCARE
          if((j%1000) == 0 && j > 10000){
            fprintf(poincare, "%lf %lf\n", modulo(vec.x, 2*M_PI), vec.v);
            ++N;
          }
          #endif
        }
        #ifdef ATTRAZIONE
        fprintf(attrazione, "%lf %lf %lf i\n", vec0.x, vec0.v, vec.v);
      }
      fprintf(attrazione, "\n");
    }
    fclose(attrazione);
    #endif
  }
  fclose(output);
  #endif

  #ifdef BIFORCAZIONE
  struct phasespace vec0b;
  FILE *biforcazione;
  vec0b.x = vec.x;
  INT k;
  biforcazione = control_fopen("biforcazione.dat", "w");
  dt = Te/100.;
  TMAX = 100*Te;
  STEPS = (INT)Te*100./dt+1;
  for(k=0; k<11; ++k){
    vec0b.v = k*M_PI/10.;
    for(f0=0.90; f0<=1.50; f0+=0.0001){
      printf("banana %llu\n", ++counter);
      vec.x = vec0b.x;
      vec.v = vec0b.v;
      for(j=0; j<STEPS; ++j){
        vec = RK4(vec, w2, dt, j*dt, gamma, f0, we);
      }
      fprintf(biforcazione, "%.10lf %.10lf %.10lf\n", f0, -modulo(vec.x, 2*M_PI), -vec.v);
    }
  }
  #endif

  fclose(poincare);
  #ifdef ERRORE
  fclose(error);
  #endif
  clock_t end = clock();
  printf("%lf\n", (double)(end-begin)/CLOCKS_PER_SEC);

}


struct phasespace leap_frog(struct phasespace vec, double xnm1, double w2, double dt){
  vec.x = 2*vec.x-xnm1-w2*vec.x*dt*dt;
  vec.v = (vec.x-xnm1)/(2*dt);
  return vec;
}

struct phasespace verlet(struct phasespace vec, double w2, double dt){
  double xnm1 = vec.x;
  vec.x += vec.v*dt-w2*vec.x/2*dt*dt;
  vec.v += -w2*(vec.x+xnm1)/2*dt;
  return vec;
}

struct phasespace sumstruct(struct phasespace vec1, struct phasespace vec2, double lambda1, double lambda2){
  struct phasespace sum;
  sum.x = lambda1*vec1.x+lambda2*vec2.x;
  sum.v = lambda1*vec1.v+lambda2*vec2.v;
  return sum;
}

struct phasespace RK2(struct phasespace vec, double w2, double dt, double t, double gamma, double f0, double we){
  struct phasespace k1, k2;
  k1 = RKn(vec, 0., 0., w2, dt, t, gamma, f0, we);
  k2 = RKn(vec, k1.x/2., k1.v/2., w2, dt, t+dt/2., gamma, f0, we);
  return vec = sumstruct(vec, k2, 1., 1.);
}

struct phasespace RK4(struct phasespace vec, double w2, double dt, double t, double gamma, double f0, double we){
  struct phasespace k1, k2, k3, k4;
  k1 = RKn(vec, 0., 0., w2, dt, t, gamma, f0, we);
  k2 = RKn(vec, k1.x/2., k1.v/2., w2, dt, t+dt/2., gamma, f0, we);
  k3 = RKn(vec, k2.x/2., k2.v/2., w2, dt, t+dt/2., gamma, f0, we);
  k4 = RKn(vec, k3.x, k3.v, w2, dt, t+dt, gamma, f0, we);
  vec.x += (k1.x+2*k2.x+2*k3.x+k4.x)/6.;
  vec.v += (k1.v+2*k2.v+2*k3.v+k4.v)/6.;
  return vec;
}

struct phasespace RKn(struct phasespace vec, double kix, double kiv, double w2, double dt, double t, double gamma, double f0, double we){
  struct phasespace kj;
  kj.x = (vec.v+kiv)*dt;
  kj.v = ((-w2*sin(vec.x+kix))-gamma*(vec.v+kiv)+f0*cos(we*(t)))*dt;
  return kj;
}

double energy(struct phasespace vec, double w2){
  return vec.v*vec.v+2*w2*(1-cos(vec.x));
}

double modulo(double a, double m){
  a += m/2.;
  double c = ((a/m)-(int)(a/m))*m;
  return (c > 0 ? c : c+m)-m/2.;
}


FILE *control_fopen(char *filename, char *mode){
  FILE *pointer;
  if((pointer = fopen(filename, mode)) == NULL){
    printf("Errore nell'apertura del file\n");
    exit(EXIT_FAILURE);
  }
  return pointer;
}