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


//This class' Header ---------------------------------------
#include "TpcDevmapCylLoader.h"

//C++ Headers ----------------------------------------------
#include <fstream>
#include <iostream>
#include <cmath>
#include <vector>

//Collaborating Headers ------------------------------------
#include "TpcDevmapCyl.h"
#include "TError.h"


//Class Member Definitions ---------------------------------

TpcDevmapCylLoader::TpcDevmapCylLoader(TpcDevmapCyl* devmap,
		const char* const fileName)
{
	fdevmap = devmap;
	ffileName = fileName;
	fvDrift = fdevmap->vDrift();
	fsetmap	  = new std::vector<std::vector<bool>*>;
}


TpcDevmapCylLoader::~TpcDevmapCylLoader()
{;}

int TpcDevmapCylLoader::loadinv()
{
	// try to find and open the file
	std::ifstream infile(ffileName, std::fstream::in);
	if (!infile.good())
	{
	  std::cout<<"TpcDevmapCylLoader::loadinv(): devmap-file"<<ffileName<<" could not be found.\n";
	  return 1;
	}

	int rSteps, zSteps;
	double rMin, rMax, rWidth, zMin, zMax, zWidth, vDrift;

	infile>>rSteps>>zSteps>>rMin>>rMax>>zMin>>zMax>>vDrift;
	rWidth = (double)(rMax - rMin)/rSteps;
	zWidth = (double)(zMax - zMin)/zSteps;

	std::cout<<"original: "<<rSteps<<" "<<zSteps<<"\n";

	fdevmap->setNominal(TVector3(0.,0.,0.));

	_rMin=rMin;
	_rMax=rMax;
	_zMin=zMin;
	_zMax=zMax;
	_rWidth=rWidth;
	_zWidth=zWidth;

	std::cout<<"test:"<<vDrift<<"\n";

	if (vDrift>1e-10)
	{
		fvDrift=vDrift;
	}

	if(fvDrift<1e-10)
	{
		Error("TpcDevMapCylLoader::loadinv()","drift velocity not set!");

	}

	// Initialize the field-map
	ffieldmap = new std::vector<std::vector<TVector3*>*>;


	int inr,inz;
	double rad, zpos;
	double devX, devY, time, dump, devZ, straight_way;

	_maxZbin=0;
	_maxRbin=0;
	// fill the matrix from the file; z-Index runs first
	for (int nr=0; nr<rSteps; nr++)
	{
		for (int nz=0; nz<zSteps; nz++)
		{
			infile>>devX>>devY>>time>>dump>>dump;
			straight_way = (nz + 0.5) * zWidth;
			devZ = time*fvDrift - straight_way;    //actual z-deviation
			rad=(nr+0.5)*rWidth; //radius points in x-direction
			//std::cout<<"radius from bin: "<<rad<<"\n";
			rad+=devX; //calc distorted radius
			//std::cout<<"radius after dist bin: "<<rad<<" devX: "<<devX<<" devY: "<<devY<<"\n";
			rad=sqrt(rad*rad+devY*devY);
			//std::cout<<"radius finally: "<<rad<<"\n";
			zpos=time*fvDrift;
			//std::cout<<"driftvel:"<<fvDrift<<"\n";

			//zpos=straight_way;
			//	  if (rad+rMin>_rMax)
			//	    _rMax=rad+rMin;
			//continue;
			//	  if (rad+rMin<_rMin)
			//	    _rMin=rad+rMin;
			//	    continue;
			if (zpos+zMin>_zMax)
				_zMax=zpos+zMin;
			if (zpos+zMin<_zMin)
				_zMin=zpos+zMin;

			inr=(int)std::floor( rad/rWidth ); //calc distorted r-bin
			inz=(int)std::floor( zpos/zWidth ); //calc distorted z-bin
			if (inr>rSteps)
			{
				//std::cout<<"continue dou to inr>rSteps "<<inr<<" "<<rSteps<<"\n";
				continue;
			}
				if (inr<0)
				continue;

			while(inr+1>ffieldmap->size())
			{
				ffieldmap->push_back(new std::vector<TVector3*>);
				fsetmap->push_back(new std::vector<bool>);
			}
			while(inz+1>(ffieldmap->at(inr))->size())
			{
				(ffieldmap->at(inr))->push_back(new TVector3(0.,0.,0.));
				(fsetmap->at(inr))->push_back(false);
			}

			if(not(fsetmap->at(inr)->at(inz)))
			{
				ffieldmap->at(inr)->at(inz) = new TVector3(devX, devY, devZ);
				fsetmap  ->at(inr)->at(inz) = true;
			}

			if (inr>_maxRbin)
				_maxRbin=inr;
			if (inz>_maxZbin)
				_maxZbin=inz;
		}
	}

	std::cout<<"filling fieldmap:"<<_maxRbin<<","<<_maxZbin<<"\n";
	double x,y,z;
	for (int nr=0;nr<_maxRbin+1;nr++)
	{
		for(int nz=ffieldmap->at(nr)->size();nz<_maxZbin+1;nz++)
		{
			if(nz+1>ffieldmap->at(nr)->size())
			{
				ffieldmap->at(nr)->push_back(new TVector3(0.,0.,0.));
				fsetmap->at(nr)  ->push_back(false);
			}
		}
	}
	//for (int ismooth=0;ismooth<4;ismooth++)
	{
		for (int nr=0;nr<_maxRbin+1;nr++)
			for(int nz=0;nz<_maxZbin+1;nz++)
				if(not(fsetmap->at(nr)->at(nz)))
				{
					TVector3 newVal;
					newVal=interpol(nr,nz,true);
					ffieldmap->at(nr)->at(nz)=new TVector3(newVal);
					fsetmap->at(nr)->at(nz)=true;
				}
	}

	rWidth = (double)(_rMax - _rMin)/_maxRbin;
	zWidth = (double)(_zMax - _zMin)/_maxZbin;

	fdevmap->setMinR(_rMin);
	fdevmap->setSpacingR(rWidth);
	fdevmap->setMinZ(_zMin);
	fdevmap->setSpacingZ(zWidth);

	fdevmap->fxBins = _maxRbin;//rSteps;
	fdevmap->fzBins = _maxZbin;//zSteps;

	_rWidth=rWidth;
	_zWidth=zWidth;

	std::cout<<"TpcDevmapCylLoader::loadinv(): successfully initialized the Dev-field\n"<<std::endl;

	//just a test
	_lengthr = ffieldmap->size();
	_lengthz = ffieldmap->at(0)->size();
	std::cout << "fieldmap has " << _lengthr <<" bins in r and \n"
			<< _lengthz << " bins in z direction.\n\n";
	std::cout.flush();
	fdevmap->fpGrid = ffieldmap;       //TpcDevmapCyl takes ownership
	infile.close();
	return 0;
}

