//-----------------------------------------------------------
// File and Version Information:
// $Id$
//
// Description:
//      Implementation of class TpcInhFieldDrifter
//     
// Environment:
//      Software developed for the PANDA Detector at FAIR.
//
// Author List:
//     	Felix Boehmer           TUM         (original author)
//	
//-----------------------------------------------------------


// This Class' Header ------------------
#include "TpcInhFieldDrifter.h"

// Collaborating Class Headers --------

#include <iostream>
#include <iomanip>
#include <fstream>
#include <cmath>
#include <assert.h>
#include <ostream>
#include <sstream>
#include "TpcEFieldCyl.h"
#include "TpcGas.h"
#include "FairParAsciiFileIo.h"
#include "FairRunAna.h"
#include "FairRunSim.h"
#include "FairParSet.h"
#include "FairRuntimeDb.h"
#include "TpcDigiPar.h"
#include "TpcRungeKutta.h"
#include "PndMultiField.h"
#include "FairField.h"
#include "TpcDigiMapper.h"

ClassImp(TpcInhFieldDrifter)


// Class Member definitions -----------

TpcInhFieldDrifter::
TpcInhFieldDrifter(const TVector3& eNom,
		const TVector3& bNom,
		const double friction,        //set friction term manually:
		const char* outFile,
		const char* paramFile,
		const int rBins, const int zBins,
		const int split,
		const int zsplit)
:
		foutFile(outFile),
		fparamFile(paramFile),
		frBins(rBins),
		fzBins(zBins),
		fsplit(split),
		fsplitz(zsplit),
		fzstepwidth(10),
		fManCalc(kFALSE),
		fUseReadoutWindow(kFALSE)

{
	if(fsplit >= 0 && split >= frBins) {   //in the case split was manually set...
		std::cout << "TpcInhFieldDrifter::TpcInhFieldDrifter : "
				<<"invalid row number requested! ABORTING ("<<split<<","<<frBins<<")"<<std::endl;
		throw 1;
	}
	if(fsplitz >= 0 && zsplit >= fzBins) {   //in the case split was manually set...
		std::cout << "TpcInhFieldDrifter::TpcInhFieldDrifter : "
				<<"invalid column number requested! ABORTING"<<std::endl;
		throw 1;
	}

	//init parameters
	initParams();

	//double vDrift = ((TpcGas*)fpar->getGas())->VDrift();
	//ffriction = 1e-5 * eNom.Z() / vDrift;  // friction term, see Rolandi Blum
	ffriction=friction;
	//fvDrift=1e-5*eNom.Z()/friction;
	fvDrift=0;

	frunKut = new TpcRungeKutta(1e-16, 1e-6, 1e-2, 1e-3,
			fsc,(TpcEFieldCyl*) NULL,(FairField*) NULL, friction);
	frunKut->setConstE(eNom);
	frunKut->setConstB(bNom);

} 


TpcInhFieldDrifter::
TpcInhFieldDrifter(const TVector3& eNom,
		const TVector3& bNom,
		const char* outFile,
		const char* paramFile,
		const int rBins, const int zBins,
		const int split,
		const int zsplit)
:
		foutFile(outFile),
		fparamFile(paramFile),
		frBins(rBins),
		fzBins(zBins),
		fsplit(split),
		fsplitz(zsplit),
		fzstepwidth(10),
		fManCalc(kFALSE),
		fUseReadoutWindow(kFALSE)
{
	if(fsplit >= 0 && split >= frBins) {   //in the case split was manually set...
		std::cout << "TpcInhFieldDrifter::TpcInhFieldDrifter : "
				<<"invalid row number requested! ABORTING ("<<split<<","<<frBins<<")"<<std::endl;
		throw 1;
	}
	if(fsplitz >= 0 && zsplit >= fzBins) {   //in the case split was manually set...
		std::cout << "TpcInhFieldDrifter::TpcInhFieldDrifter : "
				<<"invalid column number requested! ABORTING"<<std::endl;
		throw 1;
	}

	//init parameters
	initParams();

	fvDrift = fpar->getManDriftVel();
	if(fvDrift<=0)
	{
		fvDrift=fpar->getGas()->VDrift();
		std::cout<<"TpcInhFieldDrifter: using drift velocity from gasfile (v="<<fvDrift<<")\n";
	}
	else
	{
		std::cout<<"TpcInhFieldDrifter: using manual drift velocity (v="<<fvDrift<<")\n";
	}

	ffriction = 1e-5 * eNom.Z() / fvDrift;  // friction term, see Rolandi Blum

	frunKut = new TpcRungeKutta(1e-16, 1e-6, 1e-2, 1e-3,
			fsc,(TpcEFieldCyl*) NULL,(FairField*) NULL, ffriction);
	frunKut->setConstE(eNom);
	frunKut->setConstB(bNom);

} 

