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

float xtalkb(float ts);
float gatti(float x,float h,float xtalk,float *qq);
float chisqr_mat(int iflag,float x,float xa,float ts,float *d);
float chifit_gatti2(float xa,float ts,float *err,float *d);

float ht;
float h;

float chifit_gatti2(float xa,float ts,float *err,float *d){
int i;
float xn,xnh,xnl;
float step;
int ifin,istep;
float chi2last,chi2min,chi2,chi2t,chi2l,chi2h,chi20;
float chi2t1,chi2t2,chi2t3;
float x,xl,xh,x0;
float errl,errh;
float erl,erh;
 int u[3]={1,1,1};float xtalk;float qq[3];float n1,n2,n3;float qerr;float syserr=0.0;

  chi2=chisqr_mat(0,0.0,xa,ts,d);
  ifin=0;
  istep=0;
  chi2min=1.0e12;
  chi2last=1.0e12;
  x=-0.5;
  step=0.5;
NEXT:
  istep=istep+1;
  if(istep>10000)goto FIN;
  
  chi2=chisqr_mat(1,x,xa,ts,d);

      //  printf(" x chi2 step  %f %f %f %f %f %f \n",x,chi2,n1,n2,n3,step);
  if(chi2<chi2last){
    chi2last=chi2;
    if(chi2min>chi2)chi2min=chi2;
    x=x+step;
    goto NEXT;
  }
  if(step<0.0001){ifin=1;goto FIN;}
  x=x-2*step;
  step=step/2.;
  chi2last=1.0e12;
  goto NEXT;
FIN:
  x=-x;
  /* now calculate error */
  x0=-x;
  errl=.001;
  errh=.001;
  for(i=0;i<3;i++){
  xl=-x-errl;
  xh=-x+errh;
  chi2l=chisqr_mat(1,xl,xa,ts,d);
  chi2l=chi2l-chi2;
  chi2h=chisqr_mat(1,xh,xa,ts,d);
  chi2h=chi2h-chi2;
  if(chi2l>0.0){errl=errl*sqrt(1.0/chi2l);}else{errl=1000.;}
  if(chi2h>0.0){errh=errh*sqrt(1.0/chi2h);}else{errh=1000.;}
  // printf(" i errl %d %f \n",i,errl);
  }
  err[0]=errl;
  err[1]=errh;
  err[2]=chi2;  

  return x;
}

