//_HADES_CLASS_DESCRIPTION 
//////////////////////////////////////////////////////////////////////////////
// HiTofGeomPar
//
// Container class for the basic iTOF geometry parameters
//
// Each sector contains  TFNX composite volumes, which contain the
// cells as components
//////////////////////////////////////////////////////////////////////////////
using namespace std;
#include "hitofgeompar.h"
#include "hades.h"
#include "hruntimedb.h"
#include "hpario.h"
#include "hdetpario.h"
#include <iostream> 
#include <iomanip>

ClassImp(HiTofGeomPar)

HiTofGeomPar::HiTofGeomPar(const Char_t* name,const Char_t* title,
			   const Char_t* context)
    : HDetGeomPar(name,title,context,"iTof") {

	setDebug(kFALSE);
}

Bool_t HiTofGeomPar::init(HParIo* inp,Int_t* set) {
  // intitializes the container from an input
  HDetParIo* input = inp->getDetParIo("HiTofParIo");
  if (input) return (input->init(this,set));
  return kFALSE;
}

Int_t HiTofGeomPar::write(HParIo* output) {
  // writes the container to an output
  HDetParIo* out = output->getDetParIo("HiTofParIo");
  if (out) return out->write(this);
  return -1;
}


//--------------------------------------------------------------------------------
//  Helper functions
//--------------------------------------------------------------------------------

Bool_t HiTofGeomPar::getCenterAndNormVecMod(Int_t sec, TVector3& center, TVector3& norm)
{
    if(sec < 0 || sec > 5 ){
	Error("getNormAndCenterVecMod()","address out of bounds ! s = %d !",sec);
    }
    norm   = normVecMod  [sec];
    center = centerVecMod[sec];
    return kTRUE;
}

//--------------------------------------------------------------------------------
//  Transformations
//--------------------------------------------------------------------------------

const HGeomTransform& HiTofGeomPar::getLabSecTrans(Int_t sec)
{
    // return transformation Lab<-->sec.
    // needs to run initGeomHelpers(HSpecGeomPar* specGeom)
    // before
    if(sec < 0 || sec > 5 ){
	Error("getLabSecTrans()","address out of bounds ! s = %d !",sec);
    }
    return fLabToSec[sec];
}

const HGeomTransform& HiTofGeomPar::getLabModTrans(Int_t sec)
{
    //return transformation lab<-->mod.
    // needs to run initGeomHelpers(HSpecGeomPar* specGeom)
    // before
    if(sec < 0 || sec > 5 ){
	Error("getLabModTrans()","address out of bounds ! s = %d !",sec);
    }
    return fLabToMod[sec];
}

const HGeomTransform& HiTofGeomPar::getSecModTrans(Int_t sec)
{
    //return transformation sec<-->mod.
    // needs to run initGeomHelpers(HSpecGeomPar* specGeom)
    // before
    if(sec < 0 || sec > 5 ){
	Error("getSecModTrans()","address out of bounds ! s = %d !",sec);
    }
    return fSecToMod[sec];
}

const HGeomTransform& HiTofGeomPar::getModCellTrans(Int_t sec, Int_t cell)
{
    // return transformation mod<-->cell.
    // needs to run initGeomHelpers(HSpecGeomPar* specGeom)
    // before
    if(sec < 0 || sec > 5 || cell < 0 || cell > ITOFMAXCELL - 1){
	Error("getModCellTrans()","address out of bounds ! s = %d, c = %d !",sec,cell);
    }
    return fModToCell[sec][cell];
}

const HGeomTransform& HiTofGeomPar::getSecCellTrans(Int_t sec, Int_t cell)
{
    // return transformation sec<-->cell.
    // needs to run initGeomHelpers(HSpecGeomPar* specGeom)
    // before
    if(sec < 0 || sec > 5 || cell < 0 || cell > ITOFMAXCELL - 1){
	Error("getModCellTrans()","address out of bounds ! s = %d, c = %d !",sec,cell);
    }
    return fSecToCell[sec][cell];
}

