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

// C/C++ Headers ----------------------
#include <iostream>

// ROOT Headers ----------------------
#include "TGeoManager.h"
#include "TGeoBBox.h"
#include "TVector3.h"
#include "TString.h"
#include "TMath.h"
// Collaborating Class Headers --------
#include "PndMvdDetectorDescr.h"

/// #include "PndMvdPixelModule.h"
/// #include "PndMvdStripModule.h"

#include "PndMvdDigiPixel.h"
#include "PndMvdDigiStrip.h"
#include "PndMvdHit.h"
#include "PndMvdCalcFePixel.h"
#include "PndMvdCalcStrip.h" 
// enum SensorSide { kTOP, kBOTTOM };

bool verbose = false;

PndMvdSimpleHitMaker::PndMvdSimpleHitMaker(PndMvdDetectorDescr* mvdDetDescr)
: PndMvdHitMaker(mvdDetDescr)
{
///   mvdDetDescr->detector()->getNomModuleParams(fpixelModules, fstripModules);
  fpixelCellX = mvdDetDescr->pixelCellX();
  fpixelCellY = mvdDetDescr->pixelCellY();
  fstripX = mvdDetDescr->stripX();
  fstripY = mvdDetDescr->stripY();
}

bool
PndMvdSimpleHitMaker::makeHits(
  std::vector<PndMvdDigiPixel*>& pixelarray,
  std::vector<PndMvdDigiStrip*>& striparray,
  std::vector<PndMvdHit*>& hitarray)
{// TODO digi arrays als const uebergeben?
  //check consistencies

  //Every digi will become one hit
//   PrintParameters();
  // TODO read Parameters from Database
  PndMvdCalcFePixel FeCalc(76, 84, 10);
  bool once = true;
      //  ---  Pixals  ----------------------------
  for (std::vector<PndMvdDigiPixel*>::iterator itpixel = pixelarray.begin();
       itpixel != pixelarray.end(); itpixel++)
  {
    PndMvdDigiPixel* myPixelDigi = *itpixel; // TODO remove this, work only with the iterator
    if (myPixelDigi==0)
    {
      std::cout<<"PndMvdSimpleHitMaker::makeHits(): myPixelDigi does not exist!"<<std::endl;
      continue;
    }
    TString detname = myPixelDigi->GetDetName();
    gGeoManager->cd(detname); 

    TGeoVolume* actVolume = gGeoManager->GetCurrentVolume();
    TGeoBBox* actBox = (TGeoBBox*)(actVolume->GetShape());
    TVector3 sensorDim;
    sensorDim.SetX(actBox->GetDX());
    sensorDim.SetY(actBox->GetDY());
    sensorDim.SetZ(actBox->GetDZ());
    Int_t fe = myPixelDigi->GetFE();
    Int_t col = myPixelDigi->GetPixelColumn();
    Int_t row = myPixelDigi->GetPixelRow();
    FeCalc.CalcSensorColRow(col,row,fe); // backmap channel&FE to pixel cell No.
    Double_t dcol,drow;
      if (col < 0){
        dcol = -0.5;
        std::cerr<<"#########   NEGATIVE  COL   ("<<col<<")   ###########"<<std::endl;
      }
      else {
        dcol = 0.5;
      }
      if (row < 0){
        drow = -0.5;
        std::cerr<<"#########   NEGATIVE  ROW   ("<<row<<")   ###########"<<std::endl;
      }
      else {
        drow = 0.5;
      }


    Double_t x = fpixelCellX * (col + dcol) - sensorDim.X();
    Double_t y = fpixelCellY * (row + drow) - sensorDim.Y();
    Double_t z = 0.;
    //Double_t z = sensorDim.Z();
    //Double_t z = -1.*sensorDim.Z();

    Double_t local[3], master[3];

    local[0]=x; local[1]=y; local[2]=z;
    gGeoManager->LocalToMaster(local,master);
    TVector3 hitPos(master[0],master[1],master[2]);

    local[0]=fpixelCellX/sqrt(12.); local[1]=fpixelCellY/sqrt(12.); local[2]=0.;
    gGeoManager->LocalToMaster(local,master);
    TVector3 hitErr(master[0],master[1],master[2]);

    // TODO Do we need the views seperated?
   PndMvdHit* hit = new PndMvdHit(myPixelDigi->GetDetID(), (myPixelDigi->GetDetName()).Data(),
        hitPos, hitErr, myPixelDigi->GetIndex(),myPixelDigi->GetCharge() , 1);

    hitarray.push_back(hit);

  } // end of pixel digiloop

      //  ---  Strips  ----------------------------
  //prepare arrays and variables

  enum SensorSide side;
  Int_t nrFeChannels = 128;
  Int_t nrTopStrips = 10*nrFeChannels;
  TVector2 topAnchor(0.,0.);
  TVector2 botAnchor(0.,0.);
  Double_t orient=TMath::Pi()*(0.5);
  Double_t skew=TMath::Pi()*(0.5);
  ///TODO: Get Params from database 
  PndMvdCalcStrip stripcalcTOP(fstripX,orient, nrTopStrips, nrFeChannels,topAnchor,0.,0.);
  PndMvdCalcStrip stripcalcBOT(fstripY,orient + skew, nrTopStrips, nrFeChannels,botAnchor,0.,0.);
/*  PndMvdCalcStrip stripcalcTOP(fstripX,TMath::Pi()*(0.5), nrStrips , 128, topAnchor,0.,0.);
  PndMvdCalcStrip stripcalcBOT(fstripY,TMath::Pi()*(1.0), nrStrips , 128, botAnchor,0.,0.);*/
  TVector2 topDirection = stripcalcTOP.GetStripDirection();
  TVector2 botDirection = stripcalcBOT.GetStripDirection();
  TVector2 point;
  TString detname;
  TGeoVolume* actVolume;
  TGeoBBox* actBox;
  TVector3 sensorDim;
  Int_t fe,channel;
  Int_t strip,index,mcindex;
  
  std::map<Int_t,std::map<Int_t,Int_t> > hitmapTOP;
  std::map<Int_t,std::map<Int_t,Int_t> > hitmapBOT;

  for (std::vector<PndMvdDigiStrip*>::iterator itstrip = striparray.begin();
       itstrip != striparray.end(); itstrip++)
  {
    index = distance(striparray.begin(),itstrip) + 1;
    mcindex = (*itstrip)->GetIndex();
    if(verbose) std::cout<<"itstrip = "<<*itstrip
      <<"   this corresponds to index "<<index
      <<" with mcindex "<<mcindex<<std::endl;
    if (*itstrip==0)
    { std::cout<<"PndMvdSimpleHitMaker::makeHits(): *itstrip does not exist!"<<std::endl;
      continue;  }

    fe = (*itstrip)->GetFE();
    channel = (*itstrip)->GetChannel();
    stripcalcTOP.CalcFeChToStrip(fe,channel,strip,side);
    if (side == kTOP)  {
      if(verbose) std::cout<<"\t\t\ttop"<<std::endl;
      hitmapTOP[mcindex][index] = strip;
    } else if (side == kBOTTOM) {
      if(verbose) std::cout<<"\t\t\tbottom"<<std::endl;
      hitmapBOT[mcindex][index] = strip;
    } else {
      std::cerr<<"PndMvdSimpleHitMaker::makeHits: SensorSide \""<<side
               <<"\" is not defined!"<<std::endl;
    }
  } // end StripDigi loop

  std::map<Int_t,Int_t> toplist;
  std::map<Int_t,Int_t> botlist;
  Int_t counter = 0;
  Double_t meanstrip = 0.,meanstriptop = 0.,meanstripbot = 0.;
  TVector2 meantopPoint, meanbotPoint;

  for(std::map<Int_t,std::map<Int_t,Int_t> >::iterator itToplist = hitmapTOP.begin();
      itToplist != hitmapTOP.end(); itToplist++)
  {
//     mcindex = distance(hitmapTOP.begin(),itToplist);
    mcindex = itToplist->first;
    toplist = itToplist->second;
    botlist = hitmapBOT[mcindex];

    // --- kTOP ---
    counter = 0;
    meanstrip = 0.;
    for(std::map<Int_t,Int_t>::iterator itTop = toplist.begin();
        itTop != toplist.end(); itTop++)
    {
      counter++;
      meanstrip+=itTop->second;
    }
    if(counter==0){ if(verbose) std::cout<<"  ***  next step (top)"<<std::endl; continue; }
    meanstrip = meanstrip / counter;
    stripcalcTOP.CalcStripPointOnLine(meanstrip, meantopPoint);
    meanstriptop = meanstrip;


    // --- kBOTTOM ---
    counter = 0;
    meanstrip = 0.;
    for(std::map<Int_t,Int_t>::iterator itBot = botlist.begin();
        itBot != botlist.end(); itBot++)
    {
      counter++;
      meanstrip+=itBot->second;
    }
    if(counter==0){ if(verbose) std::cout<<"  ***  next step (bot)"<<std::endl; continue; }
    meanstrip = meanstrip / counter;
    stripcalcBOT.CalcStripPointOnLine(meanstrip, meanbotPoint);
    meanstripbot = meanstrip;
    // --- Convert ---
//     TVector2 onsensorPoint = stripcalcTOP.CalcCrossingPoint(meanstriptop,stripcalcBOT,meanstripbot);
    TVector2 onsensorPoint = CalcLineCross(meantopPoint, topDirection, meanbotPoint, botDirection);
    if (verbose) std::cout<<"Mean Top side: ("<<meantopPoint.X()<<" ; "<<meantopPoint.Y()<<")"<<"\n"
      <<"Mean Bot side: ("<<meanbotPoint.X()<<" ; "<<meanbotPoint.Y()<<")"<<"\n"
      <<"On Sensor Hit: ("<<onsensorPoint.X()<<" ; "<<onsensorPoint.Y()<<")"<<std::endl;
    PndMvdDigiStrip* aDigi = striparray[(toplist.begin())->first];
    detname = aDigi->GetDetName();
    gGeoManager->cd(detname);
    actVolume = gGeoManager->GetCurrentVolume();
    actBox = (TGeoBBox*)(actVolume->GetShape());
    sensorDim.SetX(actBox->GetDX());
    sensorDim.SetY(actBox->GetDY());
    sensorDim.SetZ(actBox->GetDZ());

    Double_t local[3], master[3];
    local[0]=onsensorPoint.X(); local[1]=onsensorPoint.Y(); local[2]=0;
    //shift sensor system into middle
    local[0]-=sensorDim.X();
    local[1]-=sensorDim.Y();
    local[2]-=sensorDim.Z();
    gGeoManager->LocalToMaster(local,master);
    TVector3 hitPos(master[0],master[1],master[2]);
    local[0]=fstripX/sqrt(12.); local[1]=fstripY/sqrt(12.); local[2]=0.;
    gGeoManager->LocalToMaster(local,master);
    TVector3 hitErr(master[0],master[1],master[2]);


    // --- add cluster to list ---
    PndMvdHit* hit = new PndMvdHit(aDigi->GetDetID(), (aDigi->GetDetName()).Data(),
        hitPos, hitErr, mcindex, aDigi->GetCharge(), 1 );



    hitarray.push_back(hit);

  } 


//   std::cout<<"\t - end of PndMvdSimpleHitMaker::makeHits()."<<std::endl;
  return true;
}


void PndMvdSimpleHitMaker::PrintParameters()
{
   std::cout<<"PndMvdSimpleHitMaker::PrintParameters()"
            <<"\nfpixelCellX:\t"<<fpixelCellX
            <<"\nfpixelCellY:\t"<<fpixelCellY
            <<"\nfstripX:\t"<<fstripX
            <<"\nfstripY:\t"<<fstripY
            <<std::endl;

}


TVector2 PndMvdSimpleHitMaker::CalcLineCross(TVector2 point1, TVector2 dir1, TVector2 point2, TVector2 dir2)
{
  Double_t dx, dy, s, t, M, x, y;
  dx = point2.X() - point1.X();
  dy = point2.Y() - point1.Y();

  M  = dir1.X()*dir2.Y() - dir1.Y()*dir2.X();
  
  if(M !=0.)
  {
    s  = dir1.Y()*dx/M - dir1.X()*dy/M;
    x  = point2.X() + dir2.X()*s;
    y  = point2.Y() + dir2.Y()*s;
  }
  else
  {
    std::cout<<"Warning in PndMvdSimpleHitMaker::CalcLineCross(): M=0 setting (x,y) to 0"<<std::endl;
    x=0.;y=0.;
  }
  
  TVector2 result(x,y);
  return result;
}