//GridTest(0.0001,-1.00501,0.0431441)
// In new version of ROOT you need to comment out 
// the gSystem->Load("libMatrix");
// and symply enter it interactively before you do 
// .L JerryKahona.C

#ifndef __CINT__
#include "TMatrixD.h"
#include "TVectorD.h"
gSystem->Load("libMatrix");
#endif

// data arrays are global variables

Int_t NumPoints=0;
Float_t x[100],y[100],errory[100];
Double_t chiSq1,chiSq2,chiSq3;

//______________________________________________________________________________
Double_t func(Double_t x,Double_t y,Double_t *par)
{
  // Bevington Chapt 5 Fit function
  //  Double_t value=( par[0]+par[1]*exp((-1.0)*x/par[3])+par[2]*exp((-1.0)*x/par[4]));

  //Temp-vs-Voltage Fit function
  
  Double_t value=( par[0]+par[1]*x);

 return value;
}

//______________________________________________________________________________
Double_t CalcChiSq(Int_t npar, Double_t *par, Int_t iflag)
{
   Int_t i;

//calculate chisquare
   Double_t chisq = 0;
   Double_t delta;
   for (i=0;i<NumPoints; i++) {
     delta  = (y[i]-func(x[i],y[i],par))/errory[i];
     chisq += delta*delta;
   }
   if(iflag==1)
     {
       printf("x[%d]=%g\t y[%d]=%g\t func=%g\n",NumPoints-1,x[NumPoints-1],NumPoints-1,y[NumPoints-1],func(x[NumPoints-1],y[NumPoints-1],par));
       for(i=0;i<npar;i++)
	 printf("par[%d]=%g\n",i,par[i]);
       printf("chisq=%g\n",chisq);

     }     
   return(chisq);
}


void LoadDataArrays(Char_t *filename, Int_t Weight)
{
  ifstream in;

  NumPoints=0;

  in.open(filename);
  while(in.good() && NumPoints < 100){
    if(Weight==1.0)
      {
	in >> x[NumPoints] >> y[NumPoints] ;
	errory[NumPoints]=0.05;
      }
    else
      {
	in >> x[NumPoints] >> y[NumPoints] ;
	if(y[NumPoints])
	  errory[NumPoints]=sqrt(y[NumPoints]);
	else
	  errory[NumPoints]=y[NumPoints];
	if(errory[NumPoints]==0)
	  errory[NumPoints]=1.0;
      }
    
    //    printf("%d\t%g\t%g\n",NumPoints,x[NumPoints],y[NumPoints]);
    NumPoints++;
  }
  in.close();
}
void DataArrayPrint(void)
{
  if(NumPoints)
    {
      printf("__X__\t__Y__\tEror Y\n");
      for(Int_t i=0;i<NumPoints;i++)
      	 printf("%g\t%g\t%g\n",x[i],y[i],errory[i]);
    }
  else
    {
      printf("Data arrays are not filled\n execute function \n\tLoadDataArrays\n");
    }
}


void LinContour(Int_t steps, Float_t a1min, Float_t a1max, Float_t a2min, Float_t a2max))
{
  //  LinContour(100,-1.2,-0.9,0.042,.048) 
  Int_t nRowsCols=2;
  TH2D *ChiMin=new TH2D("ChiMin","ChiMin",steps,a1min,a1max,steps,a2min,a2max);

  if(NumPoints==0)
    {
      printf("Data arrays are not filled\n execute function \n\tLoadDataArrays\n");
      return;
    }
    

  // ALl the data is loaded in 

  //     Alpha()=-1.00503 +- 0.0210647   0.0431444 +- 0.000360375    
  //ChiSqrd = 43.4671
  
  Double_t ChiSqrd,Yfit,Alpha[100];
  Double_t a1Step=(a1max-a1min)/steps;
  Double_t a2Step=(a2max-a2min)/steps;

  //  printf("a2Step=%g\t a2Step=%g\n",a1Step,a2Step);


  //  Alpha[0]=-1.00503;
  //  Alpha[1]=0.043144;
  
  Alpha[1]=a2min;
  for(Int_t k=0;k<steps;k++)
    {
      Alpha[1]+=a2Step;
      Alpha[0]=a1min;
      for(Int_t l=0;l<steps;l++)
	{
      Alpha[0]+=a1Step;
      
      ChiSqrd=0;

      for(j=0;j<NumPoints;j++)
	{
	  Yfit=0;
	  for(m=0;m<nRowsCols;m++)
	    {
	      Yfit+=Alpha[m]*pow(x[j],m);
	    }
	  //      printf("y[j]-Yfit=%g-%g=%g\n",y[j],Yfit,y[j]-Yfit);
	  ChiSqrd+= (y[j]-Yfit)*(y[j]-Yfit)/errory[j]/errory[j];
	}
      //      printf("A1=%g\tA2=%g\tChiSqrd=%g\tnu=%d\n",Alpha[0],Alpha[1],ChiSqrd,NumPoints-nRowsCols);
      //      TH2D *ChiMin=new TH2D("ChiMin","ChiMin",steps,a1min,a1max,steps,a2min,a2max);
      ChiMin->Fill(Alpha[0],Alpha[1],ChiSqrd);
      
	}
    }  
  //  ChiMin->Draw();
  ChiMin->Draw("Surf1");

}