TpcInhFieldDrifter::
TpcInhFieldDrifter(TpcEFieldCyl* eField,
		FairField* bField,
		const char* outFile,
		const char* paramFile,
		const int rBins, const int zBins,
		const int split,
		const int zsplit)
:
		foutFile(outFile),
		fparamFile(paramFile),
		frBins(rBins),
		fzBins(zBins),
		fsplit(split),
		fsplitz(zsplit),
		fzstepwidth(10),
		fManCalc(kFALSE),
		fUseReadoutWindow(kFALSE)
{
	if(fsplit >= 0 && split >= frBins) {   //in the case split was manually set...
		std::cout << "TpcInhFieldDrifter::TpcInhFieldDrifter : "
				<<"invalid row number requested! ABORTING ("<<split<<","<<frBins<<")"<<std::endl;
		throw 1;
	}
	if(fsplitz >= 0 && zsplit >= fzBins) {   //in the case split was manually set...
		std::cout << "TpcInhFieldDrifter::TpcInhFieldDrifter : "
				<<"invalid column number requested! ABORTING"<<std::endl;
		throw 1;
	}

	//init parameters
	initParams();
	fvDrift = fpar->getManDriftVel();
	if(fvDrift<=0)
	{
		fvDrift=fpar->getGas()->VDrift();
		std::cout<<"TpcInhFieldDrifter: using drift velocity from gasfile (v="<<fvDrift<<")\n";
	}
	else
	{
		std::cout<<"TpcInhFieldDrifter: using manual drift velocity (v="<<fvDrift<<")\n";
	}

	double eNomZ = (eField->nominal()).Z();
	ffriction = 1e-5 * eNomZ / fvDrift;  // friction term, see Rolandi Blum

	frunKut = new TpcRungeKutta(1e-16, 1e-6, 1e-2, 1e-3,
			fsc, eField,bField, ffriction);

}


TpcInhFieldDrifter::
TpcInhFieldDrifter(const char* eFieldFile,
		const char* bFieldFile,
		double beamMom,
		const char* outFile,
		const char* paramFile,
		const int rBins, const int zBins,
		const int split,
		const int zsplit)
:
		foutFile(outFile),
		fparamFile(paramFile),
		frBins(rBins),
		fzBins(zBins),
		fsplit(split),
		fsplitz(zsplit),
		fzstepwidth(10),
		fManCalc(kFALSE),
		fUseReadoutWindow(kFALSE)
{
	if(fsplit >= 0 && split >= frBins) {   //in the case split was manually set...
		std::cout << "TpcInhFieldDrifter::TpcInhFieldDrifter : "
				<<"invalid row number requested! ABORTING ("<<split<<","<<frBins<<")"<<std::endl;
		throw 1;
	}
	if(fsplitz >= 0 && zsplit >= fzBins) {   //in the case split was manually set...
		std::cout << "TpcInhFieldDrifter::TpcInhFieldDrifter : "
				<<"invalid column number requested! ABORTING"<<std::endl;
		throw 1;
	}

	//init parameters
	initParams();

	fvDrift = fpar->getManDriftVel();
	if(fvDrift<=0)
	{
		fvDrift=fpar->getGas()->VDrift();
		std::cout<<"TpcInhFieldDrifter: using drift velocity from gasfile (v="<<fvDrift<<")\n";
	}
	else
	{
		std::cout<<"TpcInhFieldDrifter: using manual drift velocity (v="<<fvDrift<<")\n";
	}

	FairRunSim::Instance()->SetBeamMom(beamMom);

	PndMultiField* bfield = new PndMultiField("FULL");
	bfield->Init();
	TpcEFieldCyl* efield = new TpcEFieldCyl(eFieldFile);
	double eNomZ = (efield->nominal()).Z();
	ffriction = 1e-5 * eNomZ / fvDrift;  // friction term, see Rolandi Blum

	frunKut = new TpcRungeKutta(1e-16, 1e-6, 1e-2, 1e-3,
			fsc, efield, bfield, ffriction);

}



TpcInhFieldDrifter::~TpcInhFieldDrifter()
{
	delete frunKut;
	int size = fvelocity_control->size();
	for (int i=0; i<size; i++)
		delete fvelocity_control->at(i);
	delete fvelocity_control;
}


