///////////////////////////////////////////////////////////////////////////////
//                                                                           
// LOpticsOpt                                                                    
//                                                                           
// http://www.jlab.org/~jinhuang/HRSOptics/
//
// HRS optics matrix optimization class                                      
// Based on THaVDC
//                                                                           
// Units used:                                                               
//        For X, Y, and Z coordinates of track    -  meters                  
//        For Theta and Phi angles of track       -  tan(angle)
//        For Momentums, Masses                   -  GeV, GeV/c^2
//
// Author: Jin Huang <jinhuang@jlab.org>
//                                       
//Modify: Navaphon Muangma
//constant change according to SRC-He4                                    
///////////////////////////////////////////////////////////////////////////////

#include "LOpticsOpt.h"
#include "THaGlobals.h"
#include "THaEvData.h"
#include "THaDetMap.h"
#include "THaTrack.h"
#include "THaScintillator.h"
#include "THaSpectrometer.h"
#include "TClonesArray.h"
#include "TList.h"
#include "TCanvas.h"
#include "TPad.h"
#include "TH2.h"
#include "TH1.h"
#include "TF1.h"
#include "TLatex.h"
#include "TVector3.h"
#include "TLine.h"
#include "TArrow.h"
#include "VarDef.h"
//#include <algorithm>
#include "TROOT.h"
#include "TTree.h"
#include "TFile.h"
#include <map>
#include <cstdio>
#include <cstdlib>

#ifdef WITH_DEBUG
#include <iostream>
#endif

using namespace std;
using THaString::Split;

/////////////////////////////////////////////////////////////////////////
// Input Sections
/////////////////////////////////////////////////////////////////////////
// HRS Positioning
// The central ray of the spectrometer is at -17.5 degrees.
// TCS is 2.31 mm perpendicular to Spectrometer (center at 16.489 deg) away from the HCS center 
// and 0.54 mm vertically (positive = up).

const Double_t HRSAngle = 17.4997/180.*TMath::Pi();
const Double_t MissPointZ = 2.31e-3;
const Double_t MissPointY =0.540e-3;

/////////////////////////////////////////////////////////////////////////
// for Calculating real sieve positions
// A positive X value is to the beam left, a positive Y is too high.
// Spectrometer Slit: (DX/DY from spectrometer centerline; DZ from target)
// 		DX DY DZ
// 		-0.34 4.49 1157
const Double_t SieveOffX = -(4.49)*1e-3;
const Double_t SieveOffY = +(-0.34)*1e-3;
const Double_t ZPos = 1157*1e-3;
const Double_t SieveSpaceX = 1.025 * 25.400e-3;
const Double_t SieveSpaceY = .513 * 25.400e-3;
const UInt_t NSieve = 7;

/////////////////////////////////////////////////////////////////////////
// Vertex Position Inputs

//  carbon optics
//  N Carbon foil settings could be different !!!
static const UInt_t NFiols = 13;
// static const UInt_t NFiols = 7;
const Double_t z0 = -3.32e-3;//test move 12 mm from -15.32 mm
const Double_t dz = 25e-3;
const Double_t targetfoils[]={
  z0-6*dz,
  z0-5*dz,
  z0-4*dz,
  z0-3*dz,
  z0-2*dz,
  z0-1*dz,
  z0,
  z0+dz,
  z0+2*dz,
  z0+3*dz,
  z0+4*dz,
  z0+5*dz,
  z0+6*dz
  };


////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////
// Vertex Position Inputs
/*
//  N Carbon foil settings could be different !!!
 static const UInt_t NFiols = 2;
// static const UInt_t NFiols = 7;
const Double_t z0 = -15.32e-3;
const Double_t targetfoils[]={
  z0 - 75e-3,
  z0 + 75e-3
};
*/

/*
//  N Carbon foil settings could be different !!!
 static const UInt_t NFiols = 5;
// static const UInt_t NFiols = 7;
const Double_t z0 = -15.32e-3;
const Double_t targetfoils[]={
  z0 - 75e-3,
  z0 -20e-3,
  z0,
  z0 + 20e-3,
  z0 + 75e-3
};

*/
////////////////////////////////////////////////////////////////////////

/////////////////////////////////////////////////////////////////////////
// Exciation state Inputs

const UInt_t NKine = 5; //N Delta Scans

#define DIPOLE_MAG2MOM(Mag) (2.702*Mag-1.6e-03*Mag*Mag*Mag)
const Double_t HRSCentralMom[NKine]=
{
	DIPOLE_MAG2MOM((0.471328+0.471327)/2),DIPOLE_MAG2MOM(0.462306)
			,DIPOLE_MAG2MOM((0.453231+0.453236)/2),DIPOLE_MAG2MOM(0.444172)
					,DIPOLE_MAG2MOM((0.435115+0.435118)/2)
};

const Double_t GroundNuclearMass = 12*.931494028-.511e-3*6; //GeV/c^2
const Double_t ExcitationEnergy[NKine]= //selected exciation states for each kinematics
{0.,0.00443891,0.00443891,0.00443891,0.00443891};

const UInt_t NExcitationStates = 8; //Carbon Excitation States
const Double_t ExcitationEnergyList[NExcitationStates]
		={0,0.00443891,0.00765420,0.009641,0.010844,0.011160,0.011828,0.012710};

/////////////////////////////////////////////////////////////////////////
// Radiation loss Inputs

// Xiaodong's version
// const Double_t AllLossExceptFoil = 1e-3*(
// 										 .075//Be Window
// 										+0.374//BeO
// 										+0.0168+0.0228//He4 in target enclosure
// 										+0.109//air, Target Enclosure to HRS
// 										+.1//target enclosure
// 										+.1//HRS Entrance
// 										);
// const Double_t LossEachFoil = 1e-3*(0.08775);

//revised Version
const Double_t AllLossExceptFoil
		= 1e-3*(//in MeV
		0.0895//Be Window
		+0.4299//BeO
		+4.7435E-04*(51.6+70.1548)//He4 in target enclosure
		+.1//target enclosure
		+3.2433E-03*(116.078-70.1548)//air, Target Enclosure to HRS
		+0.0798//HRS Entrance
		);
const Double_t LossEachFoil = 1e-3*(0.1072);

//Array of FoilID
const Double_t RadiationLossByFoil[]={
	AllLossExceptFoil+LossEachFoil*1,
 AllLossExceptFoil+LossEachFoil*2,
 AllLossExceptFoil+LossEachFoil*3,
 AllLossExceptFoil+LossEachFoil*4,
 AllLossExceptFoil+LossEachFoil*5,
 AllLossExceptFoil+LossEachFoil*6,
 AllLossExceptFoil+LossEachFoil*7
};
//Warning: these numbers are calculated with small angle approximation

/////////////////////////////////////////////////////////////////////////
// Extended Target Correction, from THaExtTarCor

const Double_t ExtTarCor_ThetaCorr = 0.61;
const Double_t ExtTarCor_DeltaCorr = 5.18;

/////////////////////////////////////////////////////////////////////////


//_____________________________________________________________________________
Double_t LOpticsOpt::SumSquareDp(Bool_t IncludeExtraData)
{
	//return square sum of diff between calculated dp_kin and expected dp_kin
	
	Double_t d_dp = 0/*, dphi=0*/;	//Difference
	Double_t rms_dp = 0/*, rmsphi=0*/; //mean square

	static UInt_t NCall = 0;
	NCall++;

	UInt_t NCalibData=0;

	if (IncludeExtraData)
	{
		Warning("SumSquareDp","Data Beyond selected excitation state is included in this calculation");
	}
	for (UInt_t idx = 0; idx<fNRawData; idx++)
	{
	
		Double_t  dp, dp_kin;
		
		EventData &eventdata = fRawData[idx];

		//jump through data beyond selected excitation states
		if (eventdata.Data[kExtraDataFlag]>0 && !IncludeExtraData) continue;
		NCalibData++;

		Double_t x_fp = eventdata.Data[kX];
		const Double_t (*powers)[5] = eventdata.powers;
		
  // calculate the matrices we need
		CalcMatrix(x_fp, fDMatrixElems);
// 		CalcMatrix(x_fp, fTMatrixElems);
// 		CalcMatrix(x_fp, fYMatrixElems);
// 		CalcMatrix(x_fp, fYTAMatrixElems);
// 		CalcMatrix(x_fp, fPMatrixElems);
// 		CalcMatrix(x_fp, fPTAMatrixElems);

  // calculate the coordinates at the target
// 		theta = CalcTargetVar(fTMatrixElems, powers);
// 		phi = CalcTargetVar(fPMatrixElems, powers)+CalcTargetVar(fPTAMatrixElems,powers);
// 		y = CalcTargetVar(fYMatrixElems, powers)+CalcTargetVar(fYTAMatrixElems,powers);

  // calculate momentum
		dp = CalcTargetVar(fDMatrixElems, powers);
		dp_kin = dp - eventdata.Data[kDpKinOffsets];

		const UInt_t KineID = (UInt_t)(eventdata.Data[kKineID]);
		assert(KineID<NKine);//check array index size
		const Double_t ArbitaryDpKinShift = fArbitaryDpKinShift[KineID];
		
		d_dp += dp_kin - eventdata.Data[kRealDpKinMatrix]+ArbitaryDpKinShift;
		rms_dp += (dp_kin - eventdata.Data[kRealDpKinMatrix]+ArbitaryDpKinShift)
				*(dp_kin - eventdata.Data[kRealDpKinMatrix]+ArbitaryDpKinShift);

		DEBUG_MASSINFO("SumSquareDp","d_dp = %f = \t%f - \t%f",
					   dp_kin - eventdata.Data[kRealDpKinMatrix], dp_kin , eventdata.Data[kRealDpKinMatrix]  );

		//save the results
		eventdata.Data[kCalcDpKinMatrix] = dp_kin;
		eventdata.Data[kCalcDpKin] = dp_kin+eventdata.Data[kRealTgX]/ExtTarCor_DeltaCorr;
	}

	if (!IncludeExtraData) 
		assert(fNCalibData==NCalibData); // check number of event for calibration

	DEBUG_INFO("SumSquareDp","#%d : d_dp = %f,rms_dp=%f",NCall,
			   d_dp/NCalibData,TMath::Sqrt(rms_dp/NCalibData));

	return rms_dp;
}

//___________________________________static const UInt_t NFiols = 13;__________________________________________
Double_t LOpticsOpt::SumSquareDTh(void)
{
	//return square sum of diff between calculated tg_th and expected tg_th
	
	Double_t dth = 0/*, dphi=0*/;	//Difference
	Double_t rmsth = 0/*, rmsphi=0*/; //mean square

	static UInt_t NCall = 0;
	NCall++;
	
	Double_t theta/*, phi, dp, p, pathl*/;
	
	for (UInt_t idx = 0; idx<fNRawData; idx++)
	{
		EventData &eventdata = fRawData[idx];

		Double_t x_fp = eventdata.Data[kX];
		const Double_t (*powers)[5] = eventdata.powers;
		
  // calculate the matrices we need
// 		CalcMatrix(x_fp, fDMatrixElems);
		CalcMatrix(x_fp, fTMatrixElems);
// 		CalcMatrix(x_fp, fYMatrixElems);
// 		CalcMatrix(x_fp, fYTAMatrixElems);
// 		CalcMatrix(x_fp, fPMatrixElems);
// 		CalcMatrix(x_fp, fPTAMatrixElems);

  // calculate the coordinates at the target
		theta = CalcTargetVar(fTMatrixElems, powers);
// 		phi = CalcTargetVar(fPMatrixElems, powers)+CalcTargetVar(fPTAMatrixElems,powers);
// 		y = CalcTargetVar(fYMatrixElems, powers)+CalcTargetVar(fYTAMatrixElems,powers);

  // calculate momentum
// 		dp = CalcTargetVar(fDMatrixElems, powers);

		dth += theta - eventdata.Data[kRealThMatrix];
		rmsth += (theta - eventdata.Data[kRealThMatrix])*(theta - eventdata.Data[kRealThMatrix]);

// 		dphi += phi - eventdata.Data[kRealPhi];
// 		rmsphi += (phi - eventdata.Data[kRealPhi])*(phi - eventdata.Data[kRealPhi]);

		DEBUG_MASSINFO("SumSquareDTh","D_Th = %f = \t%f - \t%f",
					   theta - eventdata.Data[kRealThMatrix], theta , eventdata.Data[kRealThMatrix]  );
		DEBUG_MASSINFO("SumSquareDTh","%d : %f, %f, %f, %f, %f"
				,kRealTh
						, eventdata.Data[kRealTh-2]
								, eventdata.Data[kRealTh-1]
										, eventdata.Data[kRealTh]
												, eventdata.Data[kRealTh+1]
														, eventdata.Data[kRealTh+2]
					  );

		//save the results
		eventdata.Data[kCalcTh] = theta;
// 		eventdata.Data[kCalcPh] = eventdata.Data[kL_tr_tg_ph];
		
	}

	DEBUG_INFO("SumSquareDTh","#%d : dth = %f,rmsth=%f",NCall,
			   dth/fNRawData,TMath::Sqrt(rmsth/fNRawData));
// 	DEBUG_INFO("VerifyMatrix_Sieve","dphi = %f, rmsphi=%f",
// 			   dphi/fNRawData,TMath::Sqrt(rmsphi/fNRawData));

	return rmsth;
}