Double_t Chi2ParabMin(Double_t *x, Double_t *y, Int_t NumPnts)
{
  // Find the minimum of a prabola using 3 points on the parabola
  //To find the parabola you choose 3 points and form a system 
  // of 3-equations and 3 unkowns
  // using a*par[i]^2 + b*par[i] + c = \chi^2
  //then solve the system of equations for a,b,c
  // by inverting the matrix
  // this will give you a,b,c but not the minima
  //you need to set derivative to zero for critical point
  // giving
  //     X=-b/2/a = MIN
  gSystem->Load("libMatrix");
  
 const Int_t nRowsCols  = 3;

  TMatrixD AlphaMat(nRowsCols,nRowsCols);
  TMatrixD CorMat(nRowsCols,nRowsCols);

  Double_t Beta[5],Alpha[5];



  for(Int_t i=0;i<5;i++)
    {
      //      printf("x[%d]=%g\ty[%d]=%g\terrory[%d]=%g\n",i,x[i],i,y[i],i,errory[i]);
    }

  // Fill the Alpha Matrix
  //	x[i] , y[i] , DeltaY[i] are already set from above

  Int_t j,k,m;

  for(m=0;m<nRowsCols;m++)
    {
      Alpha[m]=0.;
      Beta[m]=0.;
    }



  for(j=0;j<5;j++)
    {
      for(m=0;m<nRowsCols;m++)
	{
	  Beta[m]+=y[j]*pow(x[j],m)/errory[j];
	  for(k=0;k<nRowsCols;k++)
	    AlphaMat(m,k)+=pow(x[j],m)*pow(x[j],k)/errory[j]/errory[j];
	}
    }

  /*
  printf("Beta()=");

  for(m=0;m<nRowsCols;m++)
    {
      printf("%g\t",Beta[m]);
    }
  printf("\n");
  */


  //  AlphaMat.Print();
  Double_t AlphaMatDet=AlphaMat.Determinant();
  //  cout << "Determinant=" <<  AlphaMatDet << endl;

  CorMat=AlphaMat.Invert();
  //  CorMat.Print();
  for(m=0;m<nRowsCols;m++)
    {

      for(k=0;k<nRowsCols;k++)
	{
	  Alpha[m]+=Beta[k]*CorMat(k,m);
	  //	  printf("Alpha[%d]+=%g\t%g\n",m,Beta[k],CorMat(k,m));
	}
    }
  /*
  printf("Regression Method:\n\tAlpha()=");

  for(m=0;m<nRowsCols;m++)
    {
      printf("%g +\- %g\t",Alpha[m],sqrt(CorMat(m,m)));
								   
    }
  printf("\n");

  */

  //setting derivative of parabla equaiton to zero and solve for X
  printf("Matrix method=%g\n",-Alpha[1]/2/Alpha[2]);  
  
  //from Bevingtons Equation 8.12 pg 147 2nd Edition
  Double_t FormulaMin;
  FormulaMin=(y[3]-y[2])/(y[3]-2*y[2]+y[1]);
  FormulaMin=x[3]-(x[2]-x[1])*(FormulaMin+0.5);
  printf("Formula=%g\n",FormulaMin);
  
  return(FormulaMin);
  
  

}


