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


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

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

#include <iostream>
#include <iomanip>
#include <fstream>
#include <cmath>
#include "TpcEFieldCyl.h"
#include "TpcGas.h"



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

TpcInhFieldDrifter::
TpcInhFieldDrifter(const char* eFieldFile,
		   PndMultiField* bField,
		   const std::string outFile,
		   int rBins, int zBins)
  :
	_tpcMinR(15.5),
	_tpcMaxR(41.5),
	_tpcMinZ(-39.5),
	_tpcMaxZ(109.5),
	_outFile(outFile),
        _sc(-1e-6*299792458*299792458/0.510998902), 
        _rBins(rBins),
        _zBins(zBins)
{
  TpcEFieldCyl* efield = new TpcEFieldCyl(eFieldFile);
  TVector3 eNom = efield->nominal();
  double eNomZ = eNom.Z();
  TpcGas* gas = new TpcGas("NEON-90_CO2-10_B2_PRES1013.asc",400);
  double vDrift = gas->VDrift();
  _friction = 1e-5 * eNomZ / vDrift;  // friction term, see Rolandi Blum    
  delete efield;
  delete gas;
  _runKut = new TpcRungeKutta(1e-16, 1e-6, 1e-2, 1e-3,
			      _sc, eFieldFile,bField, _friction);
}

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


void 
TpcInhFieldDrifter::run()
{
  // Calculate the Binning
  _rBinWidth = (double)(_tpcMaxR - _tpcMinR) / _rBins;
  _zBinWidth = (double)(_tpcMaxZ - _tpcMinZ) / _zBins;

  // Init the fields
  for (int nr = 0; nr < _rBins; nr++)
  {
    _devX.push_back(std::vector<double>(_zBins, 0));
    _devY.push_back(std::vector<double>(_zBins, 0));
    _time.push_back(std::vector<double>(_zBins, 0));
    _stepCount.push_back(std::vector<int>(_zBins, 0));
    _pathLength.push_back(std::vector<double>(_zBins, 0));
    
  }

  _velocity_control = new std::vector<std::vector<double>*>;
  _velocity_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 < _rBins; nr++)
  {
    for (int nz = 0; nz < _zBins; nz++)
    {
      double rPos, zPos, dx, dy, dz;

      rPos = (double)(_tpcMinR + (nr+0.5)*_rBinWidth)/100.; // [m]
      zPos = (double)(_tpcMinZ + (nz+0.5)*_zBinWidth)/100.; 

      // 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];
      
      _runKut->setTimeStep(1e-16);
      double time_temp;

      for (int i=0; ; i++)  // get serious
      {
        if (x[5]>(_tpcMaxZ)/100. || x[5]<(_tpcMinZ/100.))	   
	  break;   	// finished when we reach the "Pads"
	
	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 = _runKut->getTimeStep();//store BEFORE step to avoid error
	  if (_runKut->getTimeStep() == 0)
	    _runKut->setTimeStep(1e-16);
	  if (_runKut->stepForwards(x))  //try until successful
	    break;
	}
	_time.at(nr).at(nz) += time_temp;;
	_stepCount.at(nr).at(nz) ++;
	
        dx = x[3] - xTemp[0];
	dy = x[4] - xTemp[1]; //coordinate changes of the last RK step
        dz = x[5] - xTemp[2];
        _pathLength.at(nr).at(nz) += sqrt(dx*dx + dy*dy + dz*dz);
	
	//build the velocity-control array (in just one bin)
	if (nr == (int) std::floor((double)_rBins/2) 
	    && nz == (int) std::floor((double)_zBins/2))
	{
	  if ((_stepCount.at(nr).at(nz))%100 == 0) 
	  {
	    std::vector<double>* temp_vec = new std::vector<double>(4);
	    for(int i = 0; i <= 2; i++)
	      temp_vec->at(i) = x[i];    //fill vector with components of V
	    temp_vec->at(3) = _runKut->getTimeStep();
	    _velocity_control->push_back(temp_vec);
	
	  }
	}  
      }
      

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

      // Now fill the 2 deviation fields with the values
      _devX.at(nr).at(nz) = (x[3]-xCopy[3]) * 100;    //[cm]
      _devY.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(_outFile);
}

void
TpcInhFieldDrifter::writeToFile(const std::string filename)
{
  std::cout << "\n\n**** Writing Deviation Data to File " << filename << " ****" << std::endl;
   
  std::ofstream outfile(filename.c_str(), std::fstream::out);
  outfile<<std::setprecision(6);
  outfile<<_rBins<<"   "<<_zBins<<"   "<<_tpcMinR<<"   "<<_tpcMaxR
         <<"   "<<_tpcMinZ<<"   "<<_tpcMaxZ;
  
  for (int nr=0; nr<_rBins; nr++)
  {
    for (int nz=0; nz<_zBins; nz++) 
    {
      outfile<<std::setprecision(8)<<std::setw(8);
      outfile<<"\n"<<_devX.at(nr).at(nz)<<"   "
	     <<_devY.at(nr).at(nz)<<"   "<<_time.at(nr).at(nz)*1e9<<"   " 
	     <<_stepCount.at(nr).at(nz)<<"   "<<_pathLength.at(nr).at(nz)*100;
    }
  }
  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.close();

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

  long int length = _velocity_control->size();

  for (int a=0; a<length; a++)
  {
    outfile2<<std::setprecision(8)<<std::setw(8);
    outfile2<<"\n"<<_velocity_control->at(a)->at(0)*1e-7<<"   "
            <<_velocity_control->at(a)->at(1)*1e-7<<"   "
            <<_velocity_control->at(a)->at(2)*1e-7<<"   "
            <<_velocity_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();
}