//--------------------------------------------------------------------------------
//  Volumes
//--------------------------------------------------------------------------------

HGeomCompositeVolume* HiTofGeomPar::getGeomCompositeVolume(Int_t sec)
{
    // return composit vol of sec (contains the cells)
    if(sec < 0 || sec > 5 ){
	Error("getGeomCompositeVolume()","address out of bounds ! s = %d !",sec);
        return 0;
    }
    HModGeomPar* modPar = getModule(sec);
    return modPar->getRefVolume();
}

HGeomVolume* HiTofGeomPar::getGeomVolumeCell(Int_t sec,Int_t cell)
{
    // return volume of cell (contains the points of volume + transformation)
    if(sec < 0 || sec > 5 || cell < 0 || cell > ITOFMAXCELL - 1){
	Error("getGeomVolumeCell()","address out of bounds ! s = %d, c = %d !",sec,cell);
        return 0;
    }
    HModGeomPar* modPar = getModule(sec);
    HGeomCompositeVolume* compVol = modPar->getRefVolume();
    if(compVol) return  compVol->getComponent(cell);
    return 0;
}

Int_t HiTofGeomPar::getGeomVolumePointsCell(Int_t sec,Int_t cell, vector<HGeomVector>& points)
{
    // return the points of cell volume
    HGeomVolume* vol = getGeomVolumeCell(sec,cell);
    points.clear();
    if(vol){
	HGeomVector v;
	for(Int_t i = 0 ; i < vol->getNumPoints(); i++){
	    v = *(vol->getPoint(i));
            points.push_back(v);
	}
        return points.size();
    }
    return 0;
}

//--------------------------------------------------------------------------------
//  calculations
//--------------------------------------------------------------------------------

Bool_t HiTofGeomPar::initGeomHelpers(HSpecGeomPar* specGeom)
{
    // Helper objects for geometric calculations
    // needs HSpecGeomPar for transformations, therefore
    // the objects have to be initialized by calling  initGeomHelpers()
    // after init() function is finished (typically in the reinit()
    // function of a HReconstructor based task). The helper objects
    // will not be streamed with  the container

    if(!specGeom) {
	Error("initGeomHelpers()","You need to provide an initialized HSpecGeomPar to make this work!");
        return kFALSE;
    }

    for(Int_t s = 0; s < 6; s++)
    {
	HGeomVolume* fVol = specGeom->getSector(s);
	fLabToSec[s] = fVol->getTransform();                     // SEC SYS <-> LAB SYS
	HModGeomPar* modGeomPar = getModule(s);                  // TFNX
	HGeomTransform modTrans = modGeomPar->getLabTransform(); // MOD SYS <-> LAB SYS  of TFNX composit volume

	HGeomTransform modSecTrans = modTrans;
        modSecTrans.transTo(fLabToSec[s]);                       // MOD SYS <-> SEC SYS

	fSecToMod[s] = modSecTrans;
        fLabToMod[s] = modTrans;

	for(Int_t c = 0; c < ITOFMAXCELL; c++){
	    //---------------------------------------------------
            // CELL  coord
	    HGeomVolume* volCell = getGeomVolumeCell(s,c); // CELL SYS TF23FX (same coordinate sys as szintillator T23L )
            fModToCell[s][c] = volCell->getTransform();
            fSecToCell[s][c] = volCell->getTransform();
            fSecToCell[s][c].transFrom(fLabToMod[s]);       // Lab <-> CELL
            fSecToCell[s][c].transTo(fLabToSec[s]);         // Sec <-> CELL

	    for(Int_t i = 0; i < 4; i++){    // read only 4 points from one side, z set 0
		cellPoint[s][c][i] = *(volCell->getPoint(i));
		cellPoint[s][c][i].setZ(0.); // midplane of volume
	    }
	}
	//---------------------------------------------------
	// MODULE coord

	HGeomVector r0_mod(0.0, 0.0, 0.0);   // MOD SYS
	HGeomVector rz_mod(0.0, 0.0, 1.0);

	HGeomVector  niTof = fSecToMod[s].transFrom(rz_mod) - fSecToMod[s].transFrom(r0_mod);  // SEC
	normVecMod  [s].SetXYZ(niTof.getX(), niTof.getY(), niTof.getZ());
	HGeomVector ciTof = fSecToMod[s].transFrom(r0_mod);               // SEC
	centerVecMod[s].SetXYZ(ciTof.getX(), ciTof.getY(), ciTof.getZ());

    }

    //--------------------------------------------------
    // calculate PMT pos in cell on the upper end of the
    // cell
    //
    //          |  |  |  |
    //       p1------------p2
    //      b\             /
    //        \           /
    //        p0----------p3
    //

    for(Int_t s = 0; s < 6; s++){
	for(Int_t c = 0; c < ITOFMAXCELL; c++){
	    HGeomVector p1 = cellPoint[s][c][1];  // left  upper point
	    HGeomVector p2 = cellPoint[s][c][2];  // right upper point

	    HGeomVector dist = p2-p1;
	    dist /= 2.*ITOFNPMT;
	    p1   += dist;
	    dist *= 2;
	    HGeomVector tmp;
	    for(Int_t i = 0; i < ITOFNPMT; i++){
		tmp = dist;
		tmp *= i;
		pmtCell[c][i] = p1 + tmp;
		if(fDebug){
		    cout<<"HiTofGeomPar::initGeomHelpers() : PMT pos NR "<<"s "<<s<<" c "<<c<<" i "<<i<<" ("<<setw(10)<<pmtCell[c][i].X()<<","<<setw(10)<<pmtCell[c][i].Y()<<","<<setw(10)<< pmtCell[c][i].Z()<<")"<<endl;
		}
	    }
	}
    }
    return kTRUE;
}

