// $Id$
//
//    File: DBCALEcalib.cc
// Created: Mon Jun  6 08:01:33 EDT 2011
// Creator: davidl (on Linux ifarm1101 2.6.18-128.7.1.el5 x86_64)
//

#include <set>
#include <cmath>
using namespace std;

#include <JANA/JApplication.h>
using namespace jana;


#include "DBCALEcalib.h"

//---------------------------------
// DBCALEcalib    (Constructor)
//---------------------------------
DBCALEcalib::DBCALEcalib(JCalibration *jcalib, string namepath)
{

	if(!jcalib)return;
  
	jout<<"Reading BCAL energy calibration map from "<<namepath<<" ..."<<endl;
	vector< vector<float> > Ecalib_map;
	jcalib->Get(namepath, Ecalib_map);
	jout<<Ecalib_map.size()<<" entries found";

	// The map should be on a grid with equal spacing in z and E_raw.
	// Here we want to determine the number of points in each of these
	// dimensions and the range.
	// The easiest way to do this is to use a map<float, int> to make a
	// histogram of the entries by using the key to hold the extent
	// so that the number of entries will be equal to the number of
	// different values.
	set<float> E_vals;
	set<float> zvals;
	E_min = z_min = 1.0E6;
	E_max = z_max = -1.0E6;
	for(unsigned int i=0; i<Ecalib_map.size(); i++){
		vector<float> &a = Ecalib_map[i];
		float &z = a[0];
		float &E_raw = a[1];

		E_vals.insert(E_raw);
		zvals.insert(z);

		if(E_raw < E_min)E_min=E_raw;
		if(E_raw > E_max)E_max=E_raw;
		if(z < z_min)z_min=z;
		if(z > z_max)z_max=z;
	}
  N_E = E_vals.size();
  N_z = zvals.size();
  jout<<" (N_E="<<N_E<<" N_z="<<N_z<<") ";
  jout<<" at 0x"<<hex<<(unsigned long)this<<dec<<endl;
  jout<<" ("<<E_min<<" GeV <= E <= "<<E_max<<" GeV)"<<endl;
  
	// Copy values into nested containers to allow fast indexing
	// via Ecalib[z][E_raw].
	vector<EcalibPoint_t> zvec(N_z);
	for(int i=0; i<N_E; i++)Etable.push_back(zvec);
  
	dE = (E_max-E_min)/(double)(N_E-1);
	dz = (z_max-z_min)/(double)(N_z-1);

	for(unsigned int i=0; i<Ecalib_map.size(); i++){
		vector<float> &a = Ecalib_map[i];
		float &z = a[0];
		float &E_raw = a[1];
		float &E_gen = a[2];

		int zindex = (int)floor((z-z_min)/dz);
		int Eindex = (int)floor((E_raw-E_min)/dE);

		if(zindex<0 || Eindex<0 || zindex>=N_z || Eindex>=N_E){
			_DBG_<<"VALUE OUT OF RANGE: zindex="<<zindex<<" Eindex="<<Eindex<<endl;
			exit(-1);
		}

		EcalibPoint_t *p = &Etable[Eindex][zindex];
		
		p->z = z;
		p->E_raw = E_raw;
		p->E = E_gen;

	}
}

//---------------------------------
// ~DBCALEcalib    (Destructor)
//---------------------------------
DBCALEcalib::~DBCALEcalib()
{

}

//---------------------------------
// GetCorrectedEnergy
//---------------------------------
double DBCALEcalib::GetCorrectedEnergy(double E_raw, double z_recon)
{
	// Find grid points surrounding the desired point
	int zindex1 = (int)floor((z_recon-z_min)/dz);
	int Eindex1 = (int)floor((E_raw-E_min)/dE);
	if(zindex1<0)zindex1=0;
	if(Eindex1<0)Eindex1=0;
	int zindex2 = zindex1 + 1;
	int Eindex2 = Eindex1 + 1;
	if(zindex2>=N_z)zindex2=N_z-1;
	if(Eindex2>=N_E)Eindex2=N_E-1;

	// account for being at outer edge so we can extrapolate
	if(zindex1>=zindex2)zindex1 = zindex2-1;
	if(Eindex1>=Eindex2)Eindex1 = Eindex2-1;
	
	// Coordinates of 4 corners of box containing E_raw and z_recon
	double z1 = z_min + dz*(double)zindex1;
	double z2 = z_min + dz*(double)zindex2;
	double E1 = E_min + dE*(double)Eindex1;
	double E2 = E_min + dE*(double)Eindex2;
	
	// Fraction of distance desired point is from low-edge point
	double zfrac = (z_recon - z1)/(z2 - z1);
	double Efrac = (E_raw - E1)/(E2 - E1);
	
	// Values of E at 4 corners
	double Ea = Etable[Eindex1][zindex1].E;
	double Eb = Etable[Eindex1][zindex2].E;
	double Ec = Etable[Eindex2][zindex1].E;
	double Ed = Etable[Eindex2][zindex2].E;
	
	// Some of the table values are zero which is probably not
	// what we want here. Find an average of the non-zero values
	// and replace any zero values with it.
	float Nnon_zero = 0;
	float Enon_zero_avg = 0.0;
	if(Ea>0.0){ Enon_zero_avg+=Ea; Nnon_zero+=1.0;}
	if(Eb>0.0){ Enon_zero_avg+=Eb; Nnon_zero+=1.0;}
	if(Ec>0.0){ Enon_zero_avg+=Ec; Nnon_zero+=1.0;}
	if(Ed>0.0){ Enon_zero_avg+=Ed; Nnon_zero+=1.0;}
	if(Nnon_zero>0.0)Enon_zero_avg /= Nnon_zero;
	if(Ea<=0.0)Ea = Enon_zero_avg;
	if(Eb<=0.0)Eb = Enon_zero_avg;
	if(Ec<=0.0)Ec = Enon_zero_avg;
	if(Ed<=0.0)Ed = Enon_zero_avg;

	// Interoplate in z and E
	double E_u = Ea  + zfrac*(Eb-Ea);
	double E_v = Ec  + zfrac*(Ed-Ec);
	
	double Ecorr = E_u + Efrac*(E_v-E_u);

	return Ecorr;
}