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

#define sqr(a) ((a)*(a))
#define cube(a) ((a)*(a)*(a))

void pendulum3(double *y,double *dy,double P[3][3])
{
 /*P should be 3 x 3, each magnet position is a column*/
  int i;
  double d1,d2,d3; 
  double R,C;

  /*Constants R, C*/
  R=0.2;
  C=0.5;

  d1=sqrt(sqr(y[0]-P[0][0])+sqr(y[2]-P[1][0])+sqr(P[2][0]));
  d2=sqrt(sqr(y[0]-P[0][1])+sqr(y[2]-P[1][1])+sqr(P[2][1]));
  d3=sqrt(sqr(y[0]-P[0][2])+sqr(y[2]-P[1][2])+sqr(P[2][2]));
  d1=cube(d1);
  d2=cube(d2);
  d3=cube(d3);

  dy[0]=y[1];
  dy[1]=-R*y[1]-C*y[0]+(P[0][0]-y[0])/d1+(P[0][1]-y[0])/d2+(P[0][2]-y[0])/d3;
  dy[2]=y[3];
  dy[3]=-R*y[3]-C*y[2]+(P[1][0]-y[2])/d1+(P[1][1]-y[2])/d2+(P[1][2]-y[2])/d3;
 
}

int main(void)
{
	FILE *fp;
	double x[4],dx[4];    /*Position and Velocity*/
	double P[3][3];  /*Magnet Positions*/
	double h, k1[4],k2[4],k3[4],k4[4];  /*Variables for Runge-Kutta*/
	double temp[4],nx[4];  /*Temporary Storage*/
	double d1,dv,thresh1,thresh2;  /*Stopping criterion*/
	int X[200][200]; 
	double xmax,xmin,ymax,ymin,xwidth,ywidth;
	int i,j,k,numits,u,v;

thresh1=0.1;
thresh2=0.01;
/*Coordinate Translation constants*/
xmin=-2.0;
ymin=-2.0;
xwidth=4.0/200.0;
ywidth=4.0/200.0;

/*Magnet Positions*/
P[0][0]=1; P[0][1]=-0.5; P[0][2]=-0.5;
P[1][0]=0; P[1][1]=0.866; P[1][2]=-0.866;
P[2][0]=0.25; P[2][1]=0.25; P[2][2]=0.25;

/*Time step for diff eqn solution*/
h=0.08;
/*Clear the storage matrix*/
for(i=0;i<200;i++)
	for(j=0;j<200;j++) X[i][j]=0;


for (u=0;u<200;u++ )
{
	printf("On iterate %d\n",u);
    for(v=0;v<200;v++)
    {

/*Initial Pendulum values*/
x[0]=xmin+u*xwidth;  /*x-coord of pend*/
x[1]=0.0;  /*dx- value of pend*/
x[2]=ymin+v*ywidth;  /*y-coord of pend*/
x[3]=0.0;  /*dy-value of pend*/

dx[0]=0.0;dx[1]=0.0;dx[2]=0.0;dx[3]=0.0;

/*Start Main Program*/
numits=2000;   /*Max number of iterations*/
 
	for(k=0;k<numits;k++){

        pendulum3(x,dx,P);
        for(i=0;i<4;i++) k1[i]=h*dx[i];
        for(i=0;i<4;i++) temp[i]=x[i]+0.5*k1[i];
	    pendulum3(temp,dx,P);
	    for(i=0;i<4;i++) k2[i]=h*dx[i];
		for(i=0;i<4;i++) temp[i]=x[i]+0.5*k2[i];
        pendulum3(temp,dx,P);
		for(i=0;i<4;i++) k3[i]=h*dx[i];
		for(i=0;i<4;i++) temp[i]=x[i]+k3[i];
		pendulum3(temp,dx,P);
		for(i=0;i<4;i++) k4[i]=h*dx[i];
	    for(i=0;i<4;i++) nx[i]=x[i]+(1.0/6.0)*(k1[i]+2*k2[i]+2*k3[i]+k4[i]);

	    for(i=0;i<4;i++) x[i]=nx[i];
         /*Test to see if over a magnet*/
        
			for(i=0;i<3;i++){
			  d1=sqrt( sqr(P[0][i]-x[0])+sqr(P[1][i]-x[2]) );
			  dv=sqrt( sqr(x[1])+sqr(x[3]) );
			  if (d1 < thresh1 & dv < thresh2 )
			  {
				X[u][v]=20*i+1+(k%10);  /*or X[u][v]=i+1;*/
				k=numits;
			   }
			 }
	}/*End of numits loop*/

	}/*End of v loop*/
}/*End of u loop*/
printf("Completed the run.  Going to output\n");

        fp=fopen("pendulum.dat","w");
        for(i=0;i<200;i++){
                for(j=0;j<200;j++){
                fprintf(fp,"%d ",X[i][j]);
                if(j==199) fprintf(fp,"\n");
                }
        }
        fclose(fp);

	return 0;
}