//______________________________________________________________________________
void GridLS(Int_t npar, Double_t *par, Double_t *parDelta)
{
  if(NumPoints==0)
    {
      printf("Data arrays are not filled\n execute function \n\tLoadDataArrays\n");
      return;
    }
  
  printf("%d Fit Parameters\n",npar);
  printf("ChiQRD=%g\n",CalcChiSq(npar, par, 0));

  Double_t ParabolaPntsX[5],ParabolaPntsY[5],NumParabolaPnts=5;
  Double_t delta;
  Double_t save, delta1, del1, del2, aa, bb, cc, disc, alpha, x1, x2;
  Int_t j;

  chiSq2 = CalcChiSq(npar, par, 0);
  // -find local minimum for each parameter- 
  //  for (j = 0; j < npar;j++)
  for (j = 0; j < npar;j++)
    {
      
      delta    = parDelta[j];
      par[j]   = par[j] + delta;
      chiSq3   = CalcChiSq(npar, par, 0);
      //      printf("par[%d]=%g\tchiSq2=%g\tchiSq3=%g\n",j,par[j],chiSq2,chiSq3);

      if (chiSq3 > chiSq2)  //started in wrong direction
	{ 
	  delta  = -delta;
	  par[j]   =  par[j] + delta;
	  save   =  chiSq2;    //interchange 2 and 3 so 3 is lower
	  chiSq2 =  chiSq3;
	  chiSq3 =  save;
	}
      
// -Increment or decrement a[j] until chi squared increases- 
      do
	{
	  chiSq1 = chiSq2; //move back to prepare for quad fit
	  chiSq2 = chiSq3;
	  par[j]   = par[j] + delta;
	  chiSq3 = CalcChiSq(npar, par, 0);
	  //	  printf("Parameter Steps: par[%d]=%g chiSq3=%g\n",j,par[j],chiSq3);
	}  while (chiSq3 < chiSq2);
      ParabolaPntsX[2]=par[j]; // value of parameter at chiSq3 for min
      ParabolaPntsY[2]=chiSq3;

      // par[j] and chiSq2 are estimates of the min chi^2
      // lets find 2 points on each side of the parabola
      // which are more than 2*chiSq2 large than chiSq2
      // so we can get a good parabola fit
      do
	{
	  par[j]= par[j] + delta;
	  chiSq3 = CalcChiSq(npar, par, 0);
	  //	  printf("par[%d]=%g chiSq3=%g\n",j,par[j],chiSq3);
	}  while (chiSq3 < 2*chiSq2);
      //	  printf("Right Side Parabola search: par[%d]=%g chiSq3=%g\n",j,par[j],chiSq3);
	  ParabolaPntsX[3]=par[j]; // value of parameter at chiSq3 for Right Max
	  ParabolaPntsY[3]=chiSq3;

      do
	{
	  par[j]= par[j] + delta;
	  chiSq3 = CalcChiSq(npar, par, 0);
	  //	  printf("par[%d]=%g chiSq3=%g\n",j,par[j],chiSq3);
	}  while (chiSq3 < 3.1*chiSq2);
      //	  printf("Right Side Parabola search: par[%d]=%g chiSq3=%g\n",j,par[j],chiSq3);
	  ParabolaPntsX[4]=par[j]; // value of parameter at chiSq3 for Right Max
	  ParabolaPntsY[4]=chiSq3;

	  par[j]= ParabolaPntsX[2];
      do
	{
	  par[j]= par[j] - delta;
	  chiSq1 = CalcChiSq(npar, par, 0);
	  //	  printf("par[%d]=%g chiSq1=%g\n",j,par[j],chiSq1);
	}  while (chiSq1 < 1.9*chiSq2);

      //	  printf("Left Side Parabola search: par[%d]=%g chiSq3=%g\n",j,par[j],chiSq3);
	  ParabolaPntsX[1]=par[j]; // value of parameter at chiSq1 for Left Max
	  ParabolaPntsY[1]=chiSq1;

      do
	{
	  par[j]= par[j] - delta;
	  chiSq1 = CalcChiSq(npar, par, 0);
	  //	  printf("par[%d]=%g chiSq1=%g\n",j,par[j],chiSq1);
	}  while (chiSq1 < 3.2*chiSq2);

      //	  printf("Left Side Parabola search: par[%d]=%g chiSq3=%g\n",j,par[j],chiSq3);
	  ParabolaPntsX[0]=par[j]; // value of parameter at chiSq1 for Left Max
	  ParabolaPntsY[0]=chiSq1;

	  par[j]= ParabolaPntsX[2];

	  printf("The five Chi^2 points for fitting min are:\n");
	  for(i=0;i<NumParabolaPnts;i++)
	    printf("\t (%g,%g)",ParabolaPntsX[i],ParabolaPntsY[i]);
	  printf("\n");

      //To find the parabola you choose 3 points and form a system of 3-equations and 3 unkowns
      // using a*par(i)^2 + b* par(i) + c = \chi^2
      //then solve the system of equations for a,b,c
      // use either method of determinants or invert matrix
      // this will give you a,b,c but not the minima
      //you need to set derivative to zero for critical point

      /*
      ParabolaPntsY[0]=chiSq1;
      ParabolaPntsY[1]=chiSq2;
      ParabolaPntsY[2]=chiSq3;
      */
	  par[j]=Chi2ParabMin(ParabolaPntsX,ParabolaPntsY,4);
	  /*
	{
	  printf("Problem:\n\t couldn't find parabola region for parameter %d\n",j);
	  printf("Expand step size of shift parameter\n");
	  printf("par[%d]=%g\tparStep=%g\n",j,par[j],parDelta[j]);

	}
	  */

    }
  

  return;


}

