//input units are meters and nanoseconds
#include <iostream>
#include <stdio.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_multiroots.h>
#include "ASlib.h"
#include "TMath.h"
//#include "/home/dbesson/zvars.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 cmpns=0.2997055;
  /*
  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));
  */
  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 Vtx4Hit(Bool_t LDeBug, Float_t HitTime[16], Float_t xRx[16], Float_t yRx[16], Float_t zRx[16], Bool_t LUseAnt[16], Float_t &r, Float_t &phi, Float_t &the, Float_t &dphi, Float_t &dthe, Int_t &ncombinations) {                         //  Antennas 1 thru 6  && Pulser labeled 7 
  r=-99999; phi=-99999; the=-99999; dphi=-99999; dthe=-99999;
  const Float_t rindex=1./1.78;
  const Int_t MSOLN=2048;
  Float_t rV[MSOLN],phiV[MSOLN],theV[MSOLN],xV[MSOLN],yV[MSOLN],zV[MSOLN],xbar,ybar,zbar;
  const Int_t MBINPHI=361;
  const Int_t MBINTHE=181;
  Float_t phiArr[MBINPHI]={0}, theArr[MBINTHE]={0};
  //vector<float>rV;
  //vector<float>phiV;
  //vector<float>theV;


  //               **1**    **2**     **3**    **4**    **5**    **6**   **7**
  //  double sx0[7]={  9.79,     9.19,       .03,   -9.53,   3.51,   -3.37,   .17};
  //  double sy0[7]={  5.46,    -4.97,      10.2,   -4.68,  -1.81,   -1.88,   -30.01};
  //  double sz0[7]={  -22.59, -25.61,    -27.07,   -24.8,   -.24,    -.29,   -17.61};
  //               **1**    **2**     **3**    **4**    **5**    **6**   **7**
  double sx[7], sy[7], sz[7];
  //  int HitOrder[4];
  int statuscheck[8];
  double d2=0;
  double d3=0;
  double d4=0;
  double xx[8]; //solution
  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");
  */

  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}};
  

  const gsl_multiroot_fsolver_type *T;
  gsl_multiroot_fsolver *s;
  int status;
  Int_t isoln=0;
  r=-999999; phi=-9999999; the=-999999;
  const Int_t MRXMX=16; 
  Float_t RxHitTime[16]={-9999999};  Int_t index[16]={-1}; //sorting doesn't actually seem to improve vertexing
  //  r=-1; phi=-1; dphi=-1; the=-1; dthe-1;
  for(Int_t irx0=0; irx0<MRXMX; irx0++){
    if(!LUseAnt[irx0])continue;
    RxHitTime[0]=HitTime[irx0];
    for(Int_t irx1=irx0+1; irx1<MRXMX; irx1++){
      if(!LUseAnt[irx1])continue;
      RxHitTime[1]=HitTime[irx1];
      for(Int_t irx2=irx1+1; irx2<MRXMX; irx2++){
	if(!LUseAnt[irx2])continue;
	RxHitTime[2]=HitTime[irx2];
	for(Int_t irx3=irx2+1; irx3<MRXMX; irx3++){
	  if(!LUseAnt[irx3])continue;
	  RxHitTime[3]=HitTime[irx3];
	  TMath::Sort(4,RxHitTime,index,false); //low->high
	  /*
	  HitOrder[0]=index[0]; //channel with earliest hit
	  HitOrder[1]=index[1]; 
	  HitOrder[2]=index[2];
	  HitOrder[3]=index[3];
	  */

	  sx[0]=xRx[irx0]; sy[0]=yRx[irx0]; sz[0]=zRx[irx0];
	  sx[1]=xRx[irx1]; sy[1]=yRx[irx1]; sz[1]=zRx[irx1];
	  sx[2]=xRx[irx2]; sy[2]=yRx[irx2]; sz[2]=zRx[irx2];
	  sx[3]=xRx[irx3]; sy[3]=yRx[irx3]; sz[3]=zRx[irx3];
	  if(sx[3]==0)printf("!!sx[%d]=0!!\n",irx3);
	  d2=rindex*(HitTime[irx1]-HitTime[irx0]);
	  d3=rindex*(HitTime[irx2]-HitTime[irx0]);
	  d4=rindex*(HitTime[irx3]-HitTime[irx0]);
	  //	  d2*=-1; d3*=-1; d4*=-1;
	  /*
	  sx[0]=sx0[0]; sy[0]=sy0[0]; sz[0]=sz0[0];
	  sx[1]=sx0[1]; sy[1]=sy0[1]; sz[1]=sz0[1];
	  sx[2]=sx0[2]; sy[2]=sy0[2]; sz[2]=sz0[2];
	  sx[3]=sx0[3]; sy[3]=sy0[3]; sz[3]=sz0[3];
	  d2=10;
	  d3=20;
	  d4=30;
	  */
	  if(LDeBug)printf("sz[0]=zRx[%d]=%g / d2=HitTime[%d]-HitTime[%d]=%g d3=%g d4=%g\n",index[0],sz[0],index[0],index[1],d2,d3,d4);

	  for (int j=0;j<8;j++) {    
	    xx[j]=1000;
	    yy[j]=1000;
	    zz[j]=1000;
	    statuscheck[j] = 0;
	    
	    size_t iter = 0;
	    const size_t n = 4;
	    struct rparams p = {sx[0], sy[0], sz[0], 
				sx[1], sy[1], sz[1], d2,
				sx[2], sy[2], sz[2], d3,
				sx[3], sy[3], sz[3], d4};
	    gsl_multiroot_function f = {&spheres, n, &p};
	    if(LDeBug)printf("vertexing with sx[0]=%g sy=%g sz=%g / sx[1]=%g sy=%g sz=%g / sx[2]=%g sy=%g sz=%g / sx[3]=%g sy=%g sz=%g d2=%g d3=%g d4=%g\n",sx[0],sy[0],sz[0],sx[1],sy[1],sz[1],sx[2],sy[2],sz[2],sx[3],sy[3],sz[3],d2,d3,d4);
	    
	    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);
	    if(LDeBug)printf("j=%d X = %.6f   Y = %.6f   Z = %.6f\n",j,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;
	    }
	    gsl_multiroot_fsolver_free (s);
	    gsl_vector_free (x);
	  }
  
	  // 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++;
		if(LDeBug)printf("soln1: x=%g y=%g z=%g\n",xx[j],yy[j],zz[j]);
		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++;
		if(LDeBug)printf("soln2: x=%g y=%g z=%g\n",xx[j],yy[j],zz[j]);
		continue;	
	      }
	      if ( fabs(xx[j]-xsoln[1])<.01 && fabs(yy[j]-ysoln[1])<.01 && fabs(zz[j]-zsoln[1])<.01 )soln2++;		
	    }
	  }
	  if(isoln==MSOLN){
	    cout<<"\nisoln==MSOLN in Vtx4Hit/breaking!\n"<<endl;
	    break;
	  }
	  if (soln1>soln2) {    
	    if(LDeBug){
	      printf("X = %.6f \n",xsoln[0]);
	      printf("Y = %.6f \n",ysoln[0]);
	      printf("Z = %.6f \n",zsoln[0]);
	    }
	    r=sqrt(xsoln[0]*xsoln[0] + ysoln[0]*ysoln[0] + zsoln[0]*zsoln[0]);
	    the = acos(zsoln[0]/r)*TMath::RadToDeg();
	    phi = atan2(ysoln[0],xsoln[0])*TMath::RadToDeg();
	    phiArr[int(phi+180)]++;
	    xV[isoln]=xsoln[0]/r;
	    yV[isoln]=ysoln[0]/r;
	    zV[isoln]=zsoln[0]/r;
	    rV[isoln]=r; //.push_back(r);
	    phiV[isoln]=phi; //.push_back(phi);
	    theV[isoln]=the; //.push_back(the);
	    if(the>=0&&the<=180)theArr[int(the)]++;
	    else cout<<"ERRZ Fit4Spher the="<<the<<" OOB!"<<endl;
	    isoln++;
	    //	    Cart2Spher(xsoln[0],ysoln[0],zsoln[0],r,phi,the);
	  }
	  
	  if (soln2>soln1) {
	    if(LDeBug){
	      printf("X = %.6f \n",xsoln[1]);
	      printf("Y = %.6f \n",ysoln[1]);
	      printf("Z = %.6f \n",zsoln[1]);
	    }
	    r=sqrt(xsoln[1]*xsoln[1] + ysoln[1]*ysoln[1] + zsoln[1]*zsoln[1]);
	    the = acos(zsoln[1]/r)*TMath::RadToDeg();
	    phi = atan2(ysoln[1],xsoln[1])*TMath::RadToDeg();
	    phiArr[int(phi+180)]++;
	    xV[isoln]=xsoln[1]/r;
	    yV[isoln]=ysoln[1]/r;
	    zV[isoln]=zsoln[1]/r;
	    rV[isoln]=r; //.push_back(r);
	    phiV[isoln]=phi; //.push_back(phi);
	    theV[isoln]=the; //.push_back(the);
	    if(the>=0&&the<=180)theArr[int(the)]++;
	    else cout<<"ERRZ Fit4Spher the="<<the<<" OOB!"<<endl;
	    isoln++;
	    //	    Cart2Spher(xsoln[0],ysoln[0],zsoln[0],r,phi,the);
	    //	    rV.push_back(r);
	    //	    phiV.push_back(phi);
	    //	    theV.push_back(the);
	    //	    Cart2Spher(xsoln[1],ysoln[1],zsoln[1],r,phi,the);
	  }  
	  if (soln1==0 && soln2==0 && LDeBug) printf("NO SOLUTION COULD BE FOUND \n");   
	}
      }
    }
  }
  if(isoln==0)return;
    
  xbar=TMath::Mean(isoln,xV);
  ybar=TMath::Mean(isoln,yV);
  zbar=TMath::Mean(isoln,zV);
  if(zbar>1||ybar>1||xbar>1)printf("Norm error in Vtx4Hit! xbar=%g / ybar=%g / zbar=%g\n",xbar,ybar,zbar);

  r=TMath::Mean(isoln,rV);
  phi=atan2(ybar,xbar)*TMath::RadToDeg();
  //  if(phi<-180)phi+=180;  if(phi>180)phi-=180;
  the=acos(zbar)*TMath::RadToDeg();

  dphi=TMath::RMS(isoln,phiV);
  dthe=TMath::RMS(isoln,theV);
  Float_t phiAvg=TMath::Mean(isoln,phiV);
  //  phiAvg*=TMath::RadToDeg();
  Float_t theAvg=TMath::Mean(isoln,theV);
  //  theAvg*=TMath::RadToDeg();
  Int_t phiPkVal=TMath::LocMax(MBINPHI,phiArr);
  Int_t thePkVal=TMath::LocMax(MBINTHE,theArr);
  phiPkVal-=180;
  printf("Spher4HitFit isoln=%d phi=%g/phiPkVal=%d/<phi>=%g/dphi=%g  the=%g/thePkVal=%d/<the>=%g/dthe=%g\n",isoln,phi,phiPkVal,phiAvg,dphi,the,thePkVal,theAvg,dthe);
  ncombinations=isoln;
}