// 2015.08.24
// This code calculates Restcicted-3body problem
// with the modified Euler scheme.
#include<stdio.h>
#include<math.h>
#include<stdlib.h>
#define np 1000         //number of test particles
#define nfmax 20        //maximum number of orbital files
#define dt 0.01         //timestep
#define TTOTAL 100      //totaltime
//#define Me 3.0e-6     //mass of earth
#define Me 1.0e-2       //mass of jupiter
#define Ms 1.0          //mass of sun
#define Tr 1.0          //orbital period
// constant timestep
// 2 dimension
double forcex(double xtmp,double ytmp,double vxtmp,double vytmp);
double forcey(double xtmp,double ytmp,double vxtmp,double vytmp);
double m1,m2,mu1,mu2,mubar,rs,re;

int main(void)
{
  FILE *fout[nfmax],*foutall,*fout2body,*foutlag;
  double t,x[np][2],v[np][2];
  double xcom[np][2],vcom[np][2];//計算用配列
  double f[np][2],xl[5][2],alpha,beta;
  char name1[100];
  int i,n,nf;

  /***** output *****/
  printf("np=%d,nfmax=%d,np/nfmax=%d\n",np,nfmax,np/nfmax);
  for(nf=0 ; nf<nfmax ; nf++){
    n=np/nfmax*nf+10;
    sprintf(name1,"orbit_n%06d.dat",n);
    if((fout[nf]=fopen(name1,"w"))==NULL){
      printf("YOU CANNOT OPEN %s FOR WRITING!!!\n",name1);
      exit(1);
    }    
  }
  if((foutall=fopen("orbitall.dat","w"))==NULL){
    printf("YOU CANNOT OPEN orbitall.dat FOR WRITING!!!\n");
    exit(1);
  }

  if((fout2body=fopen("2body.dat","w"))==NULL){
    printf("YOU CANNOT OPEN 2body.dat FOR WRITING!!!\n");
    exit(1);
  }

  if((foutlag=fopen("lagrange.dat","w"))==NULL){
    printf("YOU CANNOT OPEN largrange.dat FOR WRITING!!!\n");
    exit(1);
  }

  //parameter
  mubar=Me/(Ms+Me);
  mu1=1.-mubar;
  mu2=mubar;
  rs=mubar;//distance of earth from origin(xg)
  re=1.-rs;   //distance of sun from origin
  printf("mubar=%e mu1=%e mu2=%e,\n",mubar,mu1,mu2);
  fprintf(fout2body,"%e %e\n",-rs,0.);    
  fprintf(fout2body,"%e %e\n",re,0.);    
  fclose(fout2body);

  //Lagrange points
  for(n=0 ; n<5 ; n++){
    for(i=0 ; i<=1 ; i++){
      xl[n][i]=0.;
    }
  }
  alpha=pow(mu2/3./mu1,1./3.);
  xl[0][0]=re-(alpha-1./3.*alpha*alpha-1./9.*alpha*alpha*alpha
	       -23./81.*alpha*alpha*alpha*alpha);//l1
  xl[1][0]=re+(alpha+1./3.*alpha*alpha-1./9.*alpha*alpha*alpha
	       -31./81.*alpha*alpha*alpha*alpha);//l2
  beta=-7./12.*(mu2/mu1)+7./12.*(mu2*mu2/mu1/mu1)
    -13223./20736.*(mu2*mu2*mu2/mu1/mu1/mu1);
  xl[2][0]=-(1+beta+rs);//l3
  xl[3][0]=1./2.-mu2;//l4
  xl[3][1]=sqrt(3.)/2.;
  xl[4][0]=1./2.-mu2;//l5
  xl[4][1]=-sqrt(3.)/2.;
  for(n=0 ; n<5 ; n++)fprintf(foutlag,"%e %e\n",xl[n][0],xl[n][1]);    
  fclose(foutlag);

  //Initialization
  t=0.0;
  for(n=0 ; n<np ; n++){
    for(i=0 ; i<=1 ; i++){
      x[n][i]=0.0;
      v[n][i]=0.0;
      xcom[n][i]=0.;
      vcom[n][i]=0.;
    }
  }
   
  //around all lagrange points
  /*
  for(i=0 ; i<=1 ; i++){
    for(n=0      ; n<np/5   ; n++)x[n][i]=xl[0][i]-0.001*n;
    for(n=np/5   ; n<np/5*2 ; n++)x[n][i]=xl[1][i]-0.001*(n-np/5);
    for(n=np/5*2 ; n<np/5*3 ; n++)x[n][i]=xl[2][i]-0.001*(n-np/5*2);
    for(n=np/5*3 ; n<np/5*4 ; n++)x[n][i]=xl[3][i]-0.001*(n-np/5*3);
    for(n=np/5*4 ; n<np     ; n++)x[n][i]=xl[4][i]-0.001*(n-np/5*4);    
  }
  */
  for(i=0 ; i<=1 ; i++){
    for(n=0       ; n<np/10   ; n++)x[n][i]=xl[0][i]-0.001*n;
    for(n=np/10   ; n<np/10*2 ; n++)x[n][i]=xl[0][i]+0.001*(n-np/10);
    for(n=np/10*2 ; n<np/10*3 ; n++)x[n][i]=xl[1][i]-0.001*(n-np/10*2);
    for(n=np/10*3 ; n<np/10*4 ; n++)x[n][i]=xl[1][i]+0.001*(n-np/10*3);
    for(n=np/10*4 ; n<np/10*5 ; n++)x[n][i]=xl[2][i]-0.001*(n-np/10*4);
    for(n=np/10*5 ; n<np/10*6 ; n++)x[n][i]=xl[2][i]+0.001*(n-np/10*5);    
    for(n=np/10*6 ; n<np/10*7 ; n++)x[n][i]=xl[3][i]-0.001*(n-np/10*6);
    for(n=np/10*7 ; n<np/10*8 ; n++)x[n][i]=xl[3][i]+0.001*(n-np/10*7);    
    for(n=np/10*8 ; n<np/10*9 ; n++)x[n][i]=xl[4][i]-0.001*(n-np/10*8);
    for(n=np/10*9 ; n<np      ; n++)x[n][i]=xl[4][i]+0.001*(n-np/10*9);    
  }
  if(t<dt){
    for(n=0 ; n<np ; n++){
      printf("t=%lf n=%d x=%lf y=%lf\n",t,n,x[n][0],x[n][1]);
    }  
  }

  /*
  // around L5
  for(i=0 ; i<=1 ; i++){
    for(n=0    ; n<np/2 ; n++) x[n][i]=xl[4][i]-0.01*n;
    for(n=np/2 ; n<np   ; n++) x[n][i]=xl[4][i]+0.01*n;            
  }
  */

  //Time evolution with the modified Euler scheme.
  for(t=0.0 ; t<=TTOTAL ; t=t+dt){
    for(n=0 ; n<np ; n++){
      f[n][0]=forcex(x[n][0],x[n][1],v[n][0],v[n][1]);
      f[n][1]=forcey(x[n][0],x[n][1],v[n][0],v[n][1]);
    }
    nf=0;
    for(n=0 ; n<np ; n++){
      for(i=0 ; i<=1 ; i++){
	vcom[n][i]=v[n][i]+dt*f[n][i];
	xcom[n][i]=x[n][i]+dt*vcom[n][i];
      }
      v[n][0]=v[n][0]
	+0.5*dt*(forcex(x[n][0],x[n][1],v[n][0],v[n][1])
		 +forcex(xcom[n][0],xcom[n][1],vcom[n][0],vcom[n][1]));
      v[n][1]=v[n][1]
	+0.5*dt*(forcey(x[n][0],x[n][1],v[n][0],v[n][1])
		 +forcey(xcom[n][0],xcom[n][1],vcom[n][0],vcom[n][1]));
      for(i=0 ; i<=1 ; i++)x[n][i]=x[n][i]+dt*v[n][i]; 
      if(n==np/nfmax*nf+10){
	printf("nf=%d n=%d %e %e\n",nf,n,x[n][0],x[n][1]);
	fprintf(fout[nf],"%e %e\n",x[n][0],x[n][1]);
	nf++;
	if(nf>nfmax){
	  printf("nf is larger than nfmax!\n");
	  printf("nf=%d nfmax=%d\n",nf,nfmax);
	  exit(1);
	}
      }
    }
  }
  for(n=0 ; n<np ; n++){
    fprintf(foutall,"%e %e %e\n",t,x[n][0],x[n][1]);
  }
  fclose(foutall);  
  for(nf=0 ; nf<nfmax ; nf++)fclose(fout[nf]);  
  return 0;
}

double forcex(double xtmp,double ytmp,double vxtmp,double vytmp){
  double fx,r1,r2;
  r1=sqrt((xtmp+rs)*(xtmp+rs)+ytmp*ytmp);//from sun
  r2=sqrt((xtmp-re)*(xtmp-re)+ytmp*ytmp);//from earth
  fx= 2/Tr*vytmp+1/Tr/Tr*xtmp
    -(mu1/r1/r1/r1*(xtmp+mu2)+mu2/r2/r2/r2*(xtmp-mu1));
  return(fx);
}

double forcey(double xtmp,double ytmp,double vxtmp,double vytmp){
  double fy,r1,r2;
  r1=sqrt((xtmp+rs)*(xtmp+rs)+ytmp*ytmp);//from sun
  r2=sqrt((xtmp-re)*(xtmp-re)+ytmp*ytmp);//from earth
  fy=-2/Tr*vxtmp+1/Tr/Tr*ytmp
    -(mu1/r1/r1/r1+mu2/r2/r2/r2)*ytmp;
  return(fy);
}