#include <mygraph.h>
#include <unistd.h>
#include <stdio.h>
#include <math.h>


#define N 100
#define D 2
#define PI 3.1415926535897932384626433832795028841971693

double x[N][D],v[N][D]; // state of the system
double dt=0.01,g=1,k=1,m=1,x0[N][D],v0[N][D];
int ItNo=1;
int r=10;
double tet=0,omega=0,fact=3;
void FF(double F[N][D],double x[N][D]){
  for (int d=0; d<D; d++){
    F[0][d]= ((d==0)?-g:0) -k*(x[0][d])+k*(x[0+1][d]-x[0][d]);
    for (int i=1; i<N-1; i++){
      F[i][d]= ((d==0)?-g:0) -k*(x[i][d]-x[i-1][d])+k*(x[i+1][d]-x[i][d]);
    }
    F[N-1][d]= ((d==0)?-g:0) -k*(x[N-1][d]-x[N-1-1][d]);
  }
}

void Verlet(double x[N][D],double v[N][D],double dt){
  double F[N][D];
  FF(F,x);
  for (int d=0; d<D; d++)
    for (int i=0; i<N; i++){
      v[i][d]+= 0.5*F[i][d]/m*dt;
      x[i][d]+= v[i][d]*dt;
    }
  FF(F,x);
  for (int d=0; d<D; d++)
    for (int i=0;i<N; i++){
      v[i][d]+= 0.5*F[i][d]/m*dt;
    }
}

/*void setX0rest(){
  for (int d=1; d<D; d++)
    for (int i=0; i<N; i++){
      x0[i][d]=0;
      v0[i][d]=0;
    }
    }*/

void setX0rest(){
  double xt[N];
  for (int i=0; i<N; i++){
    xt[i]=((i==0)?0:xt[i-1])-(N-i)*m*g/k;
  }
  for (int i=0; i<N; i++){
    x0[i][0]=cos(tet*PI/180)*xt[i];
    v0[i][0]=-sin(tet*PI/180)*xt[i]*omega;
  }
  for (int i=0; i<N; i++){
    x0[i][1]= sin(tet*PI/180)*xt[i];
    v0[i][1]=cos(tet*PI/180)*xt[i]*omega;
  }
}

void Init(){
  for (int d=0; d<D; d++)
    for (int i=0;i<N;i++){
      x[i][d]=x0[i][d];
      v[i][d]=v0[i][d];
    }
}
 
void Newton(){
  for (int i=0; i<ItNo;i++){
    Verlet(x,v,dt);
  }
}

void Draw(int xdim,int ydim){
  double hexp=fact*(N*(N-1)/2.*m*g/k);
  if (D==1)
    for (int i=0; i<N; i++){
      mycircle(1,xdim/2,ydim/2-x[i][0]/hexp*ydim,r);
    }
  else
    for (int i=0; i<N; i++){
      mycircle(1,xdim/2+x[i][1]/hexp*ydim,ydim/2-x[i][0]/hexp*ydim,r);
    }
}

int main(){
  int sstep=0,cont=0,done=0;
  Init();
  AddFreedraw("Oscillators",&Draw);
  //  DefineGraphN_R("x",&x[0],&ItNo,NULL);
  //  DefineGraphN_R("v",&v[0],&ItNo,NULL);
  StartMenu("Start",1);
  //  DefineGraph(curve2d_,"Graphics");
  DefineGraph(freedraw_,"Oscillators");
  DefineInt("ItNo",&ItNo);
  DefineDouble("dt",&dt);
  StartMenu("Init",0);
  DefineDouble("tet",&tet);
  DefineDouble("omega",&omega);
  DefineFunction("Rest",&setX0rest);
  /*  for (int d=0; d<D; d++){
    for (int i=0; i<N; i++)
      DefineDouble("x0",&x0[i][d]);
    for (int i=0; i<N; i++)
      DefineDouble("v0",&v0[i][d]);
      }*/
  DefineFunction("Init",&Init);
  EndMenu();
  DefineDouble("g",&g);
  DefineDouble("k",&k);
  DefineDouble("m",&m);
  DefineDouble("scale",&fact);
  DefineInt("radius",&r);
  DefineBool("sstep",&sstep);
  DefineBool("cont",&cont);
  DefineBool("done",&done);
  EndMenu();
  while (!done){
    Events(1);
    DrawGraphs();
    if (cont||sstep){
      sstep=0;
      Newton();
    }
    else sleep(1);
  }
}