#include <iostream>
#include <stdio.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_multiroots.h>
#include <ASlib.h>

// Set up parameters

struct rparams

{

  double a; // first x cord
  double b; // first y cord
  double e; // first z cord
  double a2; // second x cord
  double b2; // second y cord
  double e2; // second z cord
  double d2; // second's time dif
  double a3; // third x cord
  double b3; // third y cord
  double e3; // third z cord
  double d3; // third's time dif
  double a4; // fourth x cord
  double b4; // fourth y cord
  double e4; // fourth z cord
  double d4; // fourth's time dif

};

int spheres (const gsl_vector * x, void *params, gsl_vector * f) {

  // Parameters

  double a = ((struct rparams *) params)->a;
  double b = ((struct rparams *) params)->b;
  double e = ((struct rparams *) params)->e;
  double a2 = ((struct rparams *) params)->a2;
  double b2 = ((struct rparams *) params)->b2;
  double e2 = ((struct rparams *) params)->e2;
  double d2 = ((struct rparams *) params)->d2;
  double a3 = ((struct rparams *) params)->a3;
  double b3 = ((struct rparams *) params)->b3;
  double e3 = ((struct rparams *) params)->e3;
  double d3 = ((struct rparams *) params)->d3;
  double a4 = ((struct rparams *) params)->a4;
  double b4 = ((struct rparams *) params)->b4;
  double e4 = ((struct rparams *) params)->e4;
  double d4 = ((struct rparams *) params)->d4;

  // Variables x0=x  x1 = y  x2 = z  x3 = time(distance)

  const double x0 = gsl_vector_get (x,0);
  const double x1 = gsl_vector_get (x,1);
  const double x2 = gsl_vector_get (x,2);
  const double x3 = gsl_vector_get (x,3);

  // Equations for the solver based off x^2 + y^2 + z^2 = r^2

  const double y0 = SQ(x0 - a) + SQ(x1 - b) + SQ(x2 - e) - SQ(InvTimeExp(x3,x2,e));
  const double y1 = SQ(x0 - a2) + SQ(x1 - b2) + SQ(x2 - e2) - SQ(InvTimeExp(x3+d2,x2,e2));
  const double y2 = SQ(x0 - a3) + SQ(x1 - b3) + SQ(x2 - e3) - SQ(InvTimeExp(x3+d3,x2,e3));
  const double y3 = SQ(x0 - a4) + SQ(x1 - b4) + SQ(x2 - e4) - SQ(InvTimeExp(x3+d4,x2,e4));
  gsl_vector_set (f, 0, y0);
  gsl_vector_set (f, 1, y1);
  gsl_vector_set (f, 2, y2);
  gsl_vector_set (f, 3, y3);
  return GSL_SUCCESS;

}