//_____________________________________________________________________________
Double_t LOpticsOpt::SumSquareDPhi(void)
{
	//return square sum of diff between calculated tg_ph and expected tg_ph
	
	Double_t /*dth = 0, */dphi=0;	//Difference
	Double_t /*rmsth = 0, */rmsphi=0; //mean square

	static UInt_t NCall = 0;
	NCall++;
	
	Double_t /*theta, */phi/*, dp, p, pathl*/;
	
	for (UInt_t idx = 0; idx<fNRawData; idx++)
	{
		EventData &eventdata = fRawData[idx];

		Double_t x_fp = eventdata.Data[kX];
		const Double_t (*powers)[5] = eventdata.powers;
		
  // calculate the matrices we need
// 		CalcMatrix(x_fp, fDMatrixElems);
// 		CalcMatrix(x_fp, fTMatrixElems);
// 		CalcMatrix(x_fp, fYMatrixElems);
// 		CalcMatrix(x_fp, fYTAMatrixElems);
		CalcMatrix(x_fp, fPMatrixElems);
// 		CalcMatrix(x_fp, fPTAMatrixElems);

  // calculate the coordinates at the target
// 		theta = CalcTargetVar(fTMatrixElems, powers);
		phi = CalcTargetVar(fPMatrixElems, powers)/*+CalcTargetVar(fPTAMatrixElems,powers)*/;
// 		y = CalcTargetVar(fYMatrixElems, powers)+CalcTargetVar(fYTAMatrixElems,powers);

  // calculate momentum
// 		dp = CalcTargetVar(fDMatrixElems, powers);

// 		dth += theta - eventdata.Data[kRealTh];
// 		rmsth += (theta - eventdata.Data[kRealTh])*(theta - eventdata.Data[kRealTh]);

		dphi += phi - eventdata.Data[kRealPhi];
		rmsphi += (phi - eventdata.Data[kRealPhi])*(phi - eventdata.Data[kRealPhi]);


		//save the results
		eventdata.Data[kCalcPh] = phi;
		
	}

// 	DEBUG_INFO("SumSquareDTh","#%d : dth = %f,rmsth=%f",NCall,
// 			   dth/fNRawData,TMath::Sqrt(rmsth/fNRawData));
	DEBUG_INFO("SumSquareDPhi","#%d : dphi = %f, rmsphi=%f",NCall,
			   dphi/fNRawData,TMath::Sqrt(rmsphi/fNRawData));

	return rmsphi;
}

//_____________________________________________________________________________
Double_t LOpticsOpt::SumSquareDTgY(void)
{

	//return square sum of diff between calculated tg_y and expected tg_y
	
	Double_t /*dth = 0, */dtg_y=0;	//Difference
	Double_t /*rmsth = 0, */dtg_y_rms=0; //mean square

	static UInt_t NCall = 0;
	NCall++;
	
	Double_t /*theta, */tg_y/*, dp, p, pathl*/;
	
	for (UInt_t idx = 0; idx<fNRawData; idx++)
	{
		EventData &eventdata = fRawData[idx];

		Double_t x_fp = eventdata.Data[kX];
		const Double_t (*powers)[5] = eventdata.powers;
		
  // calculate the matrices we need
// 		CalcMatrix(x_fp, fDMatrixElems);
// 		CalcMatrix(x_fp, fTMatrixElems);
		CalcMatrix(x_fp, fYMatrixElems);
// 		CalcMatrix(x_fp, fYTAMatrixElems);
// 		CalcMatrix(x_fp, fPMatrixElems);
// 		CalcMatrix(x_fp, fPTAMatrixElems);

  // calculate the coordinates at the target
// 		theta = CalcTargetVar(fTMatrixElems, powers);
// 		phi = CalcTargetVar(fPMatrixElems, powers)/*+CalcTargetVar(fPTAMatrixElems,powers)*/;
		tg_y = CalcTargetVar(fYMatrixElems, powers)
				/*+CalcTargetVar(fYTAMatrixElems,powers)*/;

  // calculate momentum
// 		dp = CalcTargetVar(fDMatrixElems, powers);

		const UInt_t FoilID = (UInt_t)eventdata.Data[kCutID];

		const Double_t ArbitaryVertexShift =
				fArbitaryVertexShift[FoilID] * (-TMath::Sin(HRSAngle));
		

		
		dtg_y += tg_y - eventdata.Data[kRealTgY]+ArbitaryVertexShift;
		dtg_y_rms += (tg_y - eventdata.Data[kRealTgY]+ArbitaryVertexShift)
				*(tg_y - eventdata.Data[kRealTgY]+ArbitaryVertexShift);

		//save the results
		eventdata.Data[kCalcTgY] = tg_y;
	}

// 	DEBUG_INFO("SumSquareDTh","#%d : dth = %f,rmsth=%f",NCall,
// 			   dth/fNRawData,TMath::Sqrt(rmsth/fNRawData));
	DEBUG_INFO("SumSquareDTgY","#%d : dtg_y = %f, dtg_y_rms=%f",NCall,
			   dtg_y/fNRawData,TMath::Sqrt(dtg_y_rms/fNRawData));

	return dtg_y_rms;
}

//_____________________________________________________________________________
Double_t LOpticsOpt::SumSquareDTgYAverFoils(void)
{

	//return square sum of diff between calculated tg_y and expected tg_y
	//Statistical Weight on each foil is same
	
	Double_t /*dth = 0, */dtg_y=0;	//Difference
	Double_t /*rmsth = 0, */dtg_y_rms=0; //mean square

	static UInt_t NCall = 0;
	NCall++;
	
	Double_t /*theta, */tg_y/*, dp, p, pathl*/;
	
	Double_t dtg_y_foil[NFiols] ={0};
	Double_t rmstg_y_foil[NFiols] ={0};
	UInt_t ndata_foil[NFiols] ={0};

	
	for (UInt_t idx = 0; idx<fNRawData; idx++)
	{
		EventData &eventdata = fRawData[idx];

		Double_t x_fp = eventdata.Data[kX];
		const Double_t (*powers)[5] = eventdata.powers;
		
  // calculate the matrices we need
// 		CalcMatrix(x_fp, fDMatrixElems);
// 		CalcMatrix(x_fp, fTMatrixElems);
		CalcMatrix(x_fp, fYMatrixElems);
// 		CalcMatrix(x_fp, fYTAMatrixElems);
// 		CalcMatrix(x_fp, fPMatrixElems);
// 		CalcMatrix(x_fp, fPTAMatrixElems);

  // calculate the coordinates at the target
// 		theta = CalcTargetVar(fTMatrixElems, powers);
// 		phi = CalcTargetVar(fPMatrixElems, powers)/*+CalcTargetVar(fPTAMatrixElems,powers)*/;
		tg_y = CalcTargetVar(fYMatrixElems, powers)/*+CalcTargetVar(fYTAMatrixElems,powers)*/;

  // calculate momentum
// 		dp = CalcTargetVar(fDMatrixElems, powers);

		dtg_y += tg_y - eventdata.Data[kRealTgY];
		dtg_y_rms += (tg_y - eventdata.Data[kRealTgY])*(tg_y - eventdata.Data[kRealTgY]);

		//save the results
		eventdata.Data[kCalcTgY] = tg_y;

		
		//statics by foils
		const UInt_t FoilID = (UInt_t)eventdata.Data[kCutID];
		const Double_t ArbitaryVertexShift =		//arbitary shifts
				fArbitaryVertexShift[FoilID] * (-TMath::Sin(HRSAngle));
		ndata_foil[FoilID]++;
		dtg_y_foil[FoilID] += eventdata.Data[kCalcTgY] - eventdata.Data[kRealTgY]+ArbitaryVertexShift;
		rmstg_y_foil[FoilID] +=
				(eventdata.Data[kCalcTgY] - eventdata.Data[kRealTgY]+ArbitaryVertexShift)*
				(eventdata.Data[kCalcTgY] - eventdata.Data[kRealTgY]+ArbitaryVertexShift);
	}

	Double_t /*dth = 0, */dtg_y_foilaver=0;	//Difference
	Double_t /*rmsth = 0, */dtg_y_rms_foilaver=0; //mean square
	for(UInt_t FoilID = 0; FoilID<NFiols; FoilID++)
	{
		assert(ndata_foil[FoilID]);// make sure there are at least one event on this foil
		const UInt_t N = ndata_foil[FoilID];

		dtg_y_foilaver += dtg_y_foil[FoilID]/N;
		dtg_y_rms_foilaver += rmstg_y_foil[FoilID]/N;;
	}
	dtg_y_foilaver = dtg_y_foilaver/NFiols*fNRawData;
	dtg_y_rms_foilaver = dtg_y_rms_foilaver/NFiols*fNRawData;
	
	DEBUG_INFO("SumSquareDTgY","#%d Foil Ave: dtg_y = %f, dtg_y_rms=%f",NCall,
			   dtg_y_foilaver/fNRawData,TMath::Sqrt(dtg_y_rms_foilaver/fNRawData));
	DEBUG_INFO("SumSquareDTgY","#%d : dtg_y = %f, dtg_y_rms=%f",NCall,
			   dtg_y/fNRawData,TMath::Sqrt(dtg_y_rms/fNRawData));

	return dtg_y_rms_foilaver;
}

//_____________________________________________________________________________

void LOpticsOpt::PrepareSieve(void)
{
	//calculate kRealTh, kRealPhi
	
// 	DEBUG_INFO("PrepareSieve","Entry Point");

	Double_t dth = 0, dphi=0;
	Double_t exttargcorr_th=0, rms_exttargcorr_th=0;
	
	for (UInt_t idx = 0; idx<fNRawData; idx++)
	{
		EventData &eventdata = fRawData[idx];

		UInt_t res = (UInt_t)eventdata.Data[kCutID];
		const UInt_t FoilID = res/(NSieve*NSieve); //starting 0!
		res = res%(NSieve*NSieve);
		const UInt_t Col = res/(NSieve); //starting 0!
		const UInt_t Row = res%(NSieve); //starting 0!

		assert(FoilID<NFiols);//check array index size
		
		const TVector3 SieveHoleTCS(
							  SieveOffX + (-1.*Row+3.)*SieveSpaceX,
											SieveOffY + (-1.*Col+3.)*SieveSpaceY,
													ZPos
							 );
		
		const TVector3 BeamSpotHCS(
							 eventdata.Data[kBeamX],
		eventdata.Data[kBeamY],
  targetfoils[FoilID]
							);

		const TVector3 BeamSpotTCS=fTCSInHCS.Inverse()*(BeamSpotHCS-fPointingOffset);
		const TVector3 MomDirectionTCS = SieveHoleTCS - BeamSpotTCS;

		eventdata.Data[kRealTh] = MomDirectionTCS.X()/MomDirectionTCS.Z();
		eventdata.Data[kRealPhi] = MomDirectionTCS.Y()/MomDirectionTCS.Z();

		const Double_t x_tg = BeamSpotTCS.X()-BeamSpotTCS.Z()*eventdata.Data[kRealTh];
		eventdata.Data[kRealTgX] = x_tg;

		//Expected th ph before ext. target correction
// 		fDeltaTh = fThetaCorr * x_tg;
// 		Double_t theta = trkifo->GetTheta() + fDeltaTh;
		eventdata.Data[kRealThMatrix]=eventdata.Data[kRealTh] - x_tg*ExtTarCor_ThetaCorr;

		exttargcorr_th += x_tg*ExtTarCor_ThetaCorr;
		rms_exttargcorr_th += x_tg*ExtTarCor_ThetaCorr*x_tg*ExtTarCor_ThetaCorr;

		
		DEBUG_MASSINFO("PrepareSieve","%d,%d,%d: D_Th = %f,\t D_Phi = %f",FoilID,Col,Row,
					   eventdata.Data[kRealThMatrix]-eventdata.Data[kL_tr_tg_th],
		eventdata.Data[kRealPhi]-eventdata.Data[kL_tr_tg_ph]
					  );
		DEBUG_MASSINFO("PrepareSieve","%f,\t%f",
					   eventdata.Data[kRealThMatrix],eventdata.Data[kL_tr_tg_th]
					  );
		
		dth+=eventdata.Data[kRealThMatrix]-eventdata.Data[kL_tr_tg_th];
		dphi+=eventdata.Data[kRealPhi]-eventdata.Data[kL_tr_tg_ph];
		
	}
	
	DEBUG_INFO("PrepareSieve","Average : D_Th = %f,\t D_Phi = %f",dth/fNRawData,dphi/fNRawData);
	DEBUG_INFO("PrepareSieve","Average Extended Target Corretion: th = %f,\t rms_th = %f"
			,exttargcorr_th/fNRawData,TMath::Sqrt(rms_exttargcorr_th/fNRawData));

	//make sure kCalcTh, kCalcPh is filled
	SumSquareDTh();
	SumSquareDPhi();
}
//_____________________________________________________________________________

void LOpticsOpt::PrepareVertex(void)
{
	//calculate kRealTgY, kRealReactZ

	//set fYMatrixElems as current matrix to optimize
	fCurrentMatrixElems = & fYMatrixElems;
	
	Double_t dtg_y = 0,dtg_y_rms = 0;
	
	for (UInt_t idx = 0; idx<fNRawData; idx++)
	{
		EventData &eventdata = fRawData[idx];

		UInt_t res = (UInt_t)eventdata.Data[kCutID];
		const UInt_t FoilID = res;

		assert(FoilID<NFiols);//check array index size

		TVector3 BeamSpotHCS(
							 eventdata.Data[kBeamX],
		eventdata.Data[kBeamY],
  targetfoils[FoilID]
							);
		TVector3 BeamSpotTCS=fTCSInHCS.Inverse()*(BeamSpotHCS-fPointingOffset);

		Double_t Real_Tg_Y = BeamSpotTCS.Y()
				+ eventdata.Data[kL_tr_tg_ph] * (0-BeamSpotTCS.Z());

		eventdata.Data[kRealTgY] = Real_Tg_Y;
		eventdata.Data[kRealReactZ] = targetfoils[FoilID];

		dtg_y += (eventdata.Data[kL_tr_tg_y] - Real_Tg_Y);
		dtg_y_rms += (eventdata.Data[kL_tr_tg_y] - Real_Tg_Y)
				*(eventdata.Data[kL_tr_tg_y] - Real_Tg_Y);

		////////////////////////////////////////////////////////////
		//redundant checks
		////////////////////////////////////////////////////////////

		TVector3 Tg_YSpotTCS(0
				,eventdata.Data[kRealTgY]
				,0
							);
		TVector3 MomDirectionTCS(0
				,eventdata.Data[kL_tr_tg_ph]
				,1
								);

		TVector3 Tg_YSpotHCS=fTCSInHCS*Tg_YSpotTCS+fPointingOffset;
		TVector3 MomDirectionHCS=fTCSInHCS*MomDirectionTCS;

		assert(Tg_YSpotHCS.Y()==MissPointY);	// check coordinates conversions
		assert(MomDirectionHCS.Y()==0);			// check coordinates conversions

		Double_t reactz = Tg_YSpotHCS.Z()
				- (Tg_YSpotHCS.X()-eventdata.Data[kBeamX])
				/MomDirectionHCS.X()*MomDirectionHCS.Z();

		DEBUG_MASSINFO("PrepareVertex","reactz =%f, eventdata.Data[kRealReactZ]=%f",
					   reactz,eventdata.Data[kRealReactZ]);
		DEBUG_MASSINFO("PrepareVertex","Real_Tg_Y =%f, eventdata.Data[kRealReactZ]=%f, targetfoils[FoilID]=%f",
					   Real_Tg_Y,eventdata.Data[kL_tr_tg_ph],targetfoils[FoilID]);
		assert(TMath::Abs(reactz - eventdata.Data[kRealReactZ])<1e-4); //check internal calculation consistency
		////////////////////////////////////////////////////////////

		
	}
	
	DEBUG_INFO("PrepareVertex","Average : dtg_y = %f, dtg_y_rms=%f"
			,dtg_y/fNRawData,dtg_y_rms/fNRawData);

	//make sure kCalcTh, kCalcPh is filled
	SumSquareDTgY();
}
//_____________________________________________________________________________