void 
TpcInhFieldDrifter::run()
{
	// Calculate the Binning
	frBinWidth = (double)(ftpcMaxR - ftpcMinR) / frBins;
	fzBinWidth = (double)(ftpcMaxZ - ftpcMinZ) / fzBins;

	// Init the fields

	//TODO: dangerous!!! change to pointers inside
	for (int nr = 0; nr < frBins; nr++)
	{
		fdevX.push_back(std::vector<double>(fzBins, 0));
		fdevY.push_back(std::vector<double>(fzBins, 0));
		ftime.push_back(std::vector<double>(fzBins, 0));
		fstepCount.push_back(std::vector<int>(fzBins, 0));
		fpathLength.push_back(std::vector<double>(fzBins, 0));

	}

	fvelocity_control = new std::vector<std::vector<double>*>;
	fvelocity_control->push_back(new std::vector<double>(4, 0)); //first position

	std::cout << "\nCalculating the Deviation Map with Runge-Kutta "<<std::endl;
	int errorcount = 0;


	//Calculate the Deviation-Map with Runge Kutta
	for (int nr = 0; nr < frBins; nr++)
	{
		if(fsplit != -1) {  //we want to split: only calculate one row in r
			if(nr != fsplit)
				continue;
		}
		for (int nz = 0; nz < fzBins; nz++)
		{
			if (fsplitz != -1 && !fManCalc)
			{
				if (nz >= fzstepwidth * (fsplitz+1) || nz < fzstepwidth * fsplitz)
				{
					continue;
				}
			}
			if (fManCalc)
			{
				int counts = std::count(fZtoCalc.begin(), fZtoCalc.end(), nz);
				if (counts==0)
					continue;
			}

			double rPos, zPos, dx, dy, dz, olddz=-1;
			int backdriftcounter=0;

			rPos = (double)(ftpcMinR + (nr+0.5)*frBinWidth)/100.; // [m]
			zPos = (double)(ftpcMinZ + (nz+0.5)*fzBinWidth)/100.;
			std::cout<<"rPos: "<<rPos<<"(bin: "<<nr<<")"<<"  zPos: "<<zPos<<"(bin: "<<nz<<")"<<std::endl;
			std::cout<<"MinZ: "<<ftpcMinZ/100.<<" stepZ: "<<fzBinWidth<<" MinR: "<<ftpcMinR/100.<<" stepR: "<<frBinWidth<<std::endl;

			// Create the speed/position-vector for RungeKutta
			double x[6]; // x[0-2] = speed, x[3-5] = pos. (SI)

			x[3] = rPos;  // == x coordinate. symmetry!
			x[4] = 0.;    // == y
			x[5] = zPos;
			x[0] = 0.;
			x[1] = 0.;
			x[2] = -0.0027314 * 1e7;


			double xCopy[6]; //store the starting position
			for (int n=0; n<=5; n++)
				xCopy[n] = x[n];

			frunKut->setTimeStep(1e-16);
			double time_temp;
			double mean[4]={0,0,0,0},oldmean[4]={0,0,0,0},diff[4]={0,0,0,0};
			bool left_inner=false;
			bool left_outer=false;

			for (int i=0; ; i++)  // get serious
			{
				mean[0]+=x[3];
				mean[1]+=x[4];
				mean[2]+=x[5];
				mean[3]+=sqrt(x[3]*x[3]+x[4]*x[4]);

				if (x[5]>(ftpcMaxZ)/100. || x[5]<(ftpcMinZ/100.))
				{
					std::cout<<"reached the pads"<<std::endl;
					break;   	// finished when we reach the "Pads"
				}

				if ( sqrt(x[3]*x[3]+x[4]*x[4])>(ftpcMaxR)/100.)
				{
					std::cout<<"outside of outer Radius"<<std::endl;
					std::cout<<"R="<<sqrt(x[3]*x[3]+x[4]*x[4])<<" MaxR="<<ftpcMaxR/100.<<std::endl;
					//	    fdevX.at(nr).at(nz) = 100000 * 100;    //[cm]
					//	    fdevY.at(nr).at(nz) = 100000 * 100;
					x[3]=100000+xCopy[3];
					x[4]=100000+xCopy[4];
					left_outer=true;
					break;
				}
				if ( sqrt(x[3]*x[3]+x[4]*x[4])<(ftpcMinR)/100. )
				{
					std::cout<<"outside of inner Radius"<<std::endl;
					std::cout<<"R="<<sqrt(x[3]*x[3]+x[4]*x[4])<<" MinR="<<ftpcMinR/100.<<std::endl;
					x[3]=-100000+xCopy[3];
					x[4]=-100000+xCopy[4];
					left_inner=true;
					break;
				}

				double xTemp[3]; //needed for the drift path monitoring
				for (int n=0; n<3; n++)
					xTemp[n] = x[n+3];

				for (int j=0; ; j++)	//one step of RungeKutta
				{
					time_temp = frunKut->getTimeStep();//store BEFORE step
					if (frunKut->getTimeStep() == 0)
						frunKut->setTimeStep(1e-16);
					if (frunKut->stepForwards(x))  //try until successful
						break;
				}

				ftime.at(nr).at(nz) += time_temp;;
				fstepCount.at(nr).at(nz) ++;

				if (fstepCount.at(nr).at(nz)%100000==0)
					std::cout<<"Now at Z:"<<x[5]*100<<"cm R:"<<sqrt(x[3]*x[3]+x[4]*x[4])*100<<"cm\n";

				/*
				if (fstepCount.at(nr).at(nz)>1e9)
				{
					std::cout<<"The Electron will probably never arrive, stopping calculation for this bin\n";
					fdevX.at(nr).at(nz) = (100000) * 100;    //[cm]
					fdevY.at(nr).at(nz) = (100000) * 100;
					break;
				}
				*/

				dx = x[3] - xTemp[0];
				dy = x[4] - xTemp[1]; //coordinate changes of the last RK step
				dz = x[5] - xTemp[2];
				fpathLength.at(nr).at(nz) += sqrt(dx*dx + dy*dy + dz*dz);

				//	if (x[5]==0.0005)


				//build the velocity-control array (in just one bin)
				//if (nr == (int) std::floor((double)frBins/2)
				//    && nz == (int) std::floor((double)fzBins/2))
				//{
				//if ((fstepCount.at(nr).at(nz))%1000 == 0) {

				//std::vector<double>* temp_vec = new std::vector<double>(4);
				//for(int ii = 0; i <= 2; ii++)
				//  temp_vec->at(ii) = x[ii];    //fill vector with components of V
				//temp_vec->at(3) = frunKut->getTimeStep();
				//fvelocity_control->push_back(temp_vec);
				// for(unsigned int k=0; k<6; k++)
				//   std::cout<<x[k]<<"   ";
				// std::cout<<frunKut->getTimeStep();
				// std::cout<<std::endl;
				// }
				//}
			}
			std::cout<<"pushing back a bin "<<nz<<" x="<<x[3]<<" y="<<x[4]<<" \n";
			fcalcZbins.push_back(nz);

			if(x[3]==xCopy[3] && x[4]==xCopy[4])
				errorcount++;

			// Now fill the 2 deviation fields with the values
			fdevX.at(nr).at(nz) = (x[3]-xCopy[3]) * 100;    //[cm]
			fdevY.at(nr).at(nz) = (x[4]-xCopy[4]) * 100;
		}
		std::cout<<"* "<<std::endl;
	} 	// Finish Loop over all bins

	std::cout << "\n\nAll done! \nThere have been " << errorcount
			<< " unprocessed bins..." << std::endl;

	writeToFile(foutFile);
}