Bool_t HiTofGeomPar::isInCell(HGeomVector& vcell, Int_t s,Int_t c, Float_t xRes, Float_t yRes)
{
    //  check if vcell (cell system) is inside cell (2D) Trapez
    //
    //       p1---------------p2
    //      b\                 /
    //        \               /
    //        p0-------------p3
    //              a
    //
    // checks if HGeomVector* vmodule

    if(s < 0 || s > 5 || c < 0 || c > ITOFMAXCELL - 1){
	Error("isInCell()","address out of bounds ! s = %d, c = %d !",s,c);
        return kFALSE;
    }


    HGeomVector& p0 = cellPoint[s][c][0];
    HGeomVector& p1 = cellPoint[s][c][1];

    if (p0.Y() > vcell.Y() + yRes){
	if(fDebug){
	    cout<<"HiTofGeomPar::isInCell  s "<<" c "<<setw(2)<<c
		<<" p0 "<<setw(10)<<  p0.X() <<" , "<<setw(10)<< p0.Y()
		<<" p1 "<<setw(10)<<  p1.X() <<" , "<<setw(10)<< p1.Y()
		<<" hit "<<setw(10)<<vcell.X()<<" , "<<setw(10)<<vcell.Y()<<" miss : hit below cell"<<endl;
	}
	return kFALSE;   // first check y range
    }

    if (p1.Y() < vcell.Y() - yRes) {
	if(fDebug){
	    cout<<"HiTofGeomPar::isInCell  s "<<s<<" c "<<setw(2)<<c
		<<" p0 "<<setw(10)<<  p0.X() <<" , "<<setw(10)<< p0.Y()
		<<" p1 "<<setw(10)<<  p1.X() <<" , "<<setw(10)<< p1.Y()
		<<" hit "<<setw(10)<<vcell.X()<<" , "<<setw(10)<<vcell.Y()<<" miss : hit above cell"<<endl;
	}
	return kFALSE;
    }

    // left side                            // x cut as function of y
    Double_t a = p1.X() - p0.X();
    Double_t b = p1.Y() - p0.Y();
    Double_t x = p0.X() + a * (( vcell.Y() - p0.Y())/b);

    if (x < vcell.X() - xRes) {
	if(fDebug){
	    cout<<"HiTofGeomPar::isInCell  s "<<s<<" c "<<setw(2)<<c
		<<" p0 "<<setw(10)<<  p0.X() <<" , "<<setw(10)<< p0.Y()
		<<" p1 "<<setw(10)<<  p1.X() <<" , "<<setw(10)<< p1.Y()
		<<" hit "<<setw(10)<<vcell.X()<<" , "<<setw(10)<<vcell.Y()<<" x cut "<<x<<" miss : x hit out of cell left side"<<endl;
	}
	return kFALSE;
    }

    HGeomVector& p2 = cellPoint[s][c][2];
    HGeomVector& p3 = cellPoint[s][c][3];

    // right side
    a = p2.X() - p3.X();
    b = p2.Y() - p3.Y();
    Double_t x1 = p3.X() + a * ((vcell.Y() - p3.Y())/b);

    if (x1 > vcell.X() + xRes) {
	if(fDebug){
	    cout<<"HiTofGeomPar::isInCell  s "<<s<<" c "<<setw(2)<<c
		<<" p0 "<<setw(10)<<  p0.X() <<" , "<<setw(10)<< p0.Y()
		<<" p1 "<<setw(10)<<  p1.X() <<" , "<<setw(10)<< p1.Y()
		<<" p2 "<<setw(10)<<  p2.X() <<" , "<<setw(10)<< p2.Y()
		<<" p3 "<<setw(10)<<  p3.X() <<" , "<<setw(10)<< p3.Y()
		<<" hit "<<setw(10)<<vcell.X()<<" , "<<setw(10)<<vcell.Y()<<" x cut "<<x1<<", miss : x hit out of cell right side"<<endl;
	}
	return kFALSE;
    }
    if(fDebug){
	cout<<"HiTofGeomPar::isInCell  s "<<s<<" c "<<setw(2)<<c
	    <<" p0 "<<setw(10)<<  p0.X() <<" , "<<setw(10)<< p0.Y()
	    <<" p1 "<<setw(10)<<  p1.X() <<" , "<<setw(10)<< p1.Y()
	    <<" p2 "<<setw(10)<<  p2.X() <<" , "<<setw(10)<< p2.Y()
	    <<" p3 "<<setw(10)<<  p3.X() <<" , "<<setw(10)<< p3.Y()
	    <<" hit "<<setw(10)<<vcell.X()<<" , "<<setw(10)<<vcell.Y()<<" calc x "<<setw(10)<<x<<" "<<x1<<" hit matched"
	    <<endl;
    }

    return kTRUE;
}

