/*----------------------------------------------------------------*/
/*  Routines which find the Shower Detector Plane Normal Vector.
    Version 11-00.
    Brian Fick and Paul Sommers */

#include    <stdio.h>
#include    <stdlib.h>
#include    <math.h>
#include    <string.h>
#include    "shwaxis.h"
#include    "PlaneFit.h"

float       cen[3], top[3] ;

void GuessPlane(void) ;
void DoPlaneFit(void) ; 
void PlaneFit(void) ;

/*----------------------------------------------------------------*/
void DoPlaneFit(void){
/* Makes the interface between the global data structures and
   the local data used by the fitting routines. */

  int i, j ;

/* Isolate the variables to be used by the fitting routine */
  npix = event.ntb ;
  for(i=0;i<event.ntb;++i) {
     pix[i].pint = event.pixel[i].pint ;
     for(j=0;j<3;++j){
        pix[i].dir[j] = event.pixel[i].dir[j] ;
     }
  }

/* Call the fitting routine. */
  PlaneFit() ;

/* Write the results back to the main data structure */
  for(j=0;j<3;++j){
     event.pln[j] = pln[j] ;
  }
}

/*----------------------------------------------------------------*/
void PlaneFit(void)
{
  int         i, j ;
  float       a, b, da, db, dot ;
  float	      x[3][2];
  Vertex      v[3];

  int iters_normal_a ;
  extern  float pln[3] ; 
  
  GuessPlane() ;

  /* Find centroid vector projected perpendicular to guessed normal */
  for(i=0,cen[0]=0,cen[1]=0,cen[2]=0;i<3;++i) {
    for(j=0;j<3;++j) {
      cen[j] += pix[i].dir[j] ;
    }
  }
  dot = dot_product(cen,pln) ;
  for (i=0;i<3;++i) {
    cen[i] -= dot*pln[i] ;
  }
  v_norm(cen) ;

  /* Get a third unit vector top = cen x event.pln */
  cross_norm(cen,pln,top) ;

  /* Now fit for two parameters that give the true normal vector.  Define:
     a:=v[0].coord[0] in direction cen shifts track parallel to itself.
     b:=v[0].coord[1] in direction top rotates track about its centroid. */

  da = 0.1 ;
  db = 0.1 ;
  
  /* Corners of initial simplex */
  x[0][0] =  0 ;       x[0][1] =  0 ;       
  x[1][0] =  da ;      x[1][1] =  0 ;           
  x[2][0] =  0  ;      x[2][1] =  db ; 

  for (i=0;i<3;++i) {
    v[i].coord = x[i];
    v[i].fval = FunNormal(x[i]) ;
  }  

  amoeba(v, 2, FunNormal, 0.001, 500, 0, &iters_normal_a) ; 
  for (i=0,a=0,b=0;i<3;++i) {
    a += *v[i].coord/3 ;
    b += *(v[i].coord+1)/3 ;
  }

  /* Start amoeba again with this as the best guess

  da = 0.05 ;
  db = 0.05 ;
  
  /* Corners of initial simplex */
  x[0][0] =  a ;       x[0][1] =  b ;       
  x[1][0] =  a+da ;      x[1][1] =  b ;           
  x[2][0] =  a  ;      x[2][1] =  b+db ; 

  for (i=0;i<3;++i) {
    v[i].coord = x[i];
    v[i].fval = FunNormal(x[i]) ;
  }  

  amoeba(v, 2, FunNormal, 0.001, 500, 0, &iters_normal_a) ; 
  for (i=0,a=0,b=0;i<3;++i) {
    a += *v[i].coord/3 ;
    b += *(v[i].coord+1)/3 ;
  }

  /* Output the plane normal vector */
  pln[0] += a*cen[0]+b*top[0] ;
  pln[1] += a*cen[1]+b*top[1] ;
  pln[2] += a*cen[2]+b*top[2] ;
  v_norm(pln) ;
}


/*----------------------------------------------------------------*/
void GuessPlane(void)
{
  /* Find cross product of pixel pairs (one pixel from the first  
       half of the tubes and the other from the second half).   
     Then find the cross product that is most like the others. */ 

  int i, j ;
  float       sum, summax ;
  float       vec[MAXTUBES][3] ;

  for (i=0;i<event.ntb/2;++i) { 
    j=i+(event.ntb+1)/2 ; 
    cross_norm(pix[i].dir,pix[j].dir,vec[i]); 
  }
  
  for (i=0,summax=-1e6;i<event.ntb/2;++i) {
    for (j=0,sum=0;j<event.ntb/2;++j) { 
      sum += dot_product(vec[i],vec[j]) ;
    }
    if (sum > summax) {
      for (j=0;j<3;++j) {
	pln[j] = vec[i][j] ;
      }
      summax = sum ;
    }
  }
}

/*----------------------------------------------------------------*/
/* Figure of merit function for best plane fit */
/* The two components of *xp are coefficients for directions cen and top */

float FunNormal(float *xp)
{ 

  int                   i ;
  float              pl[3] ;
  float              adif ;
  float              wt ;
  float              val ; 

  val = 0.0 ;
  wt = 0.0 ;
  
  pl[0] = pln[0]+*xp*cen[0]+*(xp+1)*top[0] ;
  pl[1] = pln[1]+*xp*cen[1]+*(xp+1)*top[1] ;
  pl[2] = pln[2]+*xp*cen[2]+*(xp+1)*top[2] ;
  v_norm(pl) ;
  
  for (i=0; i<event.ntb; ++i) {
    adif = (pix[i].dir[0]*pl[0]
	    + pix[i].dir[1]*pl[1]
	    + pix[i].dir[2]*pl[2] ) ; 
    val += adif*adif*pix[i].pint ;
    wt  += pix[i].pint ;
  }

  val /= wt ;
  return(-(float)val) ;  /* Sign change for maximizing version of amoeba */
  
}