// Final Links, Tim Jones, Drexel University
// Use at your own risk.  Beaware of self-crossing crossing.
// Run with command line:
// ./final_links orbit_1_file orbit_2_file

#include <stdio.h>
#include <stdlib.h>
#include <assert.h>
#include <math.h>
#include <error.h>
#include <errno.h>
#include <iostream>
#include <fstream>
#include "vect.c"
#include "mmatrix.c"
#include "mreal.c"
using namespace std;

static inline double sqr(const double x) { return x*x; }


static inline double distxy(const double x1, const double y1, const double x2, const double y2) { return sqrt( sqr(x1-x2) + sqr(y1-y2)); }

static inline double distxyz(const double x1, const double y1, const double x2, const double y2, const double z1, const double z2) { return sqrt( sqr(x1-x2) + sqr(y1-y2) + sqr(z1-z2)); }

int main(int argc, char *argv[])
{

  //Get data as input1,input2
  double **input1;
  double **input2;
  input1=mmatrix(1,3);
  input2=mmatrix(1,3);

  double **match2;
  match2=mmatrix(1,7);

  double junk1;
  ifstream inFile1;
  ifstream inFile2;
  inFile1.open(argv[1]);
  inFile2.open(argv[2]);
  
  inFile1.precision(15);
  inFile2.precision(15);

  int count1i=0;
  int count2i=0;
  int matchcount=0;
  double max1=0;
  double max2=0;
  double min1=1000;
  double min2=1000;
  double min,max;

  //Load the files
  
  while( inFile1>>input1[count1i][0]>>input1[count1i][1]>>input1[count1i][2]>>junk1)
    {
      if(count1i>0)
	{
	 if(distxy(input1[count1i-1][0],input1[count1i-1][1],input1[count1i][0],input1[count1i][1])>max1)
	   max1=distxy(input1[count1i-1][0],input1[count1i-1][1],input1[count1i][0],input1[count1i][1]);
	 if(distxy(input1[count1i-1][0],input1[count1i-1][1],input1[count1i][0],input1[count1i][1])<min1)
	   min1=distxy(input1[count1i-1][0],input1[count1i-1][1],input1[count1i][0],input1[count1i][1]);
	}

    count1i++;
    input1=mreal(input1,count1i,3,count1i+1);
    }

  while( inFile2>>input2[count2i][0]>>input2[count2i][1]>>input2[count2i][2]>>junk1)
    {
      if(count2i>0)
	{
	 if(distxy(input2[count2i-1][0],input2[count2i-1][1],input2[count2i][0],input2[count2i][1])>max2)
	   max2=distxy(input2[count2i-1][0],input2[count2i-1][1],input2[count2i][0],input2[count2i][1]);
	 if(distxy(input2[count2i-1][0],input2[count2i-1][1],input2[count2i][0],input2[count2i][1])<min2)
	   min2=distxy(input2[count2i-1][0],input2[count2i-1][1],input2[count2i][0],input2[count2i][1]);
	 
	}
    count2i++;
    input2=mreal(input2,count2i,3,count2i+1);
    }

  
  //don't really need min but keep it anyway

  // max is the largest possible distance, given these two sets, that a point 
  // could be from the next point in the set...thus are worst case scenario:
  if(max2>max1){max = max2;}else{max=max1;}
  if(min2<min1){min = min2;}else{min=min1;}
  // forward box...follow the trajectory with a forward box of size max X max

  // printf("Max: %f\n",max);  

  bool flagfound;
  double adist;
  int counter=0;

  // New module:  Forward looking...project point[i][xyz] -> point[i+1][xyz]
  // and form a line from such; do the same for other curve; if these lines
  // cross, then we have a crossing to record.

  double xt,yt;
  double tmin, xmin, ymin, xmin2,ymin2;
  double alpha1,alpha2,alpha3, beta1, beta2, beta3,t,ot;
  bool flagmin;
 
  for(int i=0; i< count1i-1; i++){
    tmin=1.0;
  
  for(int j=0; j< count2i-2; j+=2){
      
    alpha1 = (input1[i+1][0] - input1[i][0]);
    alpha2 = (input2[j+2][0] - input2[j][0]);
    alpha3 = (input1[i][0] - input2[j][0]);

    beta1 = (input1[i+1][1] - input1[i][1]);
    beta2 = (input2[j+2][1] - input2[j][1]);
    beta3 = (input1[i][1] - input2[j][1]);

    t = ( alpha3 - alpha2*(beta3/beta2) ) / (alpha1 - alpha2*(beta1/beta2));
    ot = beta1*t/beta2 + beta3/beta2;
  

    adist=distxy(input1[i][0],input1[i][1],input2[j][0],input2[j][1]);

    // printf("%f %f %f %f\n", input1[i][0],input1[i+1][0],t,ot);
    // if(fabs(t) < 2 && fabs(ot) < 2)printf("%f %f %f %f\n", input2[j][0],input2[j+1][0],t,ot);
	

    if(t < tmin && t > 0 && ot < tmin && ot > 0 ) 
      {
	//	printf("%f %f %f %f\n", input1[i][0],input1[i+1][0],t,ot);
	//	printf("%f %f %f %f\n", input2[j][0],input2[j+1][0],t,adist);
	
	            
		  
		    for(int k=0; k<3; k++){
		    match2[counter][k]=input1[i][k];
		    match2[counter][k+3]=input2[j][k];}
		    match2[counter][6]=adist;
		    counter++;
		    match2=mreal(match2,counter,7,counter+1);
		    j+=1;
		    i+=1;
		
      }
  
     }
  
	
  }
     
    
	  

  //////////////////////////////////////////
  // count the links
  /////////////////////////////////////////

  int n=3;
  double x=0,*y,*dydx,*yout;
  y = vect(n);  dydx = vect(n);  yout = vect(n);
  double a,b,c;
  b=2.0; c=4.0;  a=0.556;
  double h=0.001;
  int H,L;
  double HANGLE,LANGLE;
  int LINK;
  int LINKTOTAL=0;

  FILE *fp;
  fp=fopen("found_crossings.dat","w");
  bool firsthigh;


 for(int j=0; j<counter; j++)
      {
		
	 
		if(match2[j][2]>match2[j][5])firsthigh=true; else firsthigh=false;
		
		////// LABEL FOR LINKING NUMBERS  
 
		 y[1]=  match2[j][0];
                 y[2]=  match2[j][1];
		 y[3]=  match2[j][2];

                for(int i=0; i< count1i-1; i++){
		  if(input1[i][0]==y[1] && input1[i][1]==y[2] && input1[i][2]==y[3])
		    { 
		      yout[1]= input1[i+1][0];
		      yout[2]= input1[i+1][1];
		      yout[3]= input1[i+1][2];
		    }
		}

		 // derivsy(x,y,dydx,a,b,c);
		 //		 runga4(y,dydx,n,x,h,yout,derivsy,a,b,c);
		 //		 x+=h;

		 if(firsthigh==true)
		   {
		     if( (yout[1]-y[1])>0 && (yout[2]-y[2])>0){ H=1;}
		     if( (yout[1]-y[1])<0 && (yout[2]-y[2])>0){ H=2;}
		     if( (yout[1]-y[1])<0 && (yout[2]-y[2])<0){ H=3;}
		     if( (yout[1]-y[1])>0 && (yout[2]-y[2])<0){ H=4;}
		     HANGLE=atan(fabs(yout[2]-y[2])/fabs(yout[1]-y[1]));
		   }

		 if(firsthigh==false)
		   {
                     if( (yout[1]-y[1])>0 && (yout[2]-y[2])>0){ L=1;}
		     if( (yout[1]-y[1])<0 && (yout[2]-y[2])>0){ L=2;}
		     if( (yout[1]-y[1])<0 && (yout[2]-y[2])<0){ L=3;}
		     if( (yout[1]-y[1])>0 && (yout[2]-y[2])<0){ L=4;}
         	     LANGLE=atan(fabs(yout[2]-y[2])/fabs(yout[1]-y[1]));
		   }
		   
                ////// LABEL FOR LINKING NUMBERS set two
		// x=0;
		 y[1]=  match2[j][3];
                 y[2]=  match2[j][4];
		 y[3]=  match2[j][5];

		 //		 derivsy(x,y,dydx,a,b,c);
		 //		 runga4(y,dydx,n,x,h,yout,derivsy,a,b,c);
		 //		 x+=h;
		 
		  for(int i=0; i< count2i-1; i++){
		  if(input2[i][0]==y[1] && input2[i][1]==y[2] && input2[i][2]==y[3])
		    { 
		      yout[1]= input2[i+1][0];
		      yout[2]= input2[i+1][1];
		      yout[3]= input2[i+1][2];
		    }
		}


		 if(firsthigh==false)
		   {
		     if( (yout[1]-y[1])>0 && (yout[2]-y[2])>0){ H=1;}
		     if( (yout[1]-y[1])<0 && (yout[2]-y[2])>0){ H=2;}
		     if( (yout[1]-y[1])<0 && (yout[2]-y[2])<0){ H=3;}
		     if( (yout[1]-y[1])>0 && (yout[2]-y[2])<0){ H=4;}
		     HANGLE=atan(fabs(yout[2]-y[2])/fabs(yout[1]-y[1]));
		   }

		 if(firsthigh==true)
		   {
                     if( (yout[1]-y[1])>0 && (yout[2]-y[2])>0){ L=1;}
		     if( (yout[1]-y[1])<0 && (yout[2]-y[2])>0){ L=2;}
		     if( (yout[1]-y[1])<0 && (yout[2]-y[2])<0){ L=3;}
		     if( (yout[1]-y[1])>0 && (yout[2]-y[2])<0){ L=4;}
		     LANGLE=atan(fabs(yout[2]-y[2])/fabs(yout[1]-y[1]));
		   }
		 
		 if((H-L)==-3){LINK=-1;}
		 if((H-L)==-2)
		   {
		     if( (HANGLE-LANGLE) > 0 && H==2) LINK=-1;
		     if( (HANGLE-LANGLE) > 0 && H==4) LINK=-1;
		     if( (HANGLE-LANGLE) > 0 && H==1) LINK=1;
		     if( (HANGLE-LANGLE) > 0 && H==3) LINK=1;

		     if( (HANGLE-LANGLE) < 0 && H==2) LINK=1;
		     if( (HANGLE-LANGLE) < 0 && H==4) LINK=1;
		     if( (HANGLE-LANGLE) < 0 && H==1) LINK=-1;
		     if( (HANGLE-LANGLE) < 0 && H==3) LINK=-1;
		   }
		 if((H-L)==-1){LINK=1;}
		 if((H-L)==0)
		   {
		     if( (HANGLE-LANGLE) > 0 && H==2) LINK=1;
		     if( (HANGLE-LANGLE) > 0 && H==4) LINK=1;
		     if( (HANGLE-LANGLE) > 0 && H==1) LINK=-1;
		     if( (HANGLE-LANGLE) > 0 && H==3) LINK=-1;

		     if( (HANGLE-LANGLE) < 0 && H==2) LINK=-1;
		     if( (HANGLE-LANGLE) < 0 && H==4) LINK=-1;
		     if( (HANGLE-LANGLE) < 0 && H==1) LINK=1;
		     if( (HANGLE-LANGLE) < 0 && H==3) LINK=1;
		     		     
		   }
		 if((H-L)==1){LINK=-1;}
		 if((H-L)==2)
		   {
                     if( (HANGLE-LANGLE) > 0 && H==2) LINK=-1;
		     if( (HANGLE-LANGLE) > 0 && H==4) LINK=-1;
		     if( (HANGLE-LANGLE) > 0 && H==1) LINK=1;
		     if( (HANGLE-LANGLE) > 0 && H==3) LINK=1;

		     if( (HANGLE-LANGLE) < 0 && H==2) LINK=1;
		     if( (HANGLE-LANGLE) < 0 && H==4) LINK=1;
		     if( (HANGLE-LANGLE) < 0 && H==1) LINK=-1;
		     if( (HANGLE-LANGLE) < 0 && H==3) LINK=-1;
		   }
		 if((H-L)==3){LINK=1;}
		 
	
		 fprintf(fp,"%f %f %f %i\n", match2[j][0], match2[j][1], match2[j][2], LINK);
	
		 fprintf(fp,"%f %f %f %i\n", match2[j][3], match2[j][4], match2[j][5], LINK); 
		 
	
		   LINKTOTAL+=LINK;

	
	
      }
    
    printf("%i\n",LINKTOTAL/2);
    
for (int j1 = 0; j1 < counter; j1++){free(match2[j1]);}
free(match2);



}