void HiTofGeomPar::calcSegPoints(HGeomVector& p1,HGeomVector& p2,
				 Double_t phiseg,Double_t thetaseg,Double_t roseg,Double_t zseg)
{
    // in sector sys : calc p1 p2 points (not MDC Hit!) on MdcSeg
    // sector system . in put RAD,mm

    Double_t Xseg[2],Yseg[2],Zseg[2];

    Xseg[0] = roseg*cos(phiseg+TMath::Pi()/2);
    Yseg[0] = roseg*sin(phiseg+TMath::Pi()/2);
    Zseg[0] = zseg;
    Xseg[1] = Xseg[0]+cos(phiseg)*sin(thetaseg);
    Yseg[1] = Yseg[0]+sin(phiseg)*sin(thetaseg);
    Zseg[1] = Zseg[0]+cos(thetaseg);

    p1.setXYZ(Xseg[0],Yseg[0],Zseg[0]);
    p2.setXYZ(Xseg[1],Yseg[1],Zseg[1]);
}


Bool_t HiTofGeomPar::transformCellHit(HGeomVector& cellhit,Int_t sec,Int_t cell, Int_t sys)
{
    // hit in CELL sys ->   LAB(sys=1) ,SEC(sys=2) or MOD(sys=3)

    if(sec < 0 || sec > 5 || cell < 0 || cell > ITOFMAXCELL - 1){
	Error("transformCellHit()","address out of bounds ! s = %d, c = %d !",sec,cell);
        return kFALSE;
    }

    cellhit = fModToCell[sec][cell].transFrom(cellhit) ; // MOD SYS
    if(sys == 2 )     cellhit = fSecToMod [sec].transFrom(cellhit) ; // SEC SYS
    else if(sys == 1) cellhit = fLabToMod [sec].transFrom(cellhit) ; // LAB SYS
    return kTRUE;
}