void
TpcInhFieldDrifter::writeToFile(const char* out_file)
{

	std::string filename(out_file);

	if(fsplit >= 0) {
		std::string s;
		std::stringstream stream;
		stream << "r";
		stream<<std::setfill('0');
		stream<<std::setw(4);
		stream << fsplit;
		stream >> s;
		filename += s;
	}
	if(fsplitz >= 0) {
		std::string s;
		std::stringstream stream;
		stream<<"z";
		stream<<std::setfill('0');
		stream<<std::setw(4);
		stream << fsplitz;
		stream >> s;
		filename += s;
	}

	if (fManCalc)
	{
		std::string s;
		std::stringstream stream;
		stream<<"zb";
		stream<<std::setfill('0');
		std::cout<<"calculated z-bins: "<<fcalcZbins.size()<<"\n";
		for (int i=0; i<fcalcZbins.size();i++)
		{
			stream<<std::setw(4)<<fcalcZbins[i];
			if (i+1!=fcalcZbins.size())
				stream<<"_";
		}
		stream>>s;
		filename +=s;
	}

	std::ofstream outfile(filename.c_str(), std::fstream::out);
	std::cout << "\n\n**** Writing Deviation Data to File " << filename
			<< " ****\n";


	std::cout<<"fsplit="<<fsplit<<" fsplitz="<<fsplitz<<" fManCalc="<<fManCalc<<"\n";
	if( ((fsplit == 0 || fsplit == -1) && (fsplitz == 0 || fsplitz == -1) && !fManCalc) || (fManCalc && fsplit==0 && std::count(fcalcZbins.begin(), fcalcZbins.end(), 0)==1) )
	{
		outfile<<std::setprecision(6);
		outfile<<frBins<<"   "<<fzBins<<"   "<<ftpcMinR<<"   "<<ftpcMaxR
				<<"   "<<ftpcMinZ<<"   "<<ftpcMaxZ<<"   "<<fvDrift<<"\n";
	}

	int nr=0;
	int nz=0;
	for (; nr<frBins; nr++)
	{
		if (fsplit>=0) {
			if (nr != fsplit)
				continue;
		}

		for (; nz<fzBins; nz++)
		{

			if (fsplitz>=0 && !fManCalc){
				if (nz >= fzstepwidth *(fsplitz+1) || nz < fzstepwidth * fsplitz )
				{
					continue;
				}
			}
			if (fManCalc)
				if (std::count(fcalcZbins.begin(), fcalcZbins.end(), nz)==0)
				{
					continue;
				}

			outfile<<std::setprecision(8)<<std::setw(8);
			outfile<<fdevX.at(nr).at(nz)<<"   "
					<<fdevY.at(nr).at(nz)<<"   "<<ftime.at(nr).at(nz)*1e9<<"   "
					<<fstepCount.at(nr).at(nz)<<"   "<<fpathLength.at(nr).at(nz)*100
					<<"   "<<nr<<"   "<<nz<<"\n";
		}
	}

	if( ( (nr == frBins ) || fsplit == -1) &&  ( ( nz == fzBins ) || fsplitz == -1))
	{
		outfile<<std::endl;
		outfile<<"\n\n#Format: First Line - Bins in r, Bins in z, minR, maxR, minZ, maxZ"
				<<"\n#Each Following Line: x-deviation [cm], y-deviation, total Runge-Kutta-Time [ns],"
				<<"\n#number of Runge-Kutta steps, real total RK-drift path [cm]"
				<<"\n#This File was created by class TpcInhFieldDrifter"
				<<std::endl;
	}

	outfile.flush();
	outfile.close();

	//   std::ofstream outfile2("DEVMAP_VELOCITY_FILE.dat", std::fstream::out);
	//   outfile<<std::setprecision(8);

	//   long int length = fvelocity_control->size();

	//   for (int a=0; a<length; a++)
	//   {
	//     outfile2<<std::setprecision(8)<<std::setw(8);
	//     outfile2<<"\n"<<fvelocity_control->at(a)->at(0)*1e-7<<"   "
	//             <<fvelocity_control->at(a)->at(1)*1e-7<<"   "
	//             <<fvelocity_control->at(a)->at(2)*1e-7<<"   "
	//             <<fvelocity_control->at(a)->at(3)*1e9;
	//   }
	//   outfile2<<std::endl;
	//   outfile2<<"#Format: v_x, v_y, v_z, corresponding Runge-Kutta-time"<<std::endl;
	//   outfile2.close();
}