//______________________________________________________________________________
void PlotResults(Int_t npar, Double_t *Params)
{
   Float_t yfit[100];


  if(NumPoints==0)
    {
      printf("Data arrays are not filled\n execute function \n\tLoadDataArrays\n");
      return;
    }

   /*
   for(Int_t i=0;i<npar;i++)
     {
       printf("Params[%d]=%g\n",i,Params[i]);
     }
   */

   TGraph *gr = new TGraphErrors(NumPoints,x,y,0,errory);
   /*
   gr->SetTitle("Counts -vs- Time");
   gr->GetXaxis()->SetTitle("Time (s)");
   gr->GetYaxis()->SetTitle("Counts");
   */
   gr->SetTitle("Temp -vs- Voltage");
   gr->GetXaxis()->SetTitle("Temp (C)");
   gr->GetYaxis()->SetTitle("Voltage(mV)");
   gr->SetMarkerColor(4);
   gr->SetMarkerSize(0.3);
   gr->SetMarkerStyle(21);
   gr->Draw("AP");
   //   gr->Draw();
   
   //Fit results

   for(Int_t i=0;i<NumPoints;i++)
     {
       //       yfit[i]=func(x[i],y[i],Parameters);
       yfit[i]=func(x[i],y[i],Params);
       //       printf("x[%d]=%g\ty[%d]=%g\tyfit=%g\n",i,x[i],i,y[i],yfit[i]);
     }

         TGraph *grfit = new TGraph(NumPoints,x,yfit);
            grfit->Draw("C");

}

void GridTest(Double_t StepSize, Double_t Param1, Double_t Param2 )
{
   const Int_t NPar=2;
   Double_t Parameter[NPar],ParameterStepSize[NPar],InitChiSqr;
   //   Parameter[0]=-1.00503;
   //   Parameter[1]=0.043144;
   Parameter[0]=Param1;
   Parameter[1]=Param2;
   //   Parameter[2]=140.0;
   //   Parameter[3]=32.5;
   //   Parameter[4]=193.;

   ParameterStepSize[0]=StepSize;
   
   ParameterStepSize[1]=StepSize;

   InitChiSqr=CalcChiSq(NPar, Parameter, 0);

   GridLS(NPar, Parameter, ParameterStepSize);
   
   printf("GridSearch Results:\n");
   
   for(Int_t i=0;i<NPar;i++)
     printf("\tParameter[%d]=%g\n",i,Parameter[i]);
   printf("Initial ChiSQR=%g\n",InitChiSqr);
   
   printf("ChiSQR=%g\n",CalcChiSq(NPar, Parameter, 0));
   printf("Reduced ChiSQR=%g\n",CalcChiSq(NPar, Parameter, 0)/(NumPoints-NPar));

   PlotResults(NPar, Parameter);
}