Bool_t HiTofGeomPar::transformModHit(HGeomVector& modhit,Int_t sec, Int_t sys)
{
    // hit in MOD sys ->   LAB(sys=1) ,SEC(sys=2)

    if(sec < 0 || sec > 5 ){
	Error("transformModHit()","address out of bounds ! s = %d !",sec);
        return kFALSE;
    }

    if     (sys == 2) modhit = fSecToMod [sec].transFrom(modhit) ; // SEC SYS
    else if(sys == 1) modhit = fLabToMod [sec].transFrom(modhit) ; // LAB SYS
    return kTRUE;
}

Bool_t HiTofGeomPar::transformSecHit(HGeomVector& sechit,Int_t sec, Int_t sys)
{
    // hit in SEC sys ->   LAB(sys=1) ,MOD(sys=3)

    if(sec < 0 || sec > 5 ){
	Error("transformSecHit()","address out of bounds ! s = %d !",sec);
        return kFALSE;
    }

    if     (sys == 3) sechit = fSecToMod [sec].transTo  (sechit) ; // MOD SYS
    else if(sys == 1) sechit = fLabToSec [sec].transFrom(sechit) ; // LAB SYS
    return kTRUE;
}

Bool_t HiTofGeomPar::transformLabToSec(HGeomVector& hit ,Int_t sec, Int_t sys)
{
    // hit (LAB or SEC) sys ->   LAB(sys=1) ,SEC(sys=1)


    if(sec < 0 || sec > 5 ){
	Error("transFormLabToSec()","address out of bounds ! s = %d !",sec);
	return kFALSE;
    }
    if     (sys == 2) hit = fLabToSec [sec].transTo  (hit) ; // SEC SYS
    else if(sys == 1) hit = fLabToSec [sec].transFrom(hit) ; // LAB SYS

    return kTRUE;
}

Bool_t HiTofGeomPar::findIntersectionLinePlane(HGeomVector& pointIntersect,
					       const HGeomVector& pos, const HGeomVector& dir,
					       const HGeomVector& planeCenter, const HGeomVector& planeNormal)
{
    // Finds the intersection point of a straight line with any plane.
    // pointIntersect: the intersection point (return value).
    // pos:            a point on the straight line.
    // dir:            direction of the straight line.
    // planeCenter:    a point on the plane.
    // planeNormal:    normal vector of the plane.

    Double_t denom = planeNormal.scalarProduct(dir);
    if (denom != 0.0) {
	Double_t t = ((planeCenter.getX() - pos.getX()) * planeNormal.getX() +
		      (planeCenter.getY() - pos.getY()) * planeNormal.getY() +
		      (planeCenter.getZ() - pos.getZ()) * planeNormal.getZ()) / denom;

	HGeomVector tmp = dir;
        tmp *= t;
	pointIntersect = pos + tmp;
	return kTRUE;
    } else {
	::Warning("findIntersection()", "No intersection point found : (plane || track)");
	return kFALSE;
    }
    return kFALSE;
}

