#include <iostream>
#include <iomanip>
#include <ap_int.h>
#include <ap_fixed.h>
#include <math.h>
using namespace std;
const int obbits=16;
const int obnumb=8;
// input is bin data 1-32, binerror 1-32, inital fit parameter R0-kb as input, outout fit parameter R0-kb
int fw_binned(ap_ufixed<obbits,obnumb> aob1, ap_ufixed<obbits,obnumb> aob2, ap_ufixed<obbits,obnumb> aob3, ap_ufixed<obbits,obnumb> aob4, ap_ufixed<obbits,obnumb> aob5, ap_ufixed<obbits,obnumb> aob6, ap_ufixed<obbits,obnumb> aob7, ap_ufixed<obbits,obnumb> aob8, ap_ufixed<obbits,obnumb> aob9, ap_ufixed<obbits,obnumb> aob10, ap_ufixed<obbits,obnumb> aob11, ap_ufixed<obbits,obnumb> aob12, ap_ufixed<obbits,obnumb> aob13, ap_ufixed<obbits,obnumb> aob14, ap_ufixed<obbits,obnumb> aob15, ap_ufixed<obbits,obnumb> aob16, ap_ufixed<obbits,obnumb> aob17, ap_ufixed<obbits,obnumb> aob18, ap_ufixed<obbits,obnumb> aob19, ap_ufixed<obbits,obnumb> aob20, ap_ufixed<obbits,obnumb> aob21, ap_ufixed<obbits,obnumb> aob22, ap_ufixed<obbits,obnumb> aob23, ap_ufixed<obbits,obnumb> aob24, ap_ufixed<obbits,obnumb> aob25, ap_ufixed<obbits,obnumb> aob26, ap_ufixed<obbits,obnumb> aob27, ap_ufixed<obbits,obnumb> aob28, ap_ufixed<obbits,obnumb> aob29, ap_ufixed<obbits,obnumb> aob30, ap_ufixed<obbits,obnumb> aob31, ap_ufixed<obbits,obnumb> aob32,
	      ap_ufixed<obbits,obnumb> aobe1, ap_ufixed<obbits,obnumb> aobe2, ap_ufixed<obbits,obnumb> aobe3, ap_ufixed<obbits,obnumb> aobe4, ap_ufixed<obbits,obnumb> aobe5, ap_ufixed<obbits,obnumb> aobe6, ap_ufixed<obbits,obnumb> aobe7, ap_ufixed<obbits,obnumb> aobe8, ap_ufixed<obbits,obnumb> aobe9, ap_ufixed<obbits,obnumb> aobe10, ap_ufixed<obbits,obnumb> aobe11, ap_ufixed<obbits,obnumb> aobe12, ap_ufixed<obbits,obnumb> aobe13, ap_ufixed<obbits,obnumb> aobe14, ap_ufixed<obbits,obnumb> aobe15, ap_ufixed<obbits,obnumb> aobe16, ap_ufixed<obbits,obnumb> aobe17, ap_ufixed<obbits,obnumb> aobe18, ap_ufixed<obbits,obnumb> aobe19, ap_ufixed<obbits,obnumb> aobe20, ap_ufixed<obbits,obnumb> aobe21, ap_ufixed<obbits,obnumb> aobe22, ap_ufixed<obbits,obnumb> aobe23, ap_ufixed<obbits,obnumb> aobe24, ap_ufixed<obbits,obnumb> aobe25, ap_ufixed<obbits,obnumb> aobe26, ap_ufixed<obbits,obnumb> aobe27, ap_ufixed<obbits,obnumb> aobe28, ap_ufixed<obbits,obnumb> aobe29, ap_ufixed<obbits,obnumb> aobe30, ap_ufixed<obbits,obnumb> aobe31, ap_ufixed<obbits,obnumb> aobe32,
	      ap_ufixed<obbits,obnumb> inR0, ap_ufixed<obbits,obnumb> insigma, ap_ufixed<obbits,obnumb> inphi0, ap_ufixed<obbits,obnumb> inkb, 
	      ap_ufixed<obbits,obnumb> *outR0, ap_ufixed<obbits,obnumb> *outsigma, ap_ufixed<obbits,obnumb> *outphi0, ap_ufixed<obbits,obnumb> *outkb 
	      ){


  //  *outR0=inR0; *outsigma=insigma; *outphi0=inphi0; *outkb=inkb;
  float aob[32]={aob1, aob2, aob3, aob4, aob5, aob6, aob7, aob8, aob9, 
		 aob10, aob11, aob12, aob13, aob14, aob15, aob16, aob17, aob18, aob19, 
		 aob20, aob21, aob22, aob23, aob24, aob25, aob26, aob27, aob28, aob29, 
		 aob30, aob31, aob32};
  float aobe[32]={aobe1, aobe2, aobe3, aobe4, aobe5, aobe6, aobe7, aobe8, aobe9, 
		  aobe10, aobe11, aobe12, aobe13, aobe14, aobe15, aobe16, aobe17, aobe18, aobe19, 
		  aobe20, aobe21, aobe22, aobe23, aobe24, aobe25, aobe26, aobe27, aobe28, aobe29, 
		  aobe30, aobe31, aobe32};

  //int fw_binned(float *aob, float *aobe, float &R0, float &sigma, float &phi0, float &birks, float initR0){
  int my_precision=16; // NODIF IN 6 DIG for integ1 //def
  int fw_nintegbin=4; // def But dont use calc by hand
  float fw_precision = 1e-4; // a parameter for numerical integration, reduce to speed up calculation

  int fw_nbin=32; //just number of bin of data 
  //HHHH LUT gen HHHHH//
  // hist infor e.g. layer widht of detector
  float bins_lut[33]=
    { 0,  2.90158,  5.7519,  8.78677,  11.6986,  14.5387,  17.5428,  20.4444,
      23.3152,  26.3193,  29.2209,  32.1122,  35.1061,  38.0179,  40.899,  43.8723,
      46.7739,  49.6755,  52.8437, 55.6837,  58.5033,  61.4561,  64.3885,  67.2798,
      70.2224,  73.1445,  75.9538,  78.6606,  81.3879,  84.4125,  87.109,  89.785,
      92.4508};
  // Calc from bin width divided by # of division to average in a bin for expected 
  float binw_lut[32]=
    { 0.580316, 0.570064, 0.606974, 0.582366, 0.56802, 0.60082, 0.58032, 0.57416,
      0.60082, 0.58032, 0.57826, 0.59878, 0.58236, 0.57622, 0.59466, 0.58032,  0.58032,
      0.63364, 0.568, 0.56392, 0.59056, 0.58648, 0.57826, 0.58852, 0.58442, 0.56186,
      0.54136, 0.54546, 0.60492, 0.5393, 0.5352, 0.53316};


  //HHHH Minimize parameters HHHH//
  int debug_lv=0;
  float rx=0;
  int max_test=128;// high enough
  //  int max_test=64;   //mid 32 also minimize almost
  //  int max_test=32;   //min //temp to compile...
  //HHHHH parameter min max setting
  float cR0=inR0; //current val
  float rR0=20; //range
  float minR0=inR0; //current R0 of minimum -2logL

  float csigma=insigma; //current val
  float rsigma=1; //range
  float minsigma=2; //current sigma of minimum -2logL

  float cphi0=inphi0; //current val *1e-7
  float rphi0=20; //range
  float minphi0=20; //current phi0 of minimum -2logL

  float cbirks=inkb; //current val
  float rbirks=0.03; //range
  float minbirks=0.07; //current birks of minimum -2logL

  int tlv=0, stagelv=0, paran=0;
  float minLogL=9999999;
  int npar=4;
  for(int t=0; t<max_test; t++){
    //HHHH Stepping HHHH//
    tlv++;
    paran=(tlv/2)%npar;
    stagelv=(tlv-2)/(npar*2);
    float staw=exp(stagelv*log(2)); //pow(2,stagelv);
    if(tlv==1){      //init
    }else if(paran==0){    
      if(tlv%2==0)cphi0+=rphi0/(staw*2);
      else if(tlv%2==1)cphi0-=rphi0/(staw);
    }else if(paran==1){    
      if(tlv%2==0)cbirks+=rbirks/(staw*2);
      else if(tlv%2==1)cbirks-=rbirks/(staw);
    }else if(paran==2){    
      if(tlv%2==0)cR0+=rR0/(staw*2);
      else if(tlv%2==1)cR0-=rR0/(staw);
    }else if(paran==3){    
      if(tlv%2==0)csigma+=rsigma/(staw*2);
      else if(tlv%2==1)csigma-=rsigma/(staw);
    }
    //HHHH LogL calc HHHH//
    float logL=0;
    for(int ibin=1; ibin<fw_nbin+1; ibin++){
      float logP = 0;
      //HH observed HH//
      float ob=aob[ibin-1];
      float obe=aobe[ibin-1];
      //HH expected HH//
      float sum_integ1=0, sum_integ2=0;
      float bin_step = binw_lut[ibin];    //float bin_width=bins[ibin]-bins[ibin-1]; //float bin_step = bin_width/(nintegbin+1);
      for(int integbin=0; integbin<fw_nintegbin; integbin++){
 	rx=bins_lut[ibin-1]+bin_step*(integbin+1);  // tierd @ here.... bellow is not optimized to FW.
 	float zDef = 5.*csigma; //This constant determines in which area the integrator of fInt1 and fInt2 gonna be defined and integrated, see definition of a and b
 	float a;
	if(rx-zDef<(1.-fw_precision-1e-3)*cR0){// integration start. Subtract constant to make sure a is smaller than b
	  a =rx-zDef;
	}else{
	  a=(1.-fw_precision-1e-3)*cR0; 
	}
 	float b; // integration end
	if(rx+zDef<(1.-fw_precision)*cR0){
	  b = rx+zDef;
	}else{
	  b=(1.-fw_precision)*cR0;
	}; 



 	// HH Bo integral  part HH// ---------------------------------------------------------------------------------------------------------
 	float integral1=0;//integ 1 par
 	float width1=(b-a)/my_precision; 
 	float xx1=a+width1/2.;
 	float integral2=0;//integ 2 par

	// 	float pF2 = gamma_H2O*beta/(1+beta*cR0);
	float beta=0.0012;
	float pF2 = 0.6*beta/(1+beta*cR0);
 	float LofZ = 0;  
 	float myLofZ=0;
 	float width2=((1-fw_precision)*cR0-rx)/my_precision;
 	float xx2=rx+width2/2.;
 	for(int i=0; i<my_precision; i++){
 	  // integ 1
 	  float pF1 = (1.+beta*(cR0-xx1))/(1+beta*cR0);
 	  float foldingTerm = 1./(2.507*csigma)*exp(-(rx-xx1)*(rx-xx1)/(2*csigma*csigma));
	  //	  float tdLdz=1./(0.2117*pow(cR0-xx1,0.425)+cbirks);
	  float tdLdz=1./(0.2117*exp(0.425*log(cR0-xx1))+cbirks);
	  // float aa=cR0-xx1;
	  // float tdLdz=1./(0.2117*exp(0.425*log(aa))+cbirks);
	  
 	  float val1 = pF1 * tdLdz * foldingTerm;
 	  integral1+=val1*width1;
 	  xx1+=width1;
 	  // integ 2
 	  if(rx<(1-fw_precision)*cR0){
	    //	    float val2  = 1./(0.2117*pow(cR0-xx2,0.425)+cbirks);
	    float val2  = 1./(0.2117*exp(0.425*log(cR0-xx2))+cbirks);
 	    myLofZ+=val2*width2;
 	    xx2+=width2;
 	  }
 	}
 	integral2 = pF2 * myLofZ;
 	sum_integ1+=integral1;
 	sum_integ2+=integral2;
      }
      float integral = (sum_integ1 + sum_integ2)/fw_nintegbin; // Put all parts together and add scaling factor    
      // Combine prefactor and integral of all parts
      float preFactor = cphi0; //S*cphi0/rho //Put all parts together and add scaling factor
      float ex = preFactor*integral;
      //HH -2 Log gaussian likelihood calculatio to estimate mini L position for minimizer
      if(ex>0){
 	logP=(ob-ex)*(ob-ex)/(obe*obe)+2*log(obe*2.507);
      }
      logL+=logP;
    } // end loop for bins for minL
    // if(debug_lv>3){
    //   cout<<setprecision(4)
    // 	  <<"stage="<<setw(3)<<stagelv<<", paran="<<paran<<", tlv="<<setw(5)<<tlv
    // 	  <<", R0="<<setw(8)<<cR0<<", minR0="<<setw(8)<<minR0
    // 	  <<", sigma="<<setw(8)<<csigma<<", minsigma="<<setw(8)<<minsigma
    // 	  <<", phi0="<<setw(8)<<cphi0<<", minphi0="<<setw(8)<<minphi0
    // 	  <<", birks="<<setw(8)<<cbirks<<", minbirks="<<setw(8)<<minbirks
    // 	  <<",  logL="<<setw(8)<<logL<<", MinLogL="<<setw(8)<<minLogL<<endl;
    // }
    //HHHH minimization HHHH//
    if(logL<minLogL){ //mini
      minLogL=logL;
      minR0=cR0;
      minsigma=csigma;
      minphi0=cphi0;
      minbirks=cbirks;
    }
    if(paran==0 and tlv%2==1){
      cphi0=minphi0;
    }else if(paran==1 and tlv%2==1){
      cbirks=minbirks;
    }else if(paran==2 and tlv%2==1){
      cR0=minR0;
    }else if(paran==3 and tlv%2==1){
      csigma=minsigma;
    }
  }
  *outR0=minR0; *outsigma=minsigma; *outphi0=minphi0; *outkb=minbirks;
  //  *outR0=inR0; *outsigma=insigma; *outphi0=inphi0; *outkb=inkb;
  return 0;
}