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


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

double x[N][D],v[N][D]; // state of the system
double dt=0.01,G=1,m[N],x0[N][D],v0[N][D];
int ItNo=1;
int iterations=0, lastit=-1;
int r=10;
double tet=0,omega=0,fact=0.3;

// data for the traces
#define LTmax 1000
int c[N];
int LT=LTmax;
double trace[N][LTmax][D];

void FF(double F[N][D],double x[N][D]){
  for (int i=0; i<N; i++)
    for (int d=0; d<D; d++)
      F[i][d]=0;
  for (int i=0; i<N; i++)
    for (int j=i+1; j<N; j++){
      double dx[D];
      for (int d=0; d<N; d++) dx[d]=x[j][d]-x[i][d];
      double dx2=0;
      for (int d=0; d<D; d++)
	dx2+=dx[d]*dx[d];
      double Fabs=G*m[i]*m[j]/pow(dx2,1.5);
      for (int d=0; d<D; d++){
	F[i][d]+=Fabs*dx[d];
	F[j][d]-=Fabs*dx[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[i]*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[i]*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];
  if (N!=2) printf("Fix initial conditions for N!=2/n");
  if (D!=2) printf("Fix initial conditions for D!=2/n");

  c[0]=2;
  c[1]=4;
  m[0]=1;
  x0[0][0]=-1;
  x0[0][1]=0;
  m[1]=1;
  x0[1][0]=1;
  x0[1][1]=0;

  v0[0][0]=0;
  v0[0][1]=sqrt(G*m[1]/4);
  v0[1][0]=0;
  v0[1][1]=-sqrt(G*m[0]/4);
}

void Init(){
  LT=0;
  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);
  }
  iterations++;
}

void AddTrace(double x[N][D]){
  if (LT<LTmax){
    LT++;
  }
  else
    for (int i=0; i<N; i++)
      memmove(&trace[i][0][0],&trace[i][1][0],(LT-1)*D*sizeof(double));
  for (int i=0; i<N; i++)
    for (int d=0; d<D; d++)
      trace[i][LT-1][d]=x[i][d];
}

void Draw(int xdim,int ydim){
  for (int i=0; i<N; i++){
    mycircle(c[i],xdim/2+x[i][0]*fact*ydim,ydim/2-x[i][1]*fact*ydim,r);
  }
}

void Drawtrace(int xdim,int ydim){
  // This should only be done if there are new data...
  if (lastit!=iterations) AddTrace(x);
  lastit=iterations;
  XPoint linetrace[LT];
  for (int i=0; i<N; i++){
    for (int t=0;t<LT; t++){
      linetrace[t].x=xdim/2+trace[i][t][0]*fact*ydim;
      linetrace[t].y=xdim/2-trace[i][t][1]*fact*ydim;
    }
    myline_polygon(c[i],linetrace,LT);
  }
}

int main(){
  int sstep=0,cont=0,done=0;
  Init();
  AddFreedraw("Planets",&Draw);
  AddFreedraw("Traces",&Drawtrace);
  StartMenu("Start",1);
  DefineGraph(freedraw_,"Oscillators");
  DefineInt("ItNo",&ItNo);
  DefineDouble("dt",&dt);
  StartMenu("Init",0);
  DefineDouble("tet",&tet);
  DefineDouble("omega",&omega);
  DefineFunction("Rest",&setX0rest);
  for (int i=0; i<N; i++){
    DefineDouble("m",&m[i]);
    for (int d=0; d<D; d++){
      DefineDouble("x0",&x0[i][d]);
      DefineDouble("v0",&v0[i][d]);
    }
  }
  DefineFunction("Init",&Init);
  EndMenu();
  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);
  }
}