Bool_t HiTofGeomPar::findHitCell(HGeomVector& pointHit,Int_t& cell,Float_t distPMT[],
				 Int_t s,Double_t phi,Double_t theta,Double_t r, Double_t z)
{
    // calc hit point of track (CELL SYS) (r [mm],z [mm],theta [deg] [0-90], phi [deg] [0-360])
    // (as in HParticleCand)
    // distPM[12] return distances of the hit to the PMTs of the cells

    pointHit.setXYZ(0,0,0);
    cell = -1;
    for(Int_t i = 0; i < ITOFNPMT; i++){ distPMT[i] = 0;}

    if(s < 0 || s > 5 ){
	Error("findHitCell()","address out of bounds ! s = %d !",s);
        return kFALSE;
    }

    HGeomVector p1,p2;
    Double_t phiSec = fmod(phi,60.F) + 60;
    calcSegPoints(p1,p2,phiSec*TMath::DegToRad(),theta*TMath::DegToRad(),r,z);

    HGeomVector pointIntersect;
    HGeomVector dir = p2 - p1;
    findIntersectionLinePlane(pointIntersect,p1,dir, centerVecMod[s], normVecMod[s]); // hit on MODULE in SEC sys

    if(fDebug){
	cout<<"HiTofGeomPar::findHitCell() :  intersect  ("
	    <<setw(10)<<pointIntersect.X()<<","<<setw(10)<<pointIntersect.Y()<<","<<setw(10)<<pointIntersect.Z()
	    <<") "<<" s "<<s<<" theta "<<setw(10)<<theta<<" phi "<<setw(10)<<phi<<" r "<<setw(10)<<r<<" z "<<setw(10)<<z
	    <<endl;
    }

    HGeomVector pMod = pointIntersect;
    pMod = fSecToMod[s].transTo(pMod); // MOD Sys

    HGeomVector tmp;
    for(Int_t c = 0; c < ITOFMAXCELL; c ++){ // find which cell has been hit
        HGeomVector cellhit = pMod;
	cellhit = fModToCell[s][c].transTo(cellhit);  // CELL sys

	if(isInCell(cellhit,s,c)){
	    for(Int_t i = 0; i < ITOFNPMT; i++){
		tmp = cellhit - pmtCell[c][i];
                distPMT[i] = (Float_t) tmp.length();
		if(fDebug){
		    cout<<"HiTofGeomPar::findHitCell() : PMT dist NR "<<i<<" ("<<setw(10)<<distPMT[i]<<","<<setw(10)<<distPMT[i]<<","<<setw(10)<<distPMT[i]<<")"<<endl;
		}
	    }
            cell = c;
	    pointHit = cellhit;
	    return kTRUE;
	}
    }


    return kFALSE;
}

Bool_t HiTofGeomPar::distPMT(Float_t distPMT[], const HGeomVector pointHit, const Int_t s,const Int_t c)
{
    // distPM[12] return distances of the hit to the PMTs of the cells s,c
    // for hit point pointHit (CELL sys)

    if(s < 0 || s > 5 || c < 0 || c > ITOFMAXCELL - 1){
	Error("distPMT(()","address out of bounds ! s = %d, c = %d !",s,c);
        return kFALSE;
    }

    HGeomVector tmp;
    for(Int_t c = 0; c < ITOFMAXCELL; c ++){ // find which cell has been hit
	for(Int_t i = 0; i < ITOFNPMT; i++){
	    tmp = pointHit - pmtCell[c][i];
	    distPMT[i] = (Float_t)tmp.length();
	    if(fDebug){
	        cout<<"distPMT() : PMT dist NR "<<i<<" ("<<setw(10)<<distPMT[i]<<","<<setw(10)<<distPMT[i]<<","<<setw(10)<<distPMT[i]<<")"<<endl;
	    }
	}
    }
    return kTRUE;
}