int 
TpcDevmapCylLoader::load()
{
	// try to find and open the file
	std::ifstream infile(ffileName, std::fstream::in);
	if (!infile.good())
	{
		Error("TpcDevmapCylLoader::load()","devmap-file could not be found.");
		return 1;
	}

	int rSteps, zSteps;
	double rMin, rMax, rWidth, zMin, zMax, zWidth,vDrift;

	infile>>rSteps>>zSteps>>rMin>>rMax>>zMin>>zMax>>vDrift;
	rWidth = (double)(rMax - rMin)/rSteps;
	zWidth = (double)(zMax - zMin)/zSteps;

	fdevmap->setNominal(TVector3(0.,0.,0.));
	fdevmap->setMinR(rMin);
	fdevmap->setSpacingR(rWidth);
	fdevmap->setMinZ(zMin);
	fdevmap->setSpacingZ(zWidth);

	fdevmap->fxBins = rSteps;
	fdevmap->fzBins = zSteps;

	_rMin=rMin;
	_rMax=rMax;
	_zMin=zMin;
	_zMax=zMax;
	_rWidth=rWidth;
	_zWidth=zWidth;


	if (vDrift>1e-10)
	{
		fvDrift=vDrift;
	}

	if(fvDrift<1e-10)
	{
		Error("TpcDevMapCylLoader::loadinv()","drift velocity not set!");

	}

	// Initialize the field-map
	ffieldmap = new std::vector<std::vector<TVector3*>*>;

	// fill the matrix from the file; z-Index runs first
	int nr,nz;
	for (nr=0; nr<rSteps; nr++)
	{
		ffieldmap->push_back(new std::vector<TVector3*>(zSteps));
		for (nz=0; nz<zSteps; nz++)
		{
			double devX, devY, time, dump, devZ, straight_way;
			infile>>devX>>devY>>time>>dump>>dump;
			straight_way = (nz + 0.5) * zWidth;
			devZ = time*fvDrift - straight_way;    //actual z-deviation
			ffieldmap->at(nr)->at(nz) = new TVector3(devX, devY, devZ);
		}
	}
	std::cout<<"\nTpcDevmapCylLoader::load(): successfully initialized the Dev-field "<<std::endl;

	//just a test
	_lengthr = ffieldmap->size();
	_lengthz = ffieldmap->at(0)->size();
	std::cout << "fieldmap has " << _lengthr <<" bins in r and \n"
			<< _lengthz << " bins in z direction.\n\n";
	std::cout.flush();
	fdevmap->fpGrid = ffieldmap;       //TpcDevmapCyl takes ownership
	infile.close();
	return 0;
}