void Sph() {
 
                         //  Antennas 1 thru 6  && Pulser labeled 7 
  //               **1**    **2**     **3**    **4**    **5**    **6**   **7**
  double sx[7]={  9.79,     9.19,       .03,   -9.53,   3.51,   -3.37,   .17};
  double sy[7]={  5.46,    -4.97,      10.2,   -4.68,  -1.81,   -1.88,   -30.01};
  double sz[7]={  -22.59, -25.61,    -27.07,   -24.8,   -.24,    -.29,   -17.61};
  //               **1**    **2**     **3**    **4**    **5**    **6**   **7**

  int HitOrder[4];
  int statuscheck[8];
  double d2,d3,d4;
  double xx[8];
  double yy[8];
  double zz[8];
  
  printf("Antenna Name      Coordinates                Number \n");
  printf("   V2         ( 9.79, 5.46, -22.59)            0 \n");
  printf("   V1         ( 9.19, -4.97, -25.61)           1 \n");
  printf("   V3         ( 0.03, 10.20, -27.07)           2 \n");
  printf("   V4         ( -9.53, -4.68, -24.80)          3 \n");
  printf("   V5         ( 3.51, -1.81, -0.24)            4 \n");
  printf("   V7         ( -3.37, -1.88, -0.29)           5 \n");

  cout << "Enter the number of the first antenna hit: ";
  cin >> HitOrder[0];
  cout << "Enter the number of the second antenna hit:";
  cin >> HitOrder[1];
  cout << "And now the time difference between the first and second:";
  cin >> d2;
  cout << "Enter the number of the third antenna hit:";
  cin >> HitOrder[2];
  cout << "And now the time difference between the first and third:";
  cin >> d3;
  cout << "Enter the number of the fourth antenna hit:";
  cin >> HitOrder[3];
  cout << "Finally enter the time difference between the first and fourth:";
  cin >> d4;
  
  // Initialization of solver
  // Uses the solver multiple times in order to
  // weed out potential extra erroneous solution 
	    
  for (int j=0;j<8;j++) {
    
    xx[j]=1000;
    yy[j]=1000;
    zz[j]=1000;
    statuscheck[j] = 0;
    
    const gsl_multiroot_fsolver_type *T;
    gsl_multiroot_fsolver *s;
    int status;
    size_t iter = 0;
    const size_t n = 4;
    struct rparams p = {sx[HitOrder[0]], sy[HitOrder[0]], sz[HitOrder[0]],
			sx[HitOrder[1]], sy[HitOrder[1]], sz[HitOrder[1]], d2,
			sx[HitOrder[2]], sy[HitOrder[2]], sz[HitOrder[2]], d3,
			sx[HitOrder[3]], sy[HitOrder[3]], sz[HitOrder[3]], d4};
    gsl_multiroot_function f = {&spheres, n, &p};
    
    double x_init[8][4] = {{100, 100, -100, 100},{100, -100, 100, -100},
			   {100, -100, -100, 100},{100, 100, -100, -100},
			   {-100, -100, -100, 100},{-100, 100, -100, -100},
			   {100, -100, -100, -100},{-100, -100, -100, -100}};
    
    gsl_vector *x = gsl_vector_alloc (n);
    gsl_vector_set (x, 0, x_init[j][0]);
    gsl_vector_set (x, 1, x_init[j][1]);
    gsl_vector_set (x, 2, x_init[j][2]);
    gsl_vector_set (x, 3, x_init[j][3]);
    T = gsl_multiroot_fsolver_hybrids;
    s = gsl_multiroot_fsolver_alloc (T, 4);
    gsl_multiroot_fsolver_set (s, &f, x);
    
    do
      {
	iter++;
	status = gsl_multiroot_fsolver_iterate (s);
	if (status)  
	  break;
	status = gsl_multiroot_test_residual (s->f, 1e-6);
      }
    while (status == GSL_CONTINUE && iter < 1000);
    
    status = gsl_multiroot_test_residual (s->f, 1e-6);
    xx[j] = gsl_vector_get (s->x, 0);
    yy[j] = gsl_vector_get (s->x, 1);
    zz[j] = gsl_vector_get (s->x, 2);
    //printf(" X = %.6f   Y = %.6f   Z = %.6f\n",xx[j],yy[j],zz[j]);
    
    if (status == GSL_SUCCESS && iter < 1000 && gsl_vector_get (s->x, 3) > 0){
      
      xx[j] = gsl_vector_get (s->x, 0);
      yy[j] = gsl_vector_get (s->x, 1);
      zz[j] = gsl_vector_get (s->x, 2);
      gsl_multiroot_fsolver_free (s);
      gsl_vector_free (x);
      statuscheck[j] = 1;
      
    }
  }
  
  // Determines which solution is closer to the first hit
  
  int soln1 = 0 ;
  int soln2 = 0;
  double xsoln[2] = {0,0};
  double ysoln[2] = {0,0};
  double zsoln[2] = {0,0};
  
  for (int j = 0; j<8; j++) {
    
    if (statuscheck[j]==1) {
      
      if (soln1==0) {
	
	xsoln[0] = xx[j];
	ysoln[0] = yy[j];
	zsoln[0] = zz[j];
	soln1++;
	continue;
	
      }
      
      if ( fabs(xx[j]-xsoln[0])<.01 && fabs(yy[j]-ysoln[0])<.01 && fabs(zz[j]-zsoln[0])<.01 ) soln1++;
		
      if ( !(fabs(xx[j]-xsoln[0])<.01) && !(fabs(yy[j]-ysoln[0])<.01) && !(fabs(zz[j]-zsoln[0])<.01) && soln2==0) {
	
	xsoln[1] = xx[j];
	ysoln[1] = yy[j];
	zsoln[1] = zz[j];
	soln2++;
	continue;
	
      }
      
      if ( fabs(xx[j]-xsoln[1])<.01 && fabs(yy[j]-ysoln[1])<.01 && fabs(zz[j]-zsoln[1])<.01 )soln2++;
		
    }
    
  }
	    
  if (soln1>soln2) {
    
    printf("X = %.6f \n",xsoln[0]);
    printf("Y = %.6f \n",ysoln[0]);
    printf("Z = %.6f \n",zsoln[0]);
    
  }
  
  if (soln2>soln1) {

    printf("X = %.6f \n",xsoln[1]);
    printf("Y = %.6f \n",ysoln[1]);
    printf("Z = %.6f \n",zsoln[1]);
    
  }  

  if (soln1==0 && soln2==0) printf("NO SOLUTION COULD BE FOUND \n");   

}