void
TpcInhFieldDrifter::initParams() {

	FairParAsciiFileIo* input = new FairParAsciiFileIo();

	//ugly test for valid param file
	std::ifstream paramf(fparamFile, std::fstream::in);
	if(!paramf.good()) {
		std::cout<<"TpcInhFieldDrifter::initParams() "
				<<"- parameter file IO error - ABORTING"<<std::endl;
		throw 1;
	}

	input->open(fparamFile, "in");

	//create dummy run manager and load database
	FairRunSim* fRun = FairRunSim::Instance();
	FairRuntimeDb* rtdb = fRun->GetRuntimeDb();
	rtdb->setFirstInput(input);
	fpar = (TpcDigiPar*) rtdb->getContainer("TpcDigiPar");

	fpar->FairParSet::init();

	//read in parameters
	ftpcMinR = fpar->getRMin();
	ftpcMaxR = fpar->getRMax();
	if(fUseReadoutWindow)
	{
		ftpcMinZ = fpar->windowMin();
		ftpcMaxZ = fpar->windowMax();
	}
	else
	{
		ftpcMinZ=0;
		ftpcMaxZ=fpar->getZMax();
	}

	std::cout<<"\n\nTpcInhFieldDrifter::initParams()\n"
			<<"---------- Used Parameters ---------------------------"<<std::endl;
	std::cout<<"MinR: "<<ftpcMinR<<",   MaxR: "<<ftpcMaxR<<std::endl;
	std::cout<<"MinZ: "<<ftpcMinZ<<",   MaxZ: "<<ftpcMaxZ<<std::endl;std::cout<<std::endl;
	std::cout<<"Bins in R: "<<frBins<<",   Bins in Z: "<<fzBins<<std::endl;

	std::cout<<"------------------------------------------------------"<<std::endl;
	std::cout<<std::endl;
	fsc = -1e-6*299792458*299792458/0.510998902;


}