void TpcDevmapCylLoader::getValues(double &rmax, double &rmin, double &zmax, double &zmin, double &rwidth, double &zwidth)
{
	rmax=_rMax;
	rmin=_rMin;
	zmax=_zMax;
	zmin=_zMin;
	rwidth=_rWidth;
	zwidth=_zWidth;
}

void TpcDevmapCylLoader::getValues(double &rmax, double &rmin, double &zmax, double &zmin, double &rwidth, double &zwidth, double &vdrift)
{
	rmax=_rMax;
	rmin=_rMin;
	zmax=_zMax;
	zmin=_zMin;
	rwidth=_rWidth;
	zwidth=_zWidth;
	vdrift=fvDrift;
}
TVector3 TpcDevmapCylLoader::interpol(int rbin, int zbin, bool inv)//, double &x, double &y, double &z)
{
	TVector3 b[8];
	for (int i =0;i<8;i++)
		b[i] = TVector3(0.,0.,0.);

	Double_t w[8];
	w[1]=w[3]=w[4]=w[6]=1.;
	w[0]=w[2]=w[5]=w[7]=(double)1./sqrt(2);

	if(rbin==_maxRbin)
		w[0]=w[1]=w[2]=0.;
	if(rbin==0)
		w[5]=w[6]=w[7]=0.;
	if(zbin==_maxZbin)
		w[2]=w[4]=w[7]=0.;
	if(zbin==0)
		w[0]=w[3]=w[5]=0.;

	if(inv)
	{
		if (w[0]!=0)
			if(not fsetmap->at(rbin+1)->at(zbin-1))
				w[0]=0;
		if(w[1]!=0)
			if(not fsetmap->at(rbin+1)->at(zbin))
				w[1]=0;
		if(w[2]!=0)
			if(not fsetmap->at(rbin+1)->at(zbin+1))
				w[2]=0;
		if(w[3]!=0)
			if(not fsetmap->at(rbin)->at(zbin-1))
				w[3]=0;
		if(w[4]!=0)
			if(not fsetmap->at(rbin)->at(zbin+1))
				w[4]=0;
		if(w[5]!=0)
			if(not fsetmap->at(rbin-1)->at(zbin-1))
				w[5]=0;
		if(w[6]!=0)
			if(not fsetmap->at(rbin-1)->at(zbin))
				w[6]=0;
		if(w[7]!=0)
			if(not fsetmap->at(rbin-1)->at(zbin+1))
				w[7]=0;
	}

	if (w[0]!=0)
		b[0]=*(ffieldmap->at(rbin+1)->at(zbin-1));
	if (w[1]!=0)
		b[1]=*(ffieldmap->at(rbin+1)->at(zbin));
	if (w[2]!=0)
		b[2]=*(ffieldmap->at(rbin+1)->at(zbin+1));
	if (w[3]!=0)
		b[3]=*(ffieldmap->at(rbin)->at(zbin-1));
	if (w[4]!=0)
		b[4]=*(ffieldmap->at(rbin)->at(zbin+1));
	if (w[5]!=0)
		b[5]=*(ffieldmap->at(rbin-1)->at(zbin-1));
	if (w[6]!=0)
		b[6]=*(ffieldmap->at(rbin-1)->at(zbin));
	if (w[7]!=0)
		b[7]=*(ffieldmap->at(rbin-1)->at(zbin+1));

	TVector3 sum ;
	sum= TVector3(0.,0.,0.);
	Double_t sumw;
	sumw=0;
	for (int i=0;i<8;i++)
	{
		if(b[i].Mag()!=0)
		{
			sum+=w[i]*b[i];
			sumw+=w[i];
		}
	}

	sum*=(double)(1./sumw);
	return (sum);
}