float chisqr_mat(int iflag,float x,float xa,float ts,float *d)
{
 float chi2;
float n1,n2,n3;
float xtalkl,xtalk,xtalkh;
float qql[3],qq[3],qqh[3];
float a11,a12,a13,a22,a23,a33;
float a11t,a12t,a13t,a22t,a23t,a33t;
float v11l,v12l,v13l,v22l,v23l,v33l;
float v11,v12,v13,v22,v23,v33;
float v11r,v12r,v13r,v22r,v23r,v33r;
float dd;
float sn11,sn12,sn13,sn21,sn22,sn23,sn31,sn32,sn33;
float sd11,sd12,sd13,sd21,sd22,sd23,sd31,sd32,sd33;
float sn1,sn2,sn3;
float syserr=.008;
// float syserr=0.0;
float vchk11,vchk12,vchk13,vchk22,vchk23,vchk33;
float chk1,chk2,chk3;

 if(iflag==0){
 // first calculate error matrices for left(l) middle() and right(r) strips
  a11=16.35;a12=8.81;a13=4.85;a22=13.96;a23=6.93;a33=12.36;
  // a11=16.35;a12=0.0;a13=0.0;a22=13.96;a23=0.0;a33=12.36;
  a11t=a11+syserr*syserr*d[0]*d[0];
  a12t=a12+syserr*syserr*d[0]*d[3];
  a13t=a13+syserr*syserr*d[0]*d[6];
  a22t=a22+syserr*syserr*d[3]*d[3];
  a23t=a23+syserr*syserr*d[3]*d[6];
  a33t=a33+syserr*syserr*d[6]*d[6];

  dd=(a11t*a33t*a22t-a11t*a23t*a23t-a33t*a12t*a12t+2.0*a12t*a13t*
a23t-a13t*a13t*a22t);
  v11l = (a33t*a22t-a23t*a23t)/dd;
  v12l = -(a33t*a12t-a13t*a23t)/dd;
  v13l = (a12t*a23t-a13t*a22t)/dd;
  v22l = (a33t*a11t-a13t*a13t)/dd;
  v23l = -(a23t*a11t-a12t*a13t)/dd;
  v33l = (a22t*a11t-a12t*a12t)/dd;

  /*  vchk11=a11t*v11l+a12t*v12l+a13t*v13l;
  vchk12=a12t*v11l+a22t*v12l+a23t*v13l;
  vchk13=a13t*v11l+a23t*v12l+a33t*v13l;
  vchk22=a12t*v12l+a22t*v22l+a23t*v23l;
  vchk23=a13t*v12l+a23t*v22l+a33t*v23l;
  vchk33=a13t*v13l+a23t*v23l+a33t*v33l;
  printf(" vchk11 %f %f %f %f %f %f \n",vchk11,vchk12,vchk13,vchk22,vchk23,vchk33);   */

  a11t=a11+syserr*syserr*d[1]*d[1];
  a12t=a12+syserr*syserr*d[1]*d[4];
  a13t=a13+syserr*syserr*d[1]*d[7];
  a22t=a22+syserr*syserr*d[4]*d[4];
  a23t=a23+syserr*syserr*d[4]*d[7];
  a33t=a33+syserr*syserr*d[7]*d[7];
  dd=(a11t*a33t*a22t-a11t*a23t*a23t-a33t*a12t*a12t+2.0*a12t*a13t*
a23t-a13t*a13t*a22t);
  v11 = (a33t*a22t-a23t*a23t)/dd;
  v12 = -(a33t*a12t-a13t*a23t)/dd;
  v13 = (a12t*a23t-a13t*a22t)/dd;
  v22 = (a33t*a11t-a13t*a13t)/dd;
  v23 = -(a23t*a11t-a12t*a13t)/dd;
  v33 = (a22t*a11t-a12t*a12t)/dd;
  a11t=a11+syserr*syserr*d[2]*d[2];
  a12t=a12+syserr*syserr*d[2]*d[5];
  a13t=a13+syserr*syserr*d[2]*d[8];
  a22t=a22+syserr*syserr*d[5]*d[5];
  a23t=a23+syserr*syserr*d[5]*d[8];
  a33t=a33+syserr*syserr*d[8]*d[8];
  dd=(a11t*a33t*a22t-a11t*a23t*a23t-a33t*a12t*a12t+2.0*a12t*a13t*
a23t-a13t*a13t*a22t);
  v11r = (a33t*a22t-a23t*a23t)/dd;
  v12r = -(a33t*a12t-a13t*a23t)/dd;
  v13r = (a12t*a23t-a13t*a22t)/dd;
  v22r = (a33t*a11t-a13t*a13t)/dd;
  v23r = -(a23t*a11t-a12t*a13t)/dd;
  v33r = (a22t*a11t-a12t*a12t)/dd;
  return 0;
 }
 if(iflag==1){
    h=ht/(.970+xa*(1.58-.970)/64.);
    xtalkl=xtalkb(ts-50);
    gatti(x,h,xtalkl,qql);
    xtalk=xtalkb(ts);
    gatti(x,h,xtalk,qq);
    xtalkh=xtalkb(ts+50);
    gatti(x,h,xtalkh,qqh);

    /* first minimize n1,n2,n3 the number of events in left, middle and right stps*/

    sn11=v11l*qql[0]*d[0]+    v11*qql[1]*d[1]+   v11r*qql[2]*d[2];
    sd11=v11l*qql[0]*qql[0]+  v11*qql[1]*qql[1]+ v11r*qql[2]*qql[2];
    sn12=v12l*qql[0]*d[3]+    v12*qql[1]*d[4]+   v12r*qql[2]*d[5];
    sd12=v12l*qql[0]*qq[0]+   v12*qql[1]*qq[1]+  v12r*qql[2]*qq[2];
    sn13=v13l*qql[0]*d[6]+    v13*qql[1]*d[7]+   v13r*qql[2]*d[8];
    sd13=v13l*qql[0]*qqh[0]+  v13*qql[1]*qqh[1]+ v13r*qql[2]*qqh[2];

    sn21=v12l*qq[0]*d[0]+     v12*qq[1]*d[1]+    v12r*qq[2]*d[2];
    sd21=v12l*qq[0]*qql[0]+   v12*qq[1]*qql[1]+  v12r*qq[2]*qql[2];
    sn22=v22l*qq[0]*d[3]+     v22*qq[1]*d[4]+    v22r*qq[2]*d[5];
    sd22=v22l*qq[0]*qq[0]+    v22*qq[1]*qq[1]+   v22r*qq[2]*qq[2];
    sn23=v23l*qq[0]*d[6]+     v23*qq[1]*d[7]+    v23r*qq[2]*d[8];
    sd23=v23l*qq[0]*qqh[0]+   v23*qq[1]*qqh[1]+  v23r*qq[2]*qqh[2];

    sn31=v13l*qqh[0]*d[0]+    v13*qqh[1]*d[1]+   v13r*qqh[2]*d[2];
    sd31=v13l*qqh[0]*qql[0]+  v13*qqh[1]*qql[1]+ v13r*qqh[2]*qql[2];
    sn32=v23l*qqh[0]*d[3]+    v23*qqh[1]*d[4]+   v23r*qqh[2]*d[5];
    sd32=v23l*qqh[0]*qq[0]+   v23*qqh[1]*qq[1]+  v23r*qqh[2]*qq[2];
    sn33=v33l*qqh[0]*d[6]+    v33*qqh[1]*d[7]+   v33r*qqh[2]*d[8];
    sd33=v33l*qqh[0]*qqh[0]+  v33*qqh[1]*qqh[1]+ v33r*qqh[2]*qqh[2];

    // printf(" sn11 %f %f %f \n",sn11,sn12,sn13);
    // printf(" sn21 %f %f %f \n",sn21,sn22,sn23);
    // printf(" sn31 %f %f %f \n",sn31,sn32,sn33);

    sn1=sn11+sn12+sn13;
    sn2=sn21+sn22+sn23;
    sn3=sn31+sn32+sn33;

/* soln to:
      sd11*N1+sd21*N2+sd31*N3-sn1=0
      sd12*N1+sd22*N2+sd32*N3-sn2=0
      sd13*N1+sd23*N2+sd33*N3-sn3=0
   code generated by maple
*/

      dd=(-sd11*sd23*sd32+sd11*sd22*sd33+sd13*sd21*sd32-sd22*sd13*sd31+sd23*sd12*sd31-sd12*sd21*sd33);
      n1 = (-sd21*sd33*sn2+sd21*sn3*sd32-sd31*sd22*sn3+sd31*sd23*sn2-sn1*sd23*sd32+sn1*sd22*sd33)/dd;
      n2 = -(-sd11*sd33*sn2+sd11*sn3*sd32+sd13*sd31*sn2+sd33*sd12*sn1-sn3*sd12*sd31-sd13*sn1*sd32)/dd;
      n3 = (-sd22*sd13*sn1+sd11*sd22*sn3-sd11*sd23*sn2+sd23*sd12*sn1+sd13*sd21*sn2-sd12*sd21*sn3)/dd;
      /*
      chk1= sd11*n1+sd21*n2+sd31*n3;
      chk2= sd12*n1+sd22*n2+sd32*n3;
      chk3= sd13*n1+sd23*n2+sd33*n3;
      printf("chk1 %f %f %f \n",sn1,chk1,n1);
      printf("chk2 %f %f %f \n",sn2,chk2,n2);
      printf("chk3 %f %f %f \n",sn3,chk3,n3);
      */

      /* now calculate chi-square */
      chi2=0.0;
      chi2=chi2+v11l*(d[0]-n1*qql[0])*(d[0]-n1*qql[0])+
                v11*(d[1]-n1*qql[1])*(d[1]-n1*qql[1])+
                v11r*(d[2]-n1*qql[2])*(d[2]-n1*qql[2]);
      chi2=chi2+2.*(
                v12l*(d[0]-n1*qql[0])*(d[3]-n2*qq[0])+
                v12*(d[1]-n1*qql[1])*(d[4]-n2*qq[1])+
                v12r*(d[2]-n1*qql[2])*(d[5]-n2*qq[2]));
      chi2=chi2+2.*(
                v13l*(d[0]-n1*qql[0])*(d[6]-n3*qqh[0])+
                v13*(d[1]-n1*qql[1])*(d[7]-n3*qqh[1])+
                v13r*(d[2]-n1*qql[2])*(d[8]-n3*qqh[2]));
      chi2=chi2+v22l*(d[3]-n2*qq[0])*(d[3]-n2*qq[0])+
                v22*(d[4]-n2*qq[1])*(d[4]-n2*qq[1])+
                v22r*(d[5]-n2*qq[2])*(d[5]-n2*qq[2]);
      chi2=chi2+2.*(
                v23l*(d[3]-n2*qq[0])*(d[6]-n3*qqh[0])+
                v23*(d[4]-n2*qq[1])*(d[7]-n3*qqh[1])+
                v23r*(d[5]-n2*qq[2])*(d[8]-n3*qqh[2])); 
      chi2=chi2+v33l*(d[6]-n3*qqh[0])*(d[6]-n3*qqh[0])+
                v33*(d[7]-n3*qqh[1])*(d[7]-n3*qqh[1])+
                v33r*(d[8]-n3*qqh[2])*(d[8]-n3*qqh[2]);
      return chi2;
 }
}