void LOpticsOpt::PrepareDp(void)
{
	//calate expected dp_kin, dp_kin offsets ....
	//Fill up fRawData[].Data[] kKineID thr kRealDpKin

	//print Central Momentums
	printf("HRSCentralMom[%d] (GeV) = {",NKine);
	for(UInt_t KineID = 0; KineID<NKine; KineID++)
		printf("%f  ",HRSCentralMom[KineID]*1000);
	printf("}\n");
	
	//print radiation loss numbers
	printf("RadiationLossByFoil[%d] (MeV) = {",NFiols);
	for(UInt_t FoilID = 0; FoilID<NFiols; FoilID++)
		printf("%f  ",RadiationLossByFoil[FoilID]*1000);
	printf("}\n");

	
	//set fDMatrixElems as current matrix to optimize
	fCurrentMatrixElems = & fDMatrixElems;

	Double_t dth = 0, dphi=0, scatang=0;
	Double_t dpkinoff = 0, dpkinoff_rms=0;
	fNCalibData = 0;
	Double_t exttargcorr_dp=0, rms_exttargcorr_dp=0;
	
	for (UInt_t idx = 0; idx<fNRawData; idx++)
	{
		DEBUG_MASSINFO("PrepareDp","=========== Event %d ===========",idx);
		
		EventData &eventdata = fRawData[idx];

		//decoding kCutID
		UInt_t res = (UInt_t)eventdata.Data[kCutID];

		const UInt_t ExtraDataFlag = res/(NSieve*NSieve*NFiols*NKine);
		res = res%(NSieve*NSieve*NFiols*NKine);
		const UInt_t KineID = res/(NSieve*NSieve*NFiols); //starting 0!
		res = res%(NSieve*NSieve*NFiols);
		const UInt_t FoilID = res/(NSieve*NSieve); //starting 0!
		res = res%(NSieve*NSieve);
		const UInt_t Col = res/(NSieve); //starting 0!
		const UInt_t Row = res%(NSieve); //starting 0!
		assert(ExtraDataFlag<2);		//Check flag range. beyond 2 is not used
		assert(KineID<NKine);//check array index size
		assert(FoilID<NFiols);//check array index size
		
		DEBUG_MASSINFO("PrepareDp","%d => KineID=%d,\tFoilID=%d,\tCol=%d,\tRow=%d",
					   (UInt_t)eventdata.Data[kCutID],KineID,FoilID,Col,Row);

		//write some variables
 		eventdata.Data[kExtraDataFlag] = ExtraDataFlag;
		if (!ExtraDataFlag) fNCalibData++;
		eventdata.Data[kKineID] = KineID;
		eventdata.Data[kCentralp] = HRSCentralMom[KineID];
		const Double_t EPICS_P = eventdata.Data[kL_tr_p]/(1.+eventdata.Data[kL_tr_tg_dp]);
		DEBUG_MASSINFO("PrepareDp","Central_P/GeV: EPICS=%f, NMR=%f",
					   EPICS_P,eventdata.Data[kCentralp]);
		assert(TMath::Abs(EPICS_P - eventdata.Data[kCentralp])<1e-3);

		TVector3 SieveHoleTCS(
							  SieveOffX + (-1.*Row+3.)*SieveSpaceX,
											SieveOffY + (-1.*Col+3.)*SieveSpaceY,
													ZPos
							 );
		
		TVector3 BeamSpotHCS(
							 eventdata.Data[kBeamX],
		eventdata.Data[kBeamY],
  targetfoils[FoilID]
							);

		// Calculate Real Scattering Angles by Sieve Holes cuts		
		TVector3 BeamSpotTCS=fTCSInHCS.Inverse()*(BeamSpotHCS-fPointingOffset);

		TVector3 MomDirectionTCS = SieveHoleTCS - BeamSpotTCS;

		eventdata.Data[kRealTh] = MomDirectionTCS.X()/MomDirectionTCS.Z();
		eventdata.Data[kRealPhi] = MomDirectionTCS.Y()/MomDirectionTCS.Z();
		
		const Double_t x_tg = BeamSpotTCS.X()-BeamSpotTCS.Z()*eventdata.Data[kRealTh];
		eventdata.Data[kRealTgX] = x_tg;
		
		DEBUG_MASSINFO("PrepareDp","%d,%d,%d: D_Th = %f,\t D_Phi = %f",FoilID,Col,Row,
					   eventdata.Data[kRealThMatrix]-eventdata.Data[kL_tr_tg_th],
						eventdata.Data[kRealPhi]-eventdata.Data[kL_tr_tg_ph]);
		DEBUG_MASSINFO("PrepareDp","RealTh=%f,\tL_tr_tg_th=%f",
					   eventdata.Data[kRealThMatrix],eventdata.Data[kL_tr_tg_th]);
		DEBUG_MASSINFO("PrepareDp","RealPh=%f,\tkL_tr_tg_ph=%f",
					   eventdata.Data[kRealPhi],eventdata.Data[kL_tr_tg_ph]);
		DEBUG_MASSINFO("PrepareDp","SieveHoleY=%f,\tMom.Y=%f,\tMom.Z=%f",
					   SieveHoleTCS.y(),MomDirectionTCS.Y(),MomDirectionTCS.Z());
		
		dth+=eventdata.Data[kRealThMatrix]-eventdata.Data[kL_tr_tg_th];
		dphi+=eventdata.Data[kRealPhi]-eventdata.Data[kL_tr_tg_ph];

		TVector3 MomDirectionHCS = fTCSInHCS*MomDirectionTCS;
		TVector3 BeamDirection(0,0,1);
		const Double_t ScatteringAngle = BeamDirection.Angle(MomDirectionHCS);
		eventdata.Data[kScatterAngle] = ScatteringAngle;
		scatang+=ScatteringAngle;

		// calculate difference between dp_kin and dp
		// dp_kin + kDpKinOffsets = dp
		const Double_t DM = ExcitationEnergy[KineID];
		const Double_t Ma = GroundNuclearMass;
		const Double_t P0 = eventdata.Data[kurb_e];
		const Double_t DpKinOffsets =
				(ScatMom(DM, Ma, P0, ScatteringAngle)
				-ScatMom(DM, Ma, P0, HRSAngle))/eventdata.Data[kCentralp];
		eventdata.Data[kDpKinOffsets] = DpKinOffsets;

		dpkinoff += DpKinOffsets;
		dpkinoff_rms += DpKinOffsets*DpKinOffsets;

		// calculate kRealDpKin, should be same for same kine settings
		eventdata.Data[kRadiLossDp]
				= RadiationLossByFoil[FoilID] / eventdata.Data[kCentralp];
		eventdata.Data[kRealDpKin] = 
				ScatMom(DM, Ma, P0, HRSAngle)/eventdata.Data[kCentralp]- 1 -eventdata.Data[kRadiLossDp] ;

		//Expected th ph before ext. target correction
// 		fDeltaTh = fThetaCorr * x_tg;
// 		fDeltaDp = x_tg / fDeltaCorr;
// 		Double_t theta = trkifo->GetTheta() + fDeltaTh;
// 		Double_t dp = trkifo->GetDp() + fDeltaDp;
		eventdata.Data[kRealDpKinMatrix]=eventdata.Data[kRealDpKin] - x_tg/ExtTarCor_DeltaCorr;

		exttargcorr_dp+=x_tg/ExtTarCor_DeltaCorr;
		rms_exttargcorr_dp+=(x_tg/ExtTarCor_DeltaCorr)*(x_tg/ExtTarCor_DeltaCorr);

		//calcalculate expected dp_kin for all other exciation states
		for (UInt_t ExcitID = 0; ExcitID<NExcitationStates; ExcitID++)
		{
			assert(kRealDpKinExcitations+ExcitID<kRealTh);//check array index size
			eventdata.Data[kRealDpKinExcitations+ExcitID] =
					ScatMom(ExcitationEnergyList[ExcitID], Ma, P0, HRSAngle)/
					eventdata.Data[kCentralp]-1;
		}

		DEBUG_MASSINFO("PrepareDp","ScatterAngle=%f,\tDpKinOffsets=%f",
					   ScatteringAngle/TMath::Pi()*180,DpKinOffsets);

		// data self consistency check
		if (idx>0)
		{
			const EventData &lasteventdata = fRawData[idx-1];
			if (eventdata.Data[kRunNum] == lasteventdata.Data[kRunNum])
			{
				assert(eventdata.Data[kKineID] == lasteventdata.Data[kKineID]);//check data continuity; check cut definition consistency
				assert(TMath::Abs(eventdata.Data[kCentralp]-lasteventdata.Data[kCentralp])<1e-5);//check data continuity; check cut definition consistency
				assert(TMath::Abs(eventdata.Data[kRealDpKin]-lasteventdata.Data[kRealDpKin])<4e-3);//check data continuity; check cut definition consistency
			}
			else
			{//new run
				DEBUG_INFO("PrepareDp","Run %4.0f : Kinematics #%1.0f, Central p = %fGeV, Excit. State Selected=%f MeV, Dp Kin=%f%%"
						,eventdata.Data[kRunNum]
								,eventdata.Data[kKineID]
										,eventdata.Data[kCentralp]
												,1000*ExcitationEnergy[KineID]
														,100*eventdata.Data[kRealDpKin]);
			}
		}
		else
		{
			//first run
			DEBUG_INFO("PrepareDp","Run %4.0f : Kinematics #%1.0f, Central p = %fGeV, Excit. State Selected=%f MeV, Dp Kin=%f%%"
					,eventdata.Data[kRunNum]
							,eventdata.Data[kKineID]
									,eventdata.Data[kCentralp]
											,1000*ExcitationEnergy[KineID]
													,100*eventdata.Data[kRealDpKin]);
		}
	}
	
	DEBUG_INFO("PrepareDp","%d out of %d data is for calibration"
			,fNCalibData,fNRawData);
	DEBUG_INFO("PrepareDp","Average : D_Th = %f,\t D_Phi = %f",dth/fNRawData,dphi/fNRawData);
	DEBUG_INFO("PrepareDp","Average : ScatteringAngle = %f",scatang/fNRawData/TMath::Pi()*180);
	DEBUG_INFO("PrepareDp","Average DpKinOffsets = %f, RMS DpKinOffsets = %f"
			,dpkinoff/fNRawData,TMath::Sqrt(dpkinoff_rms/fNRawData));
	DEBUG_INFO("PrepareDp","Average Extended Target Corretion: dp = %f,\t rms_dp = %f"
			,exttargcorr_dp/fNRawData,TMath::Sqrt(rms_exttargcorr_dp/fNRawData));

	//make sure kCalcTh, kCalcPh is filled, although not necessary
	SumSquareDTh();
	SumSquareDPhi();
	SumSquareDp();
}
//_____________________________________________________________________________

TCanvas * LOpticsOpt::CheckSieve()
{
	//Visualize Sieve Plane 
	
	DEBUG_INFO("CheckSieve","Entry Point");

	TH2D * HSievePlane[NFiols] = {0};
	for(UInt_t idx = 0; idx<NFiols; idx++)
	{
		HSievePlane[idx] = new TH2D(Form("Sieve_Foil%d",idx),
									Form("Sieve Plane (tg_X vs tg_Y) for Foil #%d",idx),
										 1000,-.05,.05,1000,-.1,.1
								   );
		assert(HSievePlane[idx]);//assure memory allocation
	}
	
	Double_t dX = 0, dY=0;

	enum {kEventID, kRealSieveX, kRealSieveY, kCalcSieveX, kCalcSieveY};
	Double_t SieveEventID[NFiols][NSieve][NSieve][5]={{{{0}}}};
	
	for (UInt_t idx = 0; idx<fNRawData; idx++)
	{
		const EventData &eventdata = fRawData[idx];

		UInt_t res = (UInt_t)eventdata.Data[kCutID];
		const UInt_t FoilID = res/(NSieve*NSieve); //starting 0!
		res = res%(NSieve*NSieve);
		const UInt_t Col = res/(NSieve); //starting 0!
		const UInt_t Row = res%(NSieve); //starting 0!


		assert(FoilID<NFiols);//array index check
		
		TVector3 SieveHoleTCS(
							  SieveOffX + (-1.*Row+3.)*SieveSpaceX,
											SieveOffY + (-1.*Col+3.)*SieveSpaceY,
													ZPos
							 );
		
		TVector3 BeamSpotHCS(
							 eventdata.Data[kBeamX],
		eventdata.Data[kBeamY],
  targetfoils[FoilID]
							);

// 		TRotation fTCSInHCS;
// 		TVector3 TCSX(0,-1,0);
// 		TVector3 TCSZ(TMath::Sin(HRSAngle),0,TMath::Cos(HRSAngle));
// 		TVector3 TCSY = TCSZ.Cross(TCSX);
// 		fTCSInHCS.RotateAxes(TCSX,TCSY,TCSZ);
		TVector3 BeamSpotTCS=fTCSInHCS.Inverse()*(BeamSpotHCS-fPointingOffset);

		TVector3 MomDirectionTCS = SieveHoleTCS - BeamSpotTCS;

		Double_t ProjectionX = BeamSpotTCS.X()
				+ (eventdata.Data[kCalcTh]+eventdata.Data[kRealTgX]*ExtTarCor_ThetaCorr) * (SieveHoleTCS.Z() - BeamSpotTCS.Z());
		Double_t ProjectionY = BeamSpotTCS.Y()
				+ eventdata.Data[kCalcPh] * (SieveHoleTCS.Z() - BeamSpotTCS.Z());

		HSievePlane[FoilID]->Fill(ProjectionY,ProjectionX);
		
		dX+= ProjectionX - SieveHoleTCS.X();
		dY+= ProjectionY - SieveHoleTCS.Y();

		SieveEventID[FoilID][Col][Row][kEventID] = idx;
		SieveEventID[FoilID][Col][Row][kRealSieveX] = SieveHoleTCS.X();
		SieveEventID[FoilID][Col][Row][kRealSieveY] = SieveHoleTCS.Y();
		SieveEventID[FoilID][Col][Row][kCalcSieveX] = ProjectionX;
		SieveEventID[FoilID][Col][Row][kCalcSieveY] = ProjectionY;
	}
	
	DEBUG_INFO("CheckSieve","Average : D_X = %f,\t D_Y = %f",dX/fNRawData,dY/fNRawData);

	TCanvas * c1 = new TCanvas("SieveCheck","SieveCheck",1900,1100);
	c1->Divide(7,2);

	for(UInt_t idx = 0; idx<NFiols; idx++)
	{
		UInt_t FoilID = idx;
		
		c1->cd(idx+1);
		assert(HSievePlane[idx]);//pointer check
		HSievePlane[idx]->Draw("COLZ");
		
		//Draw Sieve
		Double_t MaxPlot = .05;
		
		for(UInt_t Row = 0; Row < NSieve; Row++)
		{
			Double_t SievePos =  SieveOffX + (-1.*Row+3.)*SieveSpaceX;
			
			TLine *l = new TLine(-MaxPlot,SievePos,+MaxPlot,SievePos);
			l->SetLineColor(6);
			l->Draw();
		}
		
		MaxPlot = .1;
		for(UInt_t Col = 0; Col < NSieve; Col++)
		{
			Double_t SievePos =  SieveOffY + (-1.*Col+3.)*SieveSpaceY;
			TLine *l = new TLine(SievePos,-MaxPlot,SievePos,MaxPlot);
			l->SetLineColor(6);
			l->Draw();
		}

		for(UInt_t Col = 0; Col < NSieve; Col++){
			for(UInt_t Row = 0; Row < NSieve; Row++){
				if (SieveEventID[FoilID][Col][Row][kEventID]>0)
				{
					assert(SieveEventID[FoilID][Col][Row][kEventID] < fNRawData);//array index bondary check
					TArrow * ar2 = new TArrow(SieveEventID[FoilID][Col][Row][kCalcSieveY]
							,SieveEventID[FoilID][Col][Row][kCalcSieveX]
							,SieveEventID[FoilID][Col][Row][kRealSieveY]
									,SieveEventID[FoilID][Col][Row][kRealSieveX]
											,0.008,"|>");
					ar2->SetAngle(40);
					ar2->SetLineColor(6);
					ar2->SetFillColor(3);
					ar2->Draw();
				}
			}
		}
	}
	
	return c1;

}

TCanvas * LOpticsOpt::CheckVertex()
{
	//Visualize ReactZ spectrum
	
	DEBUG_INFO("CheckVertex","Entry Point");

	const Double_t VertexRange=.25;
	TH2D * hYVSReactZ = new TH2D("hYVSReactZ","Rot. Y VS ReactZ",1000,-VertexRange,VertexRange, 1000,-.1,.1);
	TH2D * hPhVSReactZ = new TH2D("hPhVSReactZ","Rot. Ph VS ReactZ",1000,-VertexRange,VertexRange, 1000,-.05,.05);
	TH2D * hrbxVSReactZ = new TH2D("hrbxVSReactZ","Beam X VS ReactZ",1000,-VertexRange,VertexRange, 1000,-.005,.005);
	TH1D * hReactZ = new TH1D("hReactZ","LHRS ReactZ",400,-VertexRange,VertexRange);
	
	Double_t dtg_vz = 0, dtg_vz_rms = 0, reactz=0;
	Double_t dtg_vz_foil[NFiols] ={0};
	Double_t dtg_y_foil[NFiols] ={0};
	Double_t rmstg_vz_foil[NFiols] ={0};
	Double_t rmstg_y_foil[NFiols] ={0};
	UInt_t ndata_foil[NFiols] ={0};

	//calculate kCalcReactZ
	for (UInt_t idx = 0; idx<fNRawData; idx++)
	{
		EventData &eventdata = fRawData[idx];

// 		TVector3 BeamSpotHCS(
// 							 eventdata.Data[kBeamX],
// 		eventdata.Data[kBeamY],
//   eventdata.Data[kRealReactZ]
// 							);

		TVector3 Tg_YSpotTCS(0
				,eventdata.Data[kCalcTgY]
				,0
							);
		TVector3 MomDirectionTCS(0
				,eventdata.Data[kL_tr_tg_ph]
				,1
								);
		
		TVector3 Tg_YSpotHCS=fTCSInHCS*Tg_YSpotTCS+fPointingOffset;
		TVector3 MomDirectionHCS=fTCSInHCS*MomDirectionTCS;

		assert(Tg_YSpotHCS.Y()==MissPointY);//internal consistency check
		assert(MomDirectionHCS.Y()==0);//internal consistency check

		reactz = Tg_YSpotHCS.Z()
				- (Tg_YSpotHCS.X()-eventdata.Data[kBeamX])
				/MomDirectionHCS.X()*MomDirectionHCS.Z();

		eventdata.Data[kCalcReactZ] = reactz;

		dtg_vz += reactz - eventdata.Data[kRealReactZ];
		dtg_vz_rms += (reactz - eventdata.Data[kRealReactZ])*(reactz - eventdata.Data[kRealReactZ]);

		hYVSReactZ->Fill(reactz,eventdata.Data[kY]);
		hPhVSReactZ->Fill(reactz,eventdata.Data[kPhi]);
		hrbxVSReactZ->Fill(reactz,eventdata.Data[kBeamX]);
		hReactZ->Fill(reactz);

		//statics by foils
		const UInt_t FoilID = (UInt_t)eventdata.Data[kCutID];
		ndata_foil[FoilID]++;
		
		dtg_vz_foil[FoilID] += eventdata.Data[kCalcReactZ] - eventdata.Data[kRealReactZ];
		rmstg_vz_foil[FoilID] += (eventdata.Data[kCalcReactZ] - eventdata.Data[kRealReactZ])*(eventdata.Data[kCalcReactZ] - eventdata.Data[kRealReactZ]);
		
		dtg_y_foil[FoilID] += eventdata.Data[kCalcTgY] - eventdata.Data[kRealTgY];
		rmstg_y_foil[FoilID] += (eventdata.Data[kCalcTgY] - eventdata.Data[kRealTgY])*(eventdata.Data[kCalcTgY] - eventdata.Data[kRealTgY]);
		
	}
	
	DEBUG_INFO("CheckVertex","dtg_vz = %f,\t dtg_vz_rms = %f",dtg_vz/fNRawData,dtg_vz_rms/fNRawData);

	TCanvas * c1 = new TCanvas("CheckVertex","SieveCheck",1800,900);
	c1->Divide(1,4);
	UInt_t idx=1;
	
	c1->cd(idx++);
	hYVSReactZ->Draw("COLZ");
	c1->cd(idx++);
	hPhVSReactZ->Draw("COLZ");
	c1->cd(idx++);
	hrbxVSReactZ->Draw("COLZ");
	c1->cd(idx++);
	hReactZ->Draw();

	for(idx = 1; idx<=4; idx++)
	{
		c1->cd(idx);

		for(UInt_t FoilID = 0; FoilID<NFiols; FoilID++)
		{
			const Double_t MaxPlot = 2000;
			
			TLine *l = new TLine(targetfoils[FoilID],-MaxPlot,targetfoils[FoilID],+MaxPlot);
			l->SetLineColor(6);
			l->Draw();
		}
	}

	cout<<"Statistic on each foils:\n";
	cout<<"Foil ID\td_vz\trms_vz\td_tgy\trms_tgy\n";
	for(UInt_t FoilID = 0; FoilID<NFiols; FoilID++)
	{
// 		assert(ndata_foil[FoilID]);//at least 1 event on each foil
		UInt_t N = ndata_foil[FoilID];
		if (N<=0) N=1;

		printf("%d\t%f\t%f\t%f\t%f\n",FoilID
				,dtg_vz_foil[FoilID]/N
						,rmstg_vz_foil[FoilID]/N
								,dtg_y_foil[FoilID]/N
										,rmstg_y_foil[FoilID]/N);
	}
	
	return c1;

}

TCanvas * LOpticsOpt::CheckDp()
{
	//Visualize 1D hitogram of dp_kin
	
	DEBUG_INFO("CheckDp","Entry Point");

	//calculate Data[kCalcDpKin] for all events
	SumSquareDp(kTRUE);
	
	const Double_t DpRange=.05;
	const UInt_t NDpRange=800;

	TH1D * hDpKinCalib[NKine];
	TH1D * hDpKinAll[NKine];
	Double_t RealDpKin[NKine];
	Double_t AverCalcDpKin[NKine]={0};
	UInt_t NEvntDpKin[NKine]={0};
	Double_t RealDpKinAllExcit[NExcitationStates][NKine];
	Double_t NewArbitaryDpKinShift[NKine];

	for(UInt_t KineID=0; KineID<NKine; KineID++)
	{
		hDpKinCalib[KineID]
				= new TH1D(Form("hDpKinCalib%d",KineID)
				,Form("Dp_Kin for Delta Scan Kine. #%d (Selected Exct. State)",KineID)
				,NDpRange,-DpRange,DpRange);
		hDpKinAll[KineID]
				= new TH1D(Form("hDpKinAll%d",KineID)
				,Form("Dp_Kin for Delta Scan Kine. #%d (All Data)",KineID)
				,NDpRange,-DpRange,DpRange);

		assert(hDpKinCalib[KineID]);//pointer check
		assert(hDpKinAll[KineID]);//pointer check
	}
	
	for (UInt_t idx = 0; idx<fNRawData; idx++)
	{
		const EventData &eventdata = fRawData[idx];
		const UInt_t ExtraDataFlag = (UInt_t)(eventdata.Data[kExtraDataFlag]);
		assert(ExtraDataFlag==0 || ExtraDataFlag==1); //flag definition consistency check
		const UInt_t KineID = (UInt_t)(eventdata.Data[kKineID]);
		assert(KineID<NKine);

		if (!ExtraDataFlag)
		{
			hDpKinCalib[KineID]->Fill(eventdata.Data[kCalcDpKin]+eventdata.Data[kRadiLossDp]);
			AverCalcDpKin[KineID]+=
					eventdata.Data[kCalcDpKin]+eventdata.Data[kRadiLossDp];
			NEvntDpKin[KineID]++;
		}
		hDpKinAll[KineID]->Fill(eventdata.Data[kCalcDpKin]+eventdata.Data[kRadiLossDp]);

		RealDpKin[KineID] = eventdata.Data[kRealDpKin]+eventdata.Data[kRadiLossDp];
		
		for (UInt_t ExcitID = 0; ExcitID<NExcitationStates; ExcitID++)
		{
			assert(kRealDpKinExcitations+ExcitID<kRealTh);//index check
			RealDpKinAllExcit[ExcitID][KineID]
					=eventdata.Data[kRealDpKinExcitations+ExcitID];
		}
	}
	
	TCanvas * c1 = new TCanvas("CheckDp","Check Dp Kin Reconstruction",1800,900);
	c1->Divide(3,2);
	UInt_t idx=1;
	
	for(UInt_t KineID=0; KineID<NKine; KineID++)
	{
		c1->cd(idx++);
		gPad -> SetLogy();

		AverCalcDpKin[KineID]/=NEvntDpKin[KineID];
		DEBUG_MASSINFO("CheckDp","AverCalcDpKin[%d] = %f"
				,KineID,AverCalcDpKin[KineID]);

		// Histograms
		hDpKinCalib[KineID]->SetLineColor(4);
		hDpKinCalib[KineID]->SetFillColor(4);
		hDpKinCalib[KineID]->SetFillStyle(3008);
		
		hDpKinAll[KineID]->SetLineColor(1);
// 		hDpKinAll[KineID]->SetFillColor(1);

		const Double_t dpRange=0.01;
		hDpKinCalib[KineID]->
				SetAxisRange(RealDpKin[KineID]-dpRange,RealDpKin[KineID]+dpRange);
		hDpKinAll[KineID]->
				SetAxisRange(RealDpKin[KineID]-dpRange,RealDpKin[KineID]+dpRange);
		
		hDpKinCalib[KineID]
				->SetXTitle("radiation corrected dp_kin (angular independant dp)");
		hDpKinAll[KineID]
				->SetXTitle("radiation corrected dp_kin (angular independant dp)");
		
		hDpKinAll[KineID]->Draw();
		hDpKinCalib[KineID]->Draw("SAME");

		// expectation lines
		const Double_t MaxPlot = 20000;
		for (UInt_t ExcitID = 0; ExcitID<NExcitationStates; ExcitID++)
		{
			const Double_t x = RealDpKinAllExcit[ExcitID][KineID];
			TLine *l = new TLine(x,0,x,+MaxPlot);
			l->SetLineColor(3);
			l->SetLineWidth(2);
			l->Draw();
		}
		TLine *l = new TLine(RealDpKin[KineID],0,RealDpKin[KineID],+MaxPlot);
		l->SetLineColor(6);
		l->SetLineWidth(2);
		l->Draw();


		// Fits
		const Double_t DefResolution = 5e-4;
		const Double_t FitRangeMultiply = 2;

		TString FitFunc = Form("DpPeak%d",KineID);
		TF1 *f = new TF1(FitFunc,"gaus+[3]+[4]*x"
				,AverCalcDpKin[KineID]-DefResolution*FitRangeMultiply
						,AverCalcDpKin[KineID]+DefResolution*FitRangeMultiply);
		f->SetParameter(1,AverCalcDpKin[KineID]);
		f->SetParameter(2,DefResolution);
		hDpKinAll[KineID] -> Fit(FitFunc,"RN0");
// 		Info("CheckDp","Fit for delta scan #%d peak:",KineID);
// 		f->Print();
		f->SetLineColor(2);
		f->Draw("SAME");

		TLatex *t = new TLatex(f->GetParameter(1)+DefResolution
				,f->GetParameter(0)+f->GetParameter(3)+f->GetParameter(4)*f->GetParameter(1)
				,Form("\\Delta \\pm \\sigma = %2.1f \\pm %2.1f \\times 10^{-4}"
						,10000*(f->GetParameter(1)-RealDpKin[KineID])
								,10000*f->GetParameter(2)));
		t->SetTextSize(0.05);
		t->SetTextAlign(12);
		t->SetTextColor(2);
		t->Draw();

		NewArbitaryDpKinShift[KineID] =
				f->GetParameter(1)-RealDpKin[KineID]+fArbitaryDpKinShift[KineID];
		
	}

	Info("CheckDp","New set of arbitary dp shifts:");
	for(UInt_t KineID=0; KineID<NKine; KineID++)
		printf("opt->fArbitaryDpKinShift[%d] = %e;\n"
				,KineID,NewArbitaryDpKinShift[KineID]);
	
	return c1;

}

TCanvas * LOpticsOpt::CheckDpVSAngle()
{

	//Visualize 2D hitogram of Scattering Angle VS dp_kin
	
	DEBUG_INFO("CheckDpVSAngle","Entry Point");

	//calculate Data[kCalcDpKin] for all events
	SumSquareDp(kTRUE);
	
	const Double_t DpRange=.05;
	const UInt_t NDpRange=800*5;
	const Double_t AngleRange=2;
	const UInt_t NAngleRange=100*5;

	TH2D * hDpKinCalib[NKine];
	TH2D * hDpKinAll[NKine];
	Double_t RealDpKin[NKine];
// 	Double_t AverCalcDpKin[NKine]={0};
// 	UInt_t NEvntDpKin[NKine]={0};
	Double_t RealDpKinAllExcit[NExcitationStates][NKine];
// 	Double_t NewArbitaryDpKinShift[NKine];

	for(UInt_t KineID=0; KineID<NKine; KineID++)
	{
		hDpKinCalib[KineID]
				= new TH2D(Form("hDpKinCalibVSAngle%d",KineID)
				,Form("Dp_Kin for Delta Scan Kine. #%d (Selected Exct. State)",KineID)
				,NDpRange,-DpRange,DpRange
						,NAngleRange,HRSAngle/TMath::Pi()*180-AngleRange,HRSAngle/TMath::Pi()*180+AngleRange);
		hDpKinAll[KineID]
				= new TH2D(Form("hDpKinAllVSAngle%d",KineID)
				,Form("Dp_Kin for Delta Scan Kine. #%d (All Data)",KineID)
				,NDpRange,-DpRange,DpRange
						,NAngleRange,HRSAngle/TMath::Pi()*180-AngleRange,HRSAngle/TMath::Pi()*180+AngleRange);

		assert(hDpKinCalib[KineID]);//pointer check
		assert(hDpKinAll[KineID]);//pointer check
	}
	
	for (UInt_t idx = 0; idx<fNRawData; idx++)
	{
		const EventData &eventdata = fRawData[idx];
		const UInt_t ExtraDataFlag = (UInt_t)(eventdata.Data[kExtraDataFlag]);
		assert(ExtraDataFlag==0 || ExtraDataFlag==1); //flag definition consistency check
		const UInt_t KineID = (UInt_t)(eventdata.Data[kKineID]);
		assert(KineID<NKine);

		if (!ExtraDataFlag)
		{
			hDpKinCalib[KineID]
					->Fill(eventdata.Data[kCalcDpKin]+eventdata.Data[kRadiLossDp]
					,+eventdata.Data[kScatterAngle]/TMath::Pi()*180);
// 			AverCalcDpKin[KineID]+=
// 					eventdata.Data[kCalcDpKin]+eventdata.Data[kRadiLossDp];
// 			NEvntDpKin[KineID]++;
		}
		hDpKinAll[KineID]
				->Fill(eventdata.Data[kCalcDpKin]+eventdata.Data[kRadiLossDp]
				,+eventdata.Data[kScatterAngle]/TMath::Pi()*180);
		
		RealDpKin[KineID] = eventdata.Data[kRealDpKin]+eventdata.Data[kRadiLossDp];
		
		for (UInt_t ExcitID = 0; ExcitID<NExcitationStates; ExcitID++)
		{
			assert(kRealDpKinExcitations+ExcitID<kRealTh);//index check
			RealDpKinAllExcit[ExcitID][KineID]
					=eventdata.Data[kRealDpKinExcitations+ExcitID];
		}
	}
	
	TCanvas * c1 = new TCanvas("CheckDpVSAngle","Check Dp Kin Reconstruction VS Scattering Angle",1800,900);
	c1->Divide(3,2);
	UInt_t idx=1;
	
	for(UInt_t KineID=0; KineID<NKine; KineID++)
	{
		c1->cd(idx++);
// 		gPad -> SetLogy();

// 		AverCalcDpKin[KineID]/=NEvntDpKin[KineID];
// 		DEBUG_MASSINFO("CheckDp","AverCalcDpKin[%d] = %f"
// 				,KineID,AverCalcDpKin[KineID]);

		// Histograms
// 		hDpKinCalib[KineID]->SetLineColor(4);
// 		hDpKinCalib[KineID]->SetFillColor(4);
// 		hDpKinCalib[KineID]->SetFillStyle(3008);
		
// 		hDpKinAll[KineID]->SetLineColor(1);
// 		hDpKinAll[KineID]->SetFillColor(1);

		const Double_t dpRange=0.01;
		hDpKinCalib[KineID]->
				SetAxisRange(RealDpKin[KineID]-dpRange,RealDpKin[KineID]+dpRange);
		hDpKinAll[KineID]->
				SetAxisRange(RealDpKin[KineID]-dpRange,RealDpKin[KineID]+dpRange);
		
		hDpKinCalib[KineID]
				->SetXTitle("radiation corrected dp_kin (angular independant dp)");
		hDpKinCalib[KineID]
				->SetYTitle("Scattering Angle (Degree)");
		hDpKinAll[KineID]
				->SetXTitle("radiation corrected dp_kin (angular independant dp)");
		hDpKinAll[KineID]
				->SetYTitle("Scattering Angle (Degree)");
		
		hDpKinAll[KineID]->Draw("COLZ");
// 		hDpKinCalib[KineID]->Draw("SAME");

		// expectation lines
		const Double_t MinPlot = HRSAngle/TMath::Pi()*180-AngleRange;
		const Double_t MaxPlot = HRSAngle/TMath::Pi()*180+AngleRange;
		for (UInt_t ExcitID = 0; ExcitID<NExcitationStates; ExcitID++)
		{
			const Double_t x = RealDpKinAllExcit[ExcitID][KineID];
			TLine *l = new TLine(x,MinPlot,x,+MaxPlot);
			l->SetLineColor(3);
			l->SetLineWidth(2);
			l->Draw();
		}
		TLine *l = new TLine(RealDpKin[KineID],MinPlot,RealDpKin[KineID],+MaxPlot);
		l->SetLineColor(6);
		l->SetLineWidth(2);
		l->Draw();
	}

	
	return c1;

}

TCanvas * LOpticsOpt::CheckDpVSCutID()
{
	//Visualize 2D hitogram of Sieve Hole+Foil ID VS dp_kin
	
	DEBUG_INFO("CheckDpVSCutID","Entry Point");

	//calculate Data[kCalcDpKin] for all events
	SumSquareDp(kTRUE);
	
	const Double_t DpRange=.05;
	const UInt_t NDpRange=800*5;

	const UInt_t NCutID = NSieve*NSieve*NFiols;
	const Double_t CutIDRangeLow = -.5;
	const Double_t CutIDRangeHigh= -.5+NCutID;

	TH2D * hDpKinCalib[NKine];
	TH2D * hDpKinAll[NKine];
	Double_t RealDpKin[NKine];
// 	Double_t AverCalcDpKin[NKine]={0};
// 	UInt_t NEvntDpKin[NKine]={0};
	Double_t RealDpKinAllExcit[NExcitationStates][NKine];
// 	Double_t NewArbitaryDpKinShift[NKine];

	for(UInt_t KineID=0; KineID<NKine; KineID++)
	{
		hDpKinCalib[KineID]
				= new TH2D(Form("hDpKinCalibVSCutID%d",KineID)
				,Form("Dp_Kin for Delta Scan Kine. #%d (Selected Exct. State)",KineID)
				,NDpRange,-DpRange,DpRange
						,NCutID,CutIDRangeLow,CutIDRangeHigh);
		hDpKinAll[KineID]
				= new TH2D(Form("hDpKinAllVSCutID%d",KineID)
				,Form("Dp_Kin for Delta Scan Kine. #%d (All Data)",KineID)
				,NDpRange,-DpRange,DpRange
						,NCutID,CutIDRangeLow,CutIDRangeHigh);

		assert(hDpKinCalib[KineID]);//pointer check
		assert(hDpKinAll[KineID]);//pointer check
	}
	
	for (UInt_t idx = 0; idx<fNRawData; idx++)
	{
		const EventData &eventdata = fRawData[idx];
		const UInt_t ExtraDataFlag = (UInt_t)(eventdata.Data[kExtraDataFlag]);
		assert(ExtraDataFlag==0 || ExtraDataFlag==1); //flag definition consistency check
		const UInt_t KineID = (UInt_t)(eventdata.Data[kKineID]);
		assert(KineID<NKine);

		if (!ExtraDataFlag)
		{
// 			assert((UInt_t)eventdata.Data[kCutID]<NCutID); // cut definition check
			hDpKinCalib[KineID]
					->Fill(eventdata.Data[kCalcDpKin]+eventdata.Data[kRadiLossDp]
					,((UInt_t)eventdata.Data[kCutID])%NCutID);
// 			AverCalcDpKin[KineID]+=
// 					eventdata.Data[kCalcDpKin]+eventdata.Data[kRadiLossDp];
// 			NEvntDpKin[KineID]++;
		}
		hDpKinAll[KineID]
				->Fill(eventdata.Data[kCalcDpKin]+eventdata.Data[kRadiLossDp]
				,((UInt_t)eventdata.Data[kCutID])%NCutID);
		
		RealDpKin[KineID] = eventdata.Data[kRealDpKin]+eventdata.Data[kRadiLossDp];
		
		for (UInt_t ExcitID = 0; ExcitID<NExcitationStates; ExcitID++)
		{
			assert(kRealDpKinExcitations+ExcitID<kRealTh);//index check
			RealDpKinAllExcit[ExcitID][KineID]
					=eventdata.Data[kRealDpKinExcitations+ExcitID];
		}
	}
	
	TCanvas * c1 = new TCanvas("CheckDpVSCutID","Check Dp Kin Reconstruction VS Scattering Angle",1800,900);
	c1->Divide(3,2);
	UInt_t idx=1;
	
	for(UInt_t KineID=0; KineID<NKine; KineID++)
	{
		c1->cd(idx++);
// 		gPad -> SetLogy();

// 		AverCalcDpKin[KineID]/=NEvntDpKin[KineID];
// 		DEBUG_MASSINFO("CheckDp","AverCalcDpKin[%d] = %f"
// 				,KineID,AverCalcDpKin[KineID]);

		// Histograms
// 		hDpKinCalib[KineID]->SetLineColor(4);
// 		hDpKinCalib[KineID]->SetFillColor(4);
// 		hDpKinCalib[KineID]->SetFillStyle(3008);
		
// 		hDpKinAll[KineID]->SetLineColor(1);
// 		hDpKinAll[KineID]->SetFillColor(1);

		const Double_t dpRange=0.01;
		hDpKinCalib[KineID]->
				SetAxisRange(RealDpKin[KineID]-dpRange,RealDpKin[KineID]+dpRange);
		hDpKinAll[KineID]->
				SetAxisRange(RealDpKin[KineID]-dpRange,RealDpKin[KineID]+dpRange);
		
		hDpKinCalib[KineID]
				->SetXTitle("radiation corrected dp_kin (angular independant dp)");
		hDpKinCalib[KineID]
				->SetYTitle("combination of foil and sieve hole ID");
		hDpKinAll[KineID]
				->SetXTitle("radiation corrected dp_kin (angular independant dp)");
		hDpKinAll[KineID]
				->SetYTitle("combination of foil and sieve hole ID");
		
		hDpKinAll[KineID]->Draw("COLZ");
// 		hDpKinCalib[KineID]->Draw("SAME");

		// expectation lines
		const Double_t MinPlot = CutIDRangeLow;
		const Double_t MaxPlot = CutIDRangeHigh;
		for (UInt_t ExcitID = 0; ExcitID<NExcitationStates; ExcitID++)
		{//all exciation states
			const Double_t x = RealDpKinAllExcit[ExcitID][KineID];
			TLine *l = new TLine(x,MinPlot,x,+MaxPlot);
			l->SetLineColor(3);
			l->SetLineWidth(2);
			l->Draw();
		}
		
		for(UInt_t FoilID = 0; FoilID<NFiols; FoilID++)
		{//Foils Divide
			const Double_t y = FoilID*NSieve*NSieve-.5;
			TLine *l = new TLine(RealDpKin[KineID]-dpRange,y,RealDpKin[KineID]+dpRange,y);
			l->SetLineColor(5);
// 			l->SetLineStyle(2);
			l->SetLineWidth(1);
			l->Draw();
		}
		
		TLine *l = new TLine(RealDpKin[KineID],MinPlot,RealDpKin[KineID],+MaxPlot);
		l->SetLineColor(6);
		l->SetLineWidth(2);
		l->Draw();
	}

	
	return c1;

}

//_____________________________________________________________________________
Double_t LOpticsOpt::VerifyMatrix_Sieve(void)
{
	//static summarize difference between tg_th, th_ph caculated from current database and those in root file
	
	Double_t dth = 0, dphi=0;	//Difference
	Double_t rmsth = 0, rmsphi=0; //mean square
	
	Double_t theta, phi/*, dp, p, pathl*/;
	
	for (UInt_t idx = 0; idx<fNRawData; idx++)
	{
		const EventData eventdata = fRawData[idx];

		Double_t x_fp = eventdata.Data[kX];
		const Double_t (*powers)[5] = eventdata.powers;
		
  // calculate the matrices we need
		CalcMatrix(x_fp, fDMatrixElems);
		CalcMatrix(x_fp, fTMatrixElems);
		CalcMatrix(x_fp, fYMatrixElems);
		CalcMatrix(x_fp, fYTAMatrixElems);
		CalcMatrix(x_fp, fPMatrixElems);
		CalcMatrix(x_fp, fPTAMatrixElems);

  // calculate the coordinates at the target
		theta = CalcTargetVar(fTMatrixElems, powers);
		phi = CalcTargetVar(fPMatrixElems, powers)+CalcTargetVar(fPTAMatrixElems,powers);
// 		y = CalcTargetVar(fYMatrixElems, powers)+CalcTargetVar(fYTAMatrixElems,powers);

  // calculate momentum
// 		dp = CalcTargetVar(fDMatrixElems, powers);

		dth += theta - eventdata.Data[kL_tr_tg_th];
		rmsth += (theta - eventdata.Data[kL_tr_tg_th])*(theta - eventdata.Data[kL_tr_tg_th]);

		dphi += phi - eventdata.Data[kL_tr_tg_ph];
		rmsphi += (phi - eventdata.Data[kL_tr_tg_ph])*(phi - eventdata.Data[kL_tr_tg_ph]);
	}

	DEBUG_INFO("VerifyMatrix_Sieve","dth = %f,rmsth=%f",
			   dth/fNRawData,TMath::Sqrt(rmsth/fNRawData));
	DEBUG_INFO("VerifyMatrix_Sieve","dphi = %f, rmsphi=%f",
			   dphi/fNRawData,TMath::Sqrt(rmsphi/fNRawData));

	return TMath::Sqrt(rmsth/fNRawData+rmsphi/fNRawData);
}

//_____________________________________________________________________________
Double_t LOpticsOpt::VerifyMatrix_Vertex(void)
{
	//static summarize difference between tg_y caculated from current database and those in root file
	
	Double_t /*dth = 0, */dtg_y=0;	//Difference
	Double_t /*rmsth = 0, */dtg_y_rms=0; //mean square

	Double_t y/*, dp, p*//*, pathl*/;
	
	for (UInt_t idx = 0; idx<fNRawData; idx++)
	{
		const EventData eventdata = fRawData[idx];

		Double_t x_fp = eventdata.Data[kX];
		const Double_t (*powers)[5] = eventdata.powers;
		
  // calculate the matrices we need
// 		CalcMatrix(x_fp, fDMatrixElems);
// 		CalcMatrix(x_fp, fTMatrixElems);
		CalcMatrix(x_fp, fYMatrixElems);
// 		CalcMatrix(x_fp, fYTAMatrixElems);
// 		CalcMatrix(x_fp, fPMatrixElems);
// 		CalcMatrix(x_fp, fPTAMatrixElems);

  // calculate the coordinates at the target
// 		theta = CalcTargetVar(fTMatrixElems, powers);
// 		phi = CalcTargetVar(fPMatrixElems, powers)+CalcTargetVar(fPTAMatrixElems,powers);
		y = CalcTargetVar(fYMatrixElems, powers)/*+CalcTargetVar(fYTAMatrixElems,powers)*/;

  // calculate momentum
// 		dp = CalcTargetVar(fDMatrixElems, powers);

		dtg_y += y - eventdata.Data[kL_tr_tg_y];
		dtg_y_rms += (y - eventdata.Data[kL_tr_tg_y])*(y - eventdata.Data[kL_tr_tg_y]);
		
		DEBUG_MASSINFO("VerifyMatrix_Vertex","y = %f; eventdata.Data[kL_tr_tg_y] = %f",
					   y , eventdata.Data[kL_tr_tg_y]);
	}

	DEBUG_INFO("VerifyMatrix_Vertex","dtg_y = %f,dtg_y_rms=%f",
			   dtg_y/fNRawData,TMath::Sqrt(dtg_y_rms/fNRawData));

	return TMath::Sqrt(dtg_y_rms/fNRawData);
}

//_____________________________________________________________________________
Double_t LOpticsOpt::VerifyMatrix_Dp(void)
{

	//static summarize difference between tg_dp caculated from current database and those in root file
	
	Double_t d_dp = 0/*, dphi=0*/;	//Difference
	Double_t rms_dp = 0/*, rmsphi=0*/; //mean square

	static UInt_t NCall = 0;
	NCall++;
	
	for (UInt_t idx = 0; idx<fNRawData; idx++)
	{
	
		Double_t  dp, dp_kin;
		
		EventData &eventdata = fRawData[idx];

		Double_t x_fp = eventdata.Data[kX];
		const Double_t (*powers)[5] = eventdata.powers;
		
  // calculate the matrices we need
		CalcMatrix(x_fp, fDMatrixElems);
// 		CalcMatrix(x_fp, fTMatrixElems);
// 		CalcMatrix(x_fp, fYMatrixElems);
// 		CalcMatrix(x_fp, fYTAMatrixElems);
// 		CalcMatrix(x_fp, fPMatrixElems);
// 		CalcMatrix(x_fp, fPTAMatrixElems);

  // calculate the coordinates at the target
// 		theta = CalcTargetVar(fTMatrixElems, powers);
// 		phi = CalcTargetVar(fPMatrixElems, powers)+CalcTargetVar(fPTAMatrixElems,powers);
// 		y = CalcTargetVar(fYMatrixElems, powers)+CalcTargetVar(fYTAMatrixElems,powers);

  // calculate momentum
		dp = CalcTargetVar(fDMatrixElems, powers);
		dp_kin = dp - eventdata.Data[kDpKinOffsets];

		d_dp += dp - eventdata.Data[kL_tr_tg_dp];
		rms_dp += (dp - eventdata.Data[kL_tr_tg_dp])*(dp - eventdata.Data[kL_tr_tg_dp]);

		DEBUG_MASSINFO("SumSquareDp","d_dp = %f = \t%f - \t%f",
					   dp_kin - eventdata.Data[kRealDpKin], dp_kin , eventdata.Data[kRealDpKin]  );
		
	}

	DEBUG_INFO("VerifyMatrix_Dp","(dp_Calc - dp in root tree) = d_dp = %f, rms_d_dp=%f",
			   d_dp/fNRawData,TMath::Sqrt(rms_dp/fNRawData));

	return TMath::Sqrt(rms_dp/fNRawData);
}

//_____________________________________________________________________________
LOpticsOpt::LOpticsOpt( const char* name, const char* description,
		THaApparatus* apparatus ) :
		THaTrackingDetector(name,description,apparatus)
{
  // Constructor

	fPrefix="L.vdc.";
	fCurrentMatrixElems = NULL;

	TVector3 TCSX(0,-1,0);
	TVector3 TCSZ(TMath::Sin(HRSAngle),0,TMath::Cos(HRSAngle));
	TVector3 TCSY = TCSZ.Cross(TCSX);
	fTCSInHCS.RotateAxes(TCSX,TCSY,TCSZ);
	
	fPointingOffset.SetXYZ(
					-MissPointZ*TMath::Cos(HRSAngle)
			,(Double_t)MissPointY
			,MissPointZ*TMath::Sin(HRSAngle)
				   );

	DEBUG_INFO("LOpticsOpt","HRS @ %f Degree, PointingOffset = (%f,%f,%f)",
			   HRSAngle/TMath::Pi()*180,
			   fPointingOffset.X(),fPointingOffset.Y(),fPointingOffset.Z());
	
	fNRawData=0;

	
	for(UInt_t i=0; i<100; i++)
		fArbitaryDpKinShift[i] = fArbitaryVertexShift[i] = 0;
}

//_____________________________________________________________________________
Int_t LOpticsOpt::LoadDataBase( TString DataBaseName )
{
  // Read VDC database

	FILE* file = fopen( DataBaseName,"r" );
	if( !file ) {
		Error("LoadDataBase","%s can not be opened", DataBaseName.Data());
		 return kFileError;
	}
	else DEBUG_INFO("LoadDataBase","Parsing Database %s", DataBaseName.Data());

  // load global VDC parameters
	static const char* const here = "LoadDataBase";
	const int LEN = 200;
	char buff[LEN];
  
  // Look for the section [<prefix>.global] in the file, e.g. [ R.global ]
	TString tag(fPrefix);
	Ssiz_t pos = tag.Index(".");
	if( pos != kNPOS )
		tag = tag(0,pos+1);
	else
		tag.Append(".");
	tag.Prepend("[");
	tag.Append("global]");
	TString line, tag2(tag);
	tag.ToLower();

	bool found = false;
	while (!found && fgets (buff, LEN, file) != NULL) {
		char* buf = ::Compress(buff);  //strip blanks
		line = buf;
		delete [] buf;
		if( line.EndsWith("\n") ) line.Chop();
		line.ToLower();
		if ( tag == line )
			found = true;
	}
	if( !found ) {
		Error(Here(here), "Database entry %s not found!", tag2.Data() );
		fclose(file);
		return kInitError;
	}

  // We found the section, now read the data

  // read in some basic constants first
  //  fscanf(file, "%lf", &fSpacing);
  // fSpacing is calculated from the actual z-positions in Init()
	fgets(buff, LEN, file); // Skip rest of line
 
  // Read in the focal plane transfer elements
  // For fine-tuning of these data, we seek to a matching time stamp, or
  // if no time stamp found, to a "configuration" section. Examples:
	//
  // [ 2002-10-10 15:30:00 ]
  // comment line goes here
  // t 0 0 0  ...
  // y 0 0 0  ... etc.
	//
  // or
	//
  // [ config=highmom ]
  // comment line
  // t 0 0 0  ... etc.
	//
// 	if( SeekDBdate( file, date ) == 0 && fConfig.Length() > 0 &&
// 		   SeekDBconfig( file, fConfig.Data() )) {}

	fgets(buff, LEN, file); // Skip comment line

	fTMatrixElems.clear();
	fDMatrixElems.clear();
	fPMatrixElems.clear();
	fPTAMatrixElems.clear();
	fYMatrixElems.clear();
	fYTAMatrixElems.clear();
	fLMatrixElems.clear();
  
	fFPMatrixElems.clear();
	fFPMatrixElems.resize(3);

	typedef vector<string>::size_type vsiz_t;
	map<string,vsiz_t> power;
	power["t"] = 3;  // transport to focal-plane tensors
	power["y"] = 3;
	power["p"] = 3;
	power["D"] = 3;  // focal-plane to target tensors
	power["T"] = 3;
	power["Y"] = 3;
	power["YTA"] = 4;
	power["P"] = 3;
	power["PTA"] = 4;
	power["L"] = 4;  // pathlength from z=0 (target) to focal plane (meters)
	power["XF"] = 5; // forward: target to focal-plane (I think)
	power["TF"] = 5;
	power["PF"] = 5;
	power["YF"] = 5;
  
	map<string,vector<THaMatrixElement>*> matrix_map;
	matrix_map["t"] = &fFPMatrixElems;
	matrix_map["y"] = &fFPMatrixElems;
	matrix_map["p"] = &fFPMatrixElems;
	matrix_map["D"] = &fDMatrixElems;
	matrix_map["T"] = &fTMatrixElems;
	matrix_map["Y"] = &fYMatrixElems;
	matrix_map["YTA"] = &fYTAMatrixElems;
	matrix_map["P"] = &fPMatrixElems;
	matrix_map["PTA"] = &fPTAMatrixElems;
	matrix_map["L"] = &fLMatrixElems;

	map <string,int> fp_map;
	fp_map["t"] = 0;
	fp_map["y"] = 1;
	fp_map["p"] = 2;

  // Read in as many of the matrix elements as there are.
  // Read in line-by-line, so as to be able to handle tensors of
  // different orders.
	while( fgets(buff, LEN, file) ) {
		string line(buff);
    // Erase trailing newline
		if( line.size() > 0 && line[line.size()-1] == '\n' ) {
			buff[line.size()-1] = 0;
			line.erase(line.size()-1,1);
		}
    // Split the line into whitespace-separated fields    
		vector<string> line_spl = Split(line);

    // Stop if the line does not start with a string referring to
    // a known type of matrix element. In particular, this will
    // stop on a subsequent timestamp or configuration tag starting with "["
		if(line_spl.empty())
			continue; //ignore empty lines
		const char* w = line_spl[0].c_str();
		vsiz_t npow = power[w];
		if( npow == 0 )
			break;


#if DEBUG_LEVEL>=4
		cout<<"Matrix Line = ";
		for (pos=1; (UInt_t)pos<(UInt_t)line_spl.size(); pos++) {
			cout<< pos <<"("<<line_spl[pos].c_str()<<"), ";
		}
		cout<<endl;
#endif

    // Looks like a good line, go parse it.
		THaMatrixElement ME;
		ME.pw.resize(npow);
		ME.iszero = true;  ME.order = 0;
		vsiz_t pos;
		for (pos=1; pos<=npow && pos<line_spl.size(); pos++) {
			ME.pw[pos-1] = atoi(line_spl[pos].c_str());
		}
		vsiz_t p_cnt;
		for ( p_cnt=0; pos<line_spl.size() && p_cnt<kPORDER && pos<=npow+kPORDER;
					pos++,p_cnt++ )
		{
			ME.poly[p_cnt] = atof(line_spl[pos].c_str());
			if (ME.poly[p_cnt] != 0.0) {
				ME.iszero = false;
				ME.order = p_cnt+1;
			}
		}
		if (p_cnt < 1) {
			Error(Here(here), "Could not read in Matrix Element %s%d%d%d!",
				  w, ME.pw[0], ME.pw[1], ME.pw[2]);
			Error(Here(here), "Line looks like: %s",line.c_str());
			fclose(file);
			return kInitError;
		}

		//order optimize to 
		ME.OptOrder = atoi(line_spl[line_spl.size()-1].c_str());
		
    // Don't bother with all-zero matrix elements
		if( ME.iszero )
			continue;
    
    // Add this matrix element to the appropriate array
		vector<THaMatrixElement> *mat = matrix_map[w];
		if (mat) {
      // Special checks for focal plane matrix elements
			if( mat == &fFPMatrixElems ) {
				if( ME.pw[0] == 0 && ME.pw[1] == 0 && ME.pw[2] == 0 ) {
					THaMatrixElement& m = (*mat)[fp_map[w]];
					if( m.order > 0 ) {
						Warning(Here(here), "Duplicate definition of focal plane "
								"matrix element: %s. Using first definition.", buff);
					} else
						m = ME;
				} else
					Warning(Here(here), "Bad coefficients of focal plane matrix "
							"element %s", buff);
			}
			else {
	// All other matrix elements are just appended to the respective array
	// but ensure that they are defined only once!
				bool match = false;
				for( vector<THaMatrixElement>::iterator it = mat->begin();
								 it != mat->end() && !(match = it->match(ME)); it++ ) {}
				if( match ) {
					Warning(Here(here), "Duplicate definition of "
							"matrix element: %s. Using first definition.", buff);
				} else
					mat->push_back(ME);
			}
		}
		else if ( fDebug > 0 )
			Warning(Here(here), "Not storing matrix for: %s !", w);
    
	} //while(fgets)

//   // Compute derived quantities and set some hardcoded parameters
//   const Double_t degrad = TMath::Pi()/180.0;
//   fTan_vdc  = fFPMatrixElems[T000].poly[0];
//   fVDCAngle = TMath::ATan(fTan_vdc);
//   fSin_vdc  = TMath::Sin(fVDCAngle);
//   fCos_vdc  = TMath::Cos(fVDCAngle);
	//
//   // Define the VDC coordinate axes in the "detector system". By definition,
//   // the detector system is identical to the VDC origin in the Hall A HRS.
//   DefineAxes(0.0*degrad);
	//
//   fNumIter = 1;      // Number of iterations for FineTrack()
//   fErrorCutoff = 1e100;
	//
//   // figure out the track length from the origin to the s1 plane
	//
//   // since we take the VDC to be the origin of the coordinate
//   // space, this is actually pretty simple
//   const THaDetector* s1 = GetApparatus()->GetDetector("s1");
//   if(s1 == NULL)
//     fCentralDist = 0;
//   else
//     fCentralDist = s1->GetOrigin().Z();

	CalcMatrix(1.,fLMatrixElems); // tensor without explicit polynomial in x_fp
  

	fIsInit = true;
	fclose(file);
	return kOK;
}

//_____________________________________________________________________________
LOpticsOpt::~LOpticsOpt()
{
  // Destructor.

}

//_____________________________________________________________________________
void LOpticsOpt::CalcTargetCoords(THaTrack *track, const ECoordTypes mode)
{
  // calculates target coordinates from focal plane coordinates

// 	const Int_t kNUM_PRECOMP_POW = 10;

	Double_t x_fp, y_fp, th_fp, ph_fp;
	Double_t powers[kNUM_PRECOMP_POW][5];  // {(x), th, y, ph, abs(th) }
	Double_t x, y, theta, phi, dp, p, pathl;

  // first select the coords to use
	if(mode == kTransport) {
		x_fp = track->GetX();
		y_fp = track->GetY();
		th_fp = track->GetTheta();
		ph_fp = track->GetPhi();
	} else {//if(mode == kRotatingTransport) {
		x_fp = track->GetRX();
		y_fp = track->GetRY();
		th_fp = track->GetRTheta();
		ph_fp = track->GetRPhi();
	}

  // calculate the powers we need
	for(int i=0; i<kNUM_PRECOMP_POW; i++) {
		powers[i][0] = pow(x_fp, i);
		powers[i][1] = pow(th_fp, i);
		powers[i][2] = pow(y_fp, i);
		powers[i][3] = pow(ph_fp, i);
		powers[i][4] = pow(TMath::Abs(th_fp),i);
	}

  // calculate the matrices we need
	CalcMatrix(x_fp, fDMatrixElems);
	CalcMatrix(x_fp, fTMatrixElems);
	CalcMatrix(x_fp, fYMatrixElems);
	CalcMatrix(x_fp, fYTAMatrixElems);
	CalcMatrix(x_fp, fPMatrixElems);
	CalcMatrix(x_fp, fPTAMatrixElems);

  // calculate the coordinates at the target
	theta = CalcTargetVar(fTMatrixElems, powers);
	phi = CalcTargetVar(fPMatrixElems, powers)+CalcTargetVar(fPTAMatrixElems,powers);
	y = CalcTargetVar(fYMatrixElems, powers)+CalcTargetVar(fYTAMatrixElems,powers);

	THaSpectrometer *app = static_cast<THaSpectrometer*>(GetApparatus());
  // calculate momentum
	dp = CalcTargetVar(fDMatrixElems, powers);
	p  = app->GetPcentral() * (1.0+dp);

  // pathlength matrix is for the Transport coord plane
	pathl = CalcTarget2FPLen(fLMatrixElems, powers);

  //FIXME: estimate x ??
	x = 0.0;

  // Save the target quantities with the tracks
	track->SetTarget(x, y, theta, phi);
	track->SetDp(dp);
	track->SetMomentum(p);
	track->SetPathLen(pathl);
  
	app->TransportToLab( p, theta, phi, track->GetPvect() );

}

//_____________________________________________________________________________
void LOpticsOpt::CalcMatrix( const Double_t x, vector<THaMatrixElement>& matrix )
{
  // calculates the values of the matrix elements for a given location
  // by evaluating a polynomial in x of order it->order with 
  // coefficients given by it->poly

	for( vector<THaMatrixElement>::iterator it=matrix.begin();
			it!=matrix.end(); it++ ) {
				it->v = 0.0;

				if(it->order > 0) {
					for(int i=it->order-1; i>=1; i--)
						it->v = x * (it->v + it->poly[i]);
					it->v += it->poly[0];
				}
			}
}

//_____________________________________________________________________________
Double_t LOpticsOpt::CalcTargetVar(const vector<THaMatrixElement>& matrix,
								   const Double_t powers[][5])
{
  // calculates the value of a variable at the target
  // the x-dependence is already in the matrix, so only 1-3 (or np) used
	Double_t retval=0.0;
	Double_t v=0;
	for( vector<THaMatrixElement>::const_iterator it=matrix.begin();
			it!=matrix.end(); it++ )
		if(it->v != 0.0) {
		v = it->v;
		unsigned int np = it->pw.size(); // generalize for extra matrix elems.
		for (unsigned int i=0; i<np; i++)
			v *= powers[it->pw[i]][i+1];
		retval += v;
  //      retval += it->v * powers[it->pw[0]][1] 
  //	              * powers[it->pw[1]][2]
  //	              * powers[it->pw[2]][3];
		}

		return retval;
}

//_____________________________________________________________________________
Double_t LOpticsOpt::CalcTarget2FPLen(const vector<THaMatrixElement>& matrix,
									  const Double_t powers[][5])
{
  // calculates distance from the nominal target position (z=0)
  // to the transport plane

	Double_t retval=0.0;
	for( vector<THaMatrixElement>::const_iterator it=matrix.begin();
			it!=matrix.end(); it++ )
		if(it->v != 0.0)
			retval += it->v * powers[it->pw[0]][0]
					* powers[it->pw[1]][1]
					* powers[it->pw[2]][2]
					* powers[it->pw[3]][3];

	return retval;
}

//_____________________________________________________________________________
UInt_t LOpticsOpt::LoadRawData(TString DataFileName,UInt_t NLoad)
{
	//load "f51" ascii data file to Rawdata[]
	
	DEBUG_INFO("LoadRawData","Loading %s",DataFileName.Data());

			
	FILE* file = fopen( DataFileName,"r" );
	if( !file ) return kFileError;

	UInt_t NRead = 0;
	const int LEN = 2000;
	char buff[LEN];

	Double_t NDataRead = 0;
	
	while( fgets(buff, LEN, file) ) {

		assert(NRead<MaxNRawData);//too much data if fails

		if (NRead>=NLoad) continue;
		
		Double_t * eventdata = fRawData[NRead].Data;
		
		string line(buff);
    // Erase trailing newline
		if( line.size() > 0 && line[line.size()-1] == '\n' ) {
			buff[line.size()-1] = 0;
			line.erase(line.size()-1,1);
		}
    // Split the line into whitespace-separated fields    
		vector<string> line_spl = Split(line);

		assert(line_spl.size()<=MaxNEventData);//array size check
		for(UInt_t idx = 0; idx<line_spl.size(); idx++)
			eventdata[idx] = atof(line_spl[idx].c_str());
		NDataRead+=line_spl.size();

		Double_t (*powers)[5] = fRawData[NRead].powers;
		Double_t x_fp = 	eventdata[kX];
		Double_t th_fp =	eventdata[kTh];
		Double_t y_fp =		eventdata[kY];
		Double_t ph_fp = 	eventdata[kPhi];
		
  // calculate the powers we need
		for(int i=0; i<kNUM_PRECOMP_POW; i++) {
			powers[i][0] = pow(x_fp, i);
			powers[i][1] = pow(th_fp, i);
			powers[i][2] = pow(y_fp, i);
			powers[i][3] = pow(ph_fp, i);
			powers[i][4] = pow(TMath::Abs(th_fp),i);
		}

		
		NRead++;
	}
	
	fclose(file);
	fNRawData = NRead;
	fNCalibData = NRead; //fNCalibData shall be updated later if only part of data read in is for calibration use
	
	DEBUG_INFO("LoadRawData","%d events x %f record/event read from %s",
			   fNRawData,NDataRead/fNRawData,DataFileName.Data());
	return NRead;
}

UInt_t LOpticsOpt::SaveDataBuffer(TTree * T)
{
	//save Rawdata[] to T Tree. return N event written

	DEBUG_INFO("SaveDataBuffer","Saving Data buffer to Tree %s ... ", T->GetName());
	
	EventData eventdata;
	
	T->Branch("CutID",&(eventdata.Data[kCutID]),"CutID/D");
	T->Branch("X",&(eventdata.Data[kX]),"X/D");
	T->Branch("Th",&(eventdata.Data[kTh]),"Th/D");
	T->Branch("Y",&(eventdata.Data[kY]),"Y/D");
	T->Branch("Phi",&(eventdata.Data[kPhi]),"Phi/D");
	T->Branch("BeamX",&(eventdata.Data[kBeamX]),"BeamX/D");
	T->Branch("BeamY",&(eventdata.Data[kBeamY]),"BeamY/D");
	T->Branch("L_tr_tg_th",&(eventdata.Data[kL_tr_tg_th]),"L_tr_tg_th/D");
	T->Branch("L_tr_tg_ph",&(eventdata.Data[kL_tr_tg_ph]),"L_tr_tg_ph/D");
	
	T->Branch("RealTh",&(eventdata.Data[kRealTh]),"RealTh/D");
	T->Branch("RealPhi",&(eventdata.Data[kRealPhi]),"RealPhi/D");
	T->Branch("RealTgX",&(eventdata.Data[kRealTgX]),"RealTgX/D");
	T->Branch("RealThMatrix",&(eventdata.Data[kRealThMatrix]),"RealThMatrix/D");
	T->Branch("CalcTh",&(eventdata.Data[kCalcTh]),"CalcTh/D");
	T->Branch("CalcPh",&(eventdata.Data[kCalcPh]),"CalcPh/D");
	
	T->Branch("L_tr_tg_y",&(eventdata.Data[kL_tr_tg_y]),"L_tr_tg_y/D");
	T->Branch("RealTgY",&(eventdata.Data[kRealTgY]),"RealTgY/D");
	T->Branch("RealReactZ",&(eventdata.Data[kRealReactZ]),"RealReactZ/D");
	T->Branch("CalcTgY",&(eventdata.Data[kCalcTgY]),"CalcTgY/D");
	T->Branch("CalcReactZ",&(eventdata.Data[kCalcReactZ]),"CalcReactZ/D");
	
	T->Branch("L_tr_tg_dp",&(eventdata.Data[kL_tr_tg_dp]),"L_tr_tg_dp/D");
	T->Branch("L_tr_p",&(eventdata.Data[kL_tr_p]),"L_tr_p/D");
	T->Branch("urb_e",&(eventdata.Data[kurb_e]),"urb_e/D");
	T->Branch("RunNum",&(eventdata.Data[kRunNum]),"RunNum/D");
	T->Branch("ExtraDataFlag",&(eventdata.Data[kExtraDataFlag]),"ExtraDataFlag/D");
	T->Branch("KineID",&(eventdata.Data[kKineID]),"KineID/D");
	T->Branch("Centralp",&(eventdata.Data[kCentralp]),"Centralp/D");
	T->Branch("RadiLossDp",&(eventdata.Data[kRadiLossDp]),"RadiLossDp/D");
	T->Branch("ScatterAngle",&(eventdata.Data[kScatterAngle]),"ScatterAngle/D");
	T->Branch("DpKinOffsets",&(eventdata.Data[kDpKinOffsets]),"DpKinOffsets/D");
	T->Branch("RealDpKin",&(eventdata.Data[kRealDpKin]),"RealDpKin/D");
	T->Branch("RealDpKinMatrix",&(eventdata.Data[kRealDpKinMatrix]),"RealDpKinMatrix/D");
	T->Branch("CalcDpKinMatrix",&(eventdata.Data[kCalcDpKinMatrix]),"CalcDpKinMatrix/D");
	T->Branch("CalcDpKin",&(eventdata.Data[kCalcDpKin]),"CalcDpKin/D");
	T->Branch("RealDpKinExcitations",&(eventdata.Data[kRealDpKinExcitations]),"RealDpKinExcitations/D");

	UInt_t idx=0;
	for ( idx = 0; idx<fNRawData; idx++)
	{
		eventdata = fRawData[idx];
		T->Fill();
	}

	T -> Write();
	
	DEBUG_INFO("SaveDataBuffer","Done : Saving Data buffer to Tree");
	return idx;
}

UInt_t LOpticsOpt::SaveDataBuffer(TString fname, TString tree)
{
	DEBUG_INFO("SaveDataBuffer","Saving Data buffer to File %s",fname.Data());

	TFile *f = new TFile(fname,"recreate");
	assert(f);
	f->cd();
	
	TTree *T = new TTree(tree.Data(),"Data buffer of HRS Optics Optimizer");
	assert(T);
	UInt_t cnt = SaveDataBuffer( T );

	f->cd();
	f->Write();

	return cnt;
}

//_____________________________________________________________________________
UInt_t LOpticsOpt::Matrix2Array(Double_t Array[], const std::vector<THaMatrixElement> &Matrix, Bool_t FreeParaFlag[])
{
	  //Matrix -> Array
// 	DEBUG_INFO("Matrix2Array","Entry Point");
	typedef vector<THaMatrixElement>::size_type vsiz_t;

	UInt_t idx =0;
	
	for (vsiz_t i=0; i<Matrix.size(); i++) {
		const THaMatrixElement& m = Matrix[i];
		UInt_t j;
		for (j=0; (int)j<m.order; j++) {
			if (FreeParaFlag) FreeParaFlag[idx] = j<m.OptOrder?kTRUE:kFALSE;
			Array[idx++]=m.poly[j];
		}
		for (; j<kPORDER; j++) {
			if (FreeParaFlag) FreeParaFlag[idx] = j<m.OptOrder?kTRUE:kFALSE;
			Array[idx++]=0;
		}
	}

	DEBUG_INFO("Matrix2Array","Fill Size = %d",idx);

	return idx;
}

//_____________________________________________________________________________
UInt_t LOpticsOpt::Array2Matrix(const Double_t Array[], std::vector<THaMatrixElement> &Matrix)
{
	//Array -> fCurrentMatrixElems
// 	DEBUG_INFO("Array2Matrix","Entry Point");
	typedef vector<THaMatrixElement>::size_type vsiz_t;

	UInt_t idx =0;
	
	for (vsiz_t i=0; i<Matrix.size(); i++) {
		THaMatrixElement& m = Matrix[i];
		int j;
		m.order = kPORDER;
		for (j=0; j<m.order; j++) {
			m.poly[j]=Array[idx++];
		}
		m.SkimPoly();
	}

	DEBUG_INFO("Array2Matrix","Load Size = %d",idx);

	return idx;
}

//_____________________________________________________________________________
void LOpticsOpt::Print(const Option_t* opt) const
{
	//Print current matrix
	
	THaTrackingDetector::Print(opt);
	typedef vector<THaMatrixElement>::size_type vsiz_t;
	
  // Print out the optics matrices, to verify they make sense
// 	printf("Matrix FP (t000, y000, p000)\n");
// 	for (vsiz_t i=0; i<fFPMatrixElems.size(); i++) {
// 		const THaMatrixElement& m = fFPMatrixElems[i];
// 		for (vsiz_t j=0; j<m.pw.size(); j++) {
// 			printf("  %2d",m.pw[j]);
// 		}
// 		for (int j=0; j<m.order; j++) {
// 			printf("  %g",m.poly[j]);
// 		}
// 		printf(" : Opt -> %d",m.OptOrder);
// 		printf("\n");
// 	}

	printf("LOpticsOpt::Print: Transport Matrix:  D-terms\n");
	for (vsiz_t i=0; i<fDMatrixElems.size(); i++) {
		const THaMatrixElement& m = fDMatrixElems[i];
		for (vsiz_t j=0; j<m.pw.size(); j++) {
			printf("  %2d",m.pw[j]);
		}
		for (int j=0; j<m.order; j++) {
			printf("\t%g",m.poly[j]);
		}
		printf(" : Opt -> %d",m.OptOrder);
		if ((UInt_t)m.order!=m.OptOrder) printf(" != Matrix Order !!");
		printf("\n");
	}

	printf("LOpticsOpt::Print: Transport Matrix:  T-terms\n");
	for (vsiz_t i=0; i<fTMatrixElems.size(); i++) {
		const THaMatrixElement& m = fTMatrixElems[i];
		for (vsiz_t j=0; j<m.pw.size(); j++) {
			printf("  %2d",m.pw[j]);
		}
		for (int j=0; j<m.order; j++) {
			printf("\t%g",m.poly[j]);
		}
		printf(" : Opt -> %d",m.OptOrder);
		if ((UInt_t)m.order!=m.OptOrder) printf(" != Matrix Order !!");
		printf("\n");
	}

	printf("LOpticsOpt::Print: Transport Matrix:  Y-terms\n");
	for (vsiz_t i=0; i<fYMatrixElems.size(); i++) {
		const THaMatrixElement& m = fYMatrixElems[i];
		for (vsiz_t j=0; j<m.pw.size(); j++) {
			printf("  %2d",m.pw[j]);
		}
		for (int j=0; j<m.order; j++) {
			printf("\t%g",m.poly[j]);
		}
		printf(" : Opt -> %d",m.OptOrder);
		if ((UInt_t)m.order!=m.OptOrder) printf(" != Matrix Order !!");
		printf("\n");
	}

// 	printf("Transport Matrix:  YTA-terms (abs(theta))\n");
// 	for (vsiz_t i=0; i<fYTAMatrixElems.size(); i++) {
// 		const THaMatrixElement& m = fYTAMatrixElems[i];
// 		for (vsiz_t j=0; j<m.pw.size(); j++) {
// 			printf("  %2d",m.pw[j]);
// 		}
// 		for (int j=0; j<m.order; j++) {
// 			printf("\t%g",m.poly[j]);
// 		}
// 		printf(" : Opt -> %d",m.OptOrder);
// 		printf("\n");
// 	}

	printf("LOpticsOpt::Print: Transport Matrix:  P-terms\n");
	for (vsiz_t i=0; i<fPMatrixElems.size(); i++) {
		const THaMatrixElement& m = fPMatrixElems[i];
		for (vsiz_t j=0; j<m.pw.size(); j++) {
			printf("  %2d",m.pw[j]);
		}
		for (int j=0; j<m.order; j++) {
			printf("\t%g",m.poly[j]);
		}
		printf(" : Opt -> %d",m.OptOrder);
		if ((UInt_t)m.order!=m.OptOrder) printf(" != Matrix Order !!");
		printf("\n");
	}

// 	printf("Transport Matrix:  PTA-terms\n");
// 	for (vsiz_t i=0; i<fPTAMatrixElems.size(); i++) {
// 		const THaMatrixElement& m = fPTAMatrixElems[i];
// 		for (vsiz_t j=0; j<m.pw.size(); j++) {
// 			printf("  %2d",m.pw[j]);
// 		}
// 		for (int j=0; j<m.order; j++) {
// 			printf("\t%g",m.poly[j]);
// 		}
// 		printf(" : Opt -> %d",m.OptOrder);
// 		printf("\n");
// 	}

// 	printf("Matrix L\n");
// 	for (vsiz_t i=0; i<fLMatrixElems.size(); i++) {
// 		const THaMatrixElement& m = fLMatrixElems[i];
// 		for (vsiz_t j=0; j<m.pw.size(); j++) {
// 			printf("  %2d",m.pw[j]);
// 		}
// 		for (int j=0; j<m.order; j++) {
// 			printf("\t%g",m.poly[j]);
// 		}
// // 		printf(" : Opt -> %d",m.OptOrder);
// 		printf("\n");
// 	}

	printf("fArbitaryVertexShift[%d] = {",NFiols);
	for(UInt_t FoilID = 0; FoilID<NFiols; FoilID++)
		printf("%f  ",fArbitaryVertexShift[FoilID]);
	printf("}\n");
	printf("fArbitaryDpKinShift[%d] = {",NKine);
	for(UInt_t KineID=0; KineID<NKine; KineID++)
		printf("%f  ",fArbitaryDpKinShift[KineID]);
	printf("}\n");
	
	return;
}

//_____________________________________________________________________________
Int_t LOpticsOpt::SaveDataBase(TString DataBaseName)
{
	
	//output database in memory to new database file
	//WARNING: Hard coded text included
	
	DEBUG_INFO("SaveDataBase","Saving to %s",DataBaseName.Data());

	typedef vector<THaMatrixElement>::size_type vsiz_t;
	
	FILE* file = fopen( DataBaseName,"w" );
	if( !file ) {
		Info("SaveDataBase","Error Openin %s",DataBaseName.Data());
		return kFileError;
	}

	//Header Part
	// 	[ L.global ]
	// 	0.3327 1 0.0 270.2 0.0 -1.6e-03        VDC Angle, Plane Spacing, Gamma Coefficents
	// 	matrix elements
	// 	t 0 0 0  -1.001135e+00 -3.313373e-01 -4.290819e-02  4.470852e-03  0.000000e+00  0.000000e+00  0.000000e+00  0
	// 			y 0 0 0  -8.060915e-03  1.071977e-03  9.019102e-04 -3.239615e-04  0.000000e+00  0.000000e+00  0.000000e+00  0
	// 			p 0 0 0  -2.861912e-03 -2.469069e-03  8.427172e-03  2.274635e-03  0.000000e+00  0.000000e+00  0.000000e+00  0
	fprintf(file,"[ L.global ]");fprintf(file,"\n");
	fprintf(file,"0.3327 1 0.0 270.2 0.0 -1.6e-03        VDC Angle, Plane Spacing, Gamma Coefficents");fprintf(file,"\n");
	fprintf(file,"matrix elements");fprintf(file,"\n");
	fprintf(file,"t 0 0 0  -1.001135e+00 -3.313373e-01 -4.290819e-02  4.470852e-03  0.000000e+00  0.000000e+00  0.000000e+00  0");fprintf(file,"\n");
	fprintf(file,"y 0 0 0  -8.060915e-03  1.071977e-03  9.019102e-04 -3.239615e-04  0.000000e+00  0.000000e+00  0.000000e+00  0");fprintf(file,"\n");
	fprintf(file,"p 0 0 0  -2.861912e-03 -2.469069e-03  8.427172e-03  2.274635e-03  0.000000e+00  0.000000e+00  0.000000e+00  0");fprintf(file,"\n");
	

	DEBUG_INFO("SaveDataBase","Transport Matrix:  D-terms");
	for (vsiz_t i=0; i<fDMatrixElems.size(); i++) {
		fprintf(file,"D ");
		const THaMatrixElement& m = fDMatrixElems[i];
		for (vsiz_t j=0; j<m.pw.size(); j++) {
			fprintf(file,"%d ",m.pw[j]);
		}
		int j;
		for (j=0; j<m.order; j++) {
			fprintf(file," %13.6e",m.poly[j]);
		}
		for (; j<kPORDER; j++) {
			fprintf(file," %13.6e",0.0);
		}
		fprintf(file,"  %d",m.OptOrder);
		fprintf(file,"\n");
	}

	DEBUG_INFO("SaveDataBase","Transport Matrix:  T-terms");
	for (vsiz_t i=0; i<fTMatrixElems.size(); i++) {
		fprintf(file,"T ");
		const THaMatrixElement& m = fTMatrixElems[i];
		for (vsiz_t j=0; j<m.pw.size(); j++) {
			fprintf(file,"%d ",m.pw[j]);
		}
		int j;
		for (j=0; j<m.order; j++) {
			fprintf(file," %13.6e",m.poly[j]);
		}
		for (; j<kPORDER; j++) {
			fprintf(file," %13.6e",0.0);
		}
		fprintf(file,"  %d",m.OptOrder);
		fprintf(file,"\n");
	}

	DEBUG_INFO("SaveDataBase","Transport Matrix:  P-terms");
	for (vsiz_t i=0; i<fPMatrixElems.size(); i++) {
		fprintf(file,"P ");
		const THaMatrixElement& m = fPMatrixElems[i];
		for (vsiz_t j=0; j<m.pw.size(); j++) {
			fprintf(file,"%d ",m.pw[j]);
		}
		int j;
		for (j=0; j<m.order; j++) {
			fprintf(file," %13.6e",m.poly[j]);
		}
		for (; j<kPORDER; j++) {
			fprintf(file," %13.6e",0.0);
		}
		fprintf(file,"  %d",m.OptOrder);
		fprintf(file,"\n");
	}
	
	DEBUG_INFO("SaveDataBase","Transport Matrix:  Y-terms");
	for (vsiz_t i=0; i<fYMatrixElems.size(); i++) {
		fprintf(file,"Y ");
		const THaMatrixElement& m = fYMatrixElems[i];
		for (vsiz_t j=0; j<m.pw.size(); j++) {
			fprintf(file,"%d ",m.pw[j]);
		}
		int j;
		for (j=0; j<m.order; j++) {
			fprintf(file," %13.6e",m.poly[j]);
		}
		for (; j<kPORDER; j++) {
			fprintf(file," %13.6e",0.0);
		}
		fprintf(file,"  %d",m.OptOrder);
		fprintf(file,"\n");
	}
	
	// L and XF Matrix
	fprintf(file,"L 0 0 0 0  25.713");fprintf(file,"\n");
	fprintf(file,"L 1 0 0 0  0.1650 ");fprintf(file,"\n");
	fprintf(file,"L 2 0 0 0 -0.05");fprintf(file,"\n");
	fprintf(file,"L 0 1 0 0 -11.6554");fprintf(file,"\n");
	fprintf(file,"L 0 2 0 0 -9.4951");fprintf(file,"\n");
	fprintf(file,"L 0 0 1 0  0.0");fprintf(file,"\n");
	fprintf(file,"L 0 0 2 0  0.0");fprintf(file,"\n");
	fprintf(file,"L 0 0 0 1  0.0");fprintf(file,"\n");
	fprintf(file,"L 0 0 0 2  0.0");fprintf(file,"\n");
	fprintf(file,"XF 1 0 0 0 0 -2.181E+00");fprintf(file,"\n");
	fprintf(file,"XF 0 1 0 0 0 -1.980E-01");fprintf(file,"\n");
	fprintf(file,"XF 0 0 0 0 1  1.191E+01");fprintf(file,"\n");
	fprintf(file,"TF 1 0 0 0 0 -1.000E-01");fprintf(file,"\n");
	fprintf(file,"TF 0 1 0 0 0 -4.690E-01");fprintf(file,"\n");
	fprintf(file,"TF 0 0 0 0 1  1.967E+00");fprintf(file,"\n");
	fprintf(file,"PF 0 0 1 0 0  3.630E-01");fprintf(file,"\n");
	fprintf(file,"PF 0 0 0 1 0 -0.902E+00");fprintf(file,"\n");
	fprintf(file,"YF 0 0 1 0 0 -5.950E-01");fprintf(file,"\n");
	fprintf(file,"YF 0 0 0 1 0 -1.274E+00");fprintf(file,"\n");


	fclose(file);

	return kOK;
}

//_____________________________________________________________________________
bool THaMatrixElement::match(const THaMatrixElement& rhs) const
{
  // Compare coefficients of this matrix element to another

	if( pw.size() != rhs.pw.size() )
		return false;
	for( vector<int>::size_type i=0; i<pw.size(); i++ ) {
		if( pw[i] != rhs.pw[i] )
			return false;
	}
	return true;
}
//_____________________________________________________________________________
void THaMatrixElement::SkimPoly()
{
  //reduce order to highest non-zero poly

	if (iszero) return;

	while(!poly[order-1] && order >0)
	{
		poly.pop_back();
		order = order-1;
	}

	if (order==0) iszero = kTRUE;
}

////////////////////////////////////////////////////////////////////////////////
//_____________________________________________________________________________
THaAnalysisObject::EStatus LOpticsOpt::Init( const TDatime& /*date*/ )
{
  // Initialize VDC. Calls standard Init(), then initializes subdetectors.


	return fStatus = kOK;
}

//_____________________________________________________________________________
Int_t LOpticsOpt::ConstructTracks( TClonesArray* /*tracks*/, Int_t /*mode*/ )
{

	return 0;
}

//_____________________________________________________________________________
void LOpticsOpt::Clear( Option_t* /*opt*/ )
{
}

//_____________________________________________________________________________
Int_t LOpticsOpt::Decode( const THaEvData& /*evdata*/ )
{
	return 0;
}

//_____________________________________________________________________________
Int_t LOpticsOpt::CoarseTrack( TClonesArray& /*tracks*/ )
{
	return 0;
}

//_____________________________________________________________________________
Int_t LOpticsOpt::FineTrack( TClonesArray& /*tracks*/ )
{

	return 0;
}

//_____________________________________________________________________________
Int_t LOpticsOpt::FindVertices( TClonesArray& tracks )
{
  // Calculate the target location and momentum at the target.
  // Assumes that CoarseTrack() and FineTrack() have both been called.

	Int_t n_exist = tracks.GetLast()+1;
	for( Int_t t = 0; t < n_exist; t++ ) {
		THaTrack* theTrack = static_cast<THaTrack*>( tracks.At(t) );
		CalcTargetCoords(theTrack, kRotatingTransport);
	}

	return 0;
}

ClassImp(LOpticsOpt);