// -------------------------------------------------------------------------
// -----                CbmStsHitProducerIdeal source file             -----
// -----                  Created 10/01/06  by V. Friese               -----
// -------------------------------------------------------------------------


#include "TClonesArray.h"
#include "TArrayD.h"
#include "TGeoManager.h"

#include "CbmRootManager.h"
#include "MvdHybridHitProducer.h"
#include "MvdHit.h"
#include "MvdPoint.h"
#include "CbmRunAna.h"
#include "CbmRuntimeDb.h"
#include "CbmGeoNode.h"
#include "CbmRuntimeDb.h"
#include "CbmGeoNode.h"
#include "CbmGeoVector.h"
#include "StringVector.h"
#include "MvdCalcPixel.h"
#include "MvdCalcFePixel.h"
#include "MvdDigiPixel.h"

// -----   Default constructor   -------------------------------------------
MvdHybridHitProducer::MvdHybridHitProducer() :
  CbmTask("MVD Hybrid Hit Producer")
{
  fBranchName   = "MVDPoint";
  pixelHits = 0;
//  fHitArray  = new TClonesArray("MvdHit");
//	fPixelArray	= new TClonesArray("MvdPixelHit");
}
// -------------------------------------------------------------------------

MvdHybridHitProducer::MvdHybridHitProducer(Double_t lx, Double_t ly, Double_t threshold, Double_t noise) :
  CbmTask("MVD Hybrid Hit Producer") 
{
  fBranchName   = "MVDPoint";
//  fHitArray  = new TClonesArray("MvdHit");
//	fPixelArray	= new TClonesArray("MvdPixelHit");
  flx = lx;
  fly = ly;
  fthreshold = threshold;
  fnoise = noise;
  pixelHits = 0;
	std::cout << "MVD Hybrid Hit Producer initiated" << std::endl;
}
// -------------------------------------------------------------------------


// -----   Destructor   ----------------------------------------------------
MvdHybridHitProducer::~MvdHybridHitProducer() 
{ 
}
// -------------------------------------------------------------------------

// -----   Initialization  of Parameter Containers -------------------------
void MvdHybridHitProducer::SetParContainers()
{
  // Get Base Container
  CbmRunAna* ana = CbmRunAna::Instance();
  CbmRuntimeDb* rtdb=ana->GetRuntimeDb();
  fDigiPar = (MvdDigiPar*)(rtdb->getContainer("MvdDigiPar"));
  fGeoMappingPar = (MvdGeoMappingPar*)(rtdb->getContainer("MvdGeoMappingPar"));
}

InitStatus MvdHybridHitProducer::ReInit()
{
  CbmRunAna* ana = CbmRunAna::Instance();
  CbmRuntimeDb* rtdb=ana->GetRuntimeDb();
  fDigiPar=(MvdDigiPar*)(rtdb->getContainer("MvdDigiPar"));
  fGeoMappingPar = (MvdGeoMappingPar*)(rtdb->getContainer("MvdGeoMappingPar"));
  
  return kSUCCESS;
}

// -----   Public method Init   --------------------------------------------
InitStatus MvdHybridHitProducer::Init() 
{
    CbmRunAna* ana = CbmRunAna::Instance();
  CbmRootManager* ioman = CbmRootManager::Instance();

  if ( ! ioman ) 
    {
      std::cout << "-E- MvdHybridHitProducer::Init: "
     << "RootManager not instantiated!" << std::endl;
      return kFATAL;
    }
  fPointArray = (TClonesArray*) ioman->GetObject(fBranchName);

  if ( ! fPointArray ) 
    {
      std::cout << "-W- MvdHybridHitProducer::Init: "
     << "No MVDPoint array!" << std::endl;
      return kERROR;
  }

  // Create and register output array
  fHitArray = new TClonesArray("MvdHit");
  ioman->Register("MVDHit", "MVD", fHitArray, kTRUE);

  // Create and register output array
  fPixelArray = new TClonesArray("MvdDigiPixel");
  ioman->Register("MVDPixelHit", "MVD", fPixelArray, kTRUE);
  
  fFePixelArray = new TClonesArray("MvdDigiPixel");
  ioman->Register("MVDFePixelHit", "MVD", fFePixelArray, kTRUE);	
  
  std::cout << "-I- MvdHybridHitProducer: Intialisation successfull" << std::endl;
  std::cout << "-I- Test: " << fGeoMappingPar->detIdPairArray->GetEntries() << std::endl;

  fDigiPar->dimX = flx;
  fDigiPar->dimY = fly;
  fDigiPar->threshold = fthreshold;
  fDigiPar->noise = fnoise;

  fDigiPar->setInputVersion(ana->GetRunId(),1);
  fDigiPar->setChanged();
  
  
  return kSUCCESS;
}
// -------------------------------------------------------------------------

// -----   Public method Exec   --------------------------------------------
void MvdHybridHitProducer::Exec(Option_t* opt)
{
  // Reset output array
  if ( ! fPixelArray )
    Fatal("Exec", "No PixelArray");
  
//   fHitArray->Clear();
  fPixelArray->Clear();
//   fFePixelArray->Clear();
  
  // Declare some variables
  MvdPoint
    *point = NULL;

  Int_t 
    detID = 0,       // Detector ID
    trackID = 0;     // Track index

  // Loop over MvdPoints
  Int_t 
    nPoints = fPointArray->GetEntriesFast();
  Int_t iPixel = 0;
  Int_t iFePixel = 0;
  
  for (Int_t iPoint = 0; iPoint < nPoints; iPoint++) 
  {
      point = (MvdPoint*) fPointArray->At(iPoint);

      if ( ! point){
        std::cout<< "No Point!" << std::endl;
         continue;
      }
      if (fVerbose > 1){
        std::cout << "****Global Point: " << std::endl;
        point->Print("");
      }
      CbmGeoVector posInL, posOutL, meanPos, meanPosL;
      GetLocalHitPoints(point, posInL, posOutL);
      
      if (fVerbose > 1){  
        std::cout << "Local Hits: " << std::endl;
        posInL.print();
        posOutL.print();
      }
      meanPosL = posInL + posOutL;
      meanPosL /= 2;
      if (fVerbose > 1){
        meanPosL.print();
        std::cout << "Energy: " << point->GetEnergyLoss() << std::endl;
      }

      std::vector<MvdPixel> myFePixels;
      if ( (point->GetDetName()).Contains("Pixel") )
      {
        // Define sensor by pixelsizes threshold and noise from macro outside
        MvdCalcPixel PixelCalc(flx, fly, fthreshold, fnoise);
        // Calculate a cluster of Pixels fired (in sensor system)
        std::vector<MvdPixel> myPixels = PixelCalc.GetPixels (posInL.getX(), posInL.getY(), posInL.getZ(),
                                       posOutL.getX(), posOutL.getY(), posOutL.getZ(),
                                       point->GetEnergyLoss()*1E9 );
        if (myPixels.size() == 0){
          if (fVerbose > 1) std::cout << "Deposited charge below threshold" << std::endl;
        } else {
          pixelHits += myPixels.size();
          if (fVerbose > 1) std::cout  << "SensorPixels: " << std::endl;
          for(uint i = 0; i < myPixels.size(); i++)
          {
            myPixels[i].SetDetName(point->GetDetName().Data());
            if (fVerbose > 1) std::cout << myPixels[i] << std::endl;
          }
          // TODO read Parameters from Database
          // Calculate channel numbers
          MvdCalcFePixel FeCalc(76, 84, 10);
          myFePixels = FeCalc.CalcFEHits(myPixels);
          if (fVerbose > 1){
            std::cout << "FePixels: " << myFePixels.size() << std::endl;
            for(uint i = 0; i < myFePixels.size(); i++){
               std::cout << myFePixels[i] << std::endl;
            }
          }
          // MCTrack ID
          trackID = point->GetTrackID();
          detID   = point->GetDetectorID();
          for (uint iPix = 0; iPix < myFePixels.size(); iPix++){
            new ((*fPixelArray)[iFePixel++])
                  MvdDigiPixel( iPoint, detID,   point->GetDetName(),myFePixels[iPix].GetFE(),
                               myFePixels[iPix].GetCol(), myFePixels[iPix].GetRow(),
                               myFePixels[iPix].GetCharge()   );
          }
        }//endif myPixels.size

      }//endif pixel


    }   // Loop over MCPoints

  // Event summary
  std::cout << "-I- MvdDigiTask: " << nPoints << " MvdPoints, "
       << pixelHits << " Hits created." << " " << iPixel<< std::endl;

}


// // -----   Public method Exec   --------------------------------------------
// void MvdHybridHitProducer::Exec(Option_t* opt) 
// {
//   // Reset output array
//   if ( ! fHitArray ) 
//     Fatal("Exec", "No HitArray");
//   
//   fHitArray->Clear();
//   fPixelArray->Clear();
//   fFePixelArray->Clear();
//   
//   // Declare some variables
//   MvdPoint
//     *point = NULL;
// 
//   Int_t 
//     detID = 0,       // Detector ID
//     trackID = 0;     // Track index
// 
//   // Loop over MvdPoints
//   Int_t 
//     nPoints = fPointArray->GetEntriesFast();
//   Int_t iPixel = 0;
//   Int_t iFePixel = 0;
//   
//   for (Int_t iPoint = 0; iPoint < nPoints; iPoint++) 
//   {
//       point = (MvdPoint*) fPointArray->At(iPoint);
// 
//       if ( ! point){
//         std::cout<< "No Point!" << std::endl;
//          continue;
//       }
//       if (fVerbose > 1){
//         std::cout << "****Global Point: " << std::endl;
//         point->Print("");
//       }
//       CbmGeoVector posInL, posOutL, meanPos, meanPosL;
//       GetLocalHitPoints(point, posInL, posOutL);
//       
//       if (fVerbose > 1){  
//         std::cout << "Local Hits: " << std::endl;
//         posInL.print();
//         posOutL.print();
//       }
//       meanPosL = posInL + posOutL;
//       meanPosL /= 2;
//       if (fVerbose > 1){
//         meanPosL.print();
//         std::cout << "Energy: " << point->GetEnergyLoss() << std::endl;
//       }
//         MvdCalcPixel PixelCalc(flx, fly, fthreshold, fnoise);
// 
// //       Calculate a cluster of Pixels fired
//         std::vector<MvdPixel> myPixels = PixelCalc.GetPixels (posInL.getX(), posInL.getY(), posInL.getZ(),
//                                        posOutL.getX(), posOutL.getY(), posOutL.getZ(),
// 															   point->GetEnergyLoss()*1E9);
//         if (myPixels.size() == 0){
// 	  	std::cout << "Deposited charge below threshold" << std::endl;
// 	  }
// 	  else {
//           pixelHits += myPixels.size();
// 	      if (fVerbose > 1)
// 		  	std::cout  << "SensorPixels: " << std::endl;
// 		  for(uint i = 0; i < myPixels.size(); i++){
//             myPixels[i].SetDetName(point->GetDetName().Data());
// 	          if (fVerbose > 1)
// 	      	 std::cout << myPixels[i] << std::endl;
//           }
//           // TODO read Parameters from Database
//           // Calculate channel numbers
//           MvdCalcFePixel FeCalc(76, 84, 10);
// 
//       // this is a cluster, ideal finding
// 		  std::vector<MvdPixel> myFePixels = FeCalc.CalcFEHits(myPixels);
// 		  
//           if (fVerbose > 1){
//             std::cout << "FePixels: " << myFePixels.size() << std::endl;
//             for(uint i = 0; i < myFePixels.size(); i++){
//                std::cout << myFePixels[i] << std::endl;
//             }
//           }
//           // MCTrack ID
//           trackID = point->GetTrackID();
//           detID   = point->GetDetectorID();
// 	      //std::cout << "Local Coordinates from Point:" << std::endl;    
// 	      //std::cout << point->GetXInLocal()<< " " << point->GetYInLocal() << " " << point->GetZInLocal() << std::endl;
// 	      //std::cout << point->GetXOutLocal() << " " << point->GetYOutLocal() << " " << point->GetZOutLocal() << std::endl;
// 
// 	      MvdHit hitCoord = CalcGlobalPoint(myPixels);
//   
// 	      TVector3 MCCoord(point->GetX() + point->GetXOut(), point->GetY()+point->GetYOut(), point->GetZ()+point->GetZOut());
// 	      MCCoord *= 0.5;
//   
//         TVector3 pos = hitCoord.GetPosition();
//         if (fVerbose > 1){
// 		      std::cout << "Calculated Hit: " << pos.X() << " " << pos.Y() << " " << pos.Z() << " " << hitCoord.GetCharge() << std::endl;
//           std::cout << "MC Hit:" << MCCoord.X() << " " << MCCoord.Y() << " " << MCCoord.Z() << std::endl;
//           std::cout << "Difference: " << (MCCoord - pos).Mag() << std::endl;
// 			  std::cout << "ChargeDifference: " << (hitCoord.GetCharge() * 3.61 - point->GetEnergyLoss()*1E9) << std::endl;
//         }
//   
//         hitCoord.SetTrackID(trackID);
//         hitCoord.SetDetectorID(detID);
//         hitCoord.SetDetName(point->GetDetName());
//         hitCoord.SetRefIndex(iPoint);
// 		new ((*fHitArray)[iPoint])MvdHit(hitCoord);
// 
//         for (uint iPix = 0; iPix < myPixels.size(); iPix++){
// //          if (myPixels[iPix].GetCharge() > 0)
// 				new ((*fPixelArray)[iPixel++]) MvdDigiPixel(iPoint, detID, point->GetDetName(),myPixels[iPix].GetFE(),myPixels[iPix].GetCol(), myPixels[iPix].GetRow(), myPixels[iPix].GetCharge());
// 		}
// 		for (uint iPix = 0; iPix < myFePixels.size(); iPix++){
// 		//	if (myPixels[iPix].GetCharge() > 0)
// 				new ((*fFePixelArray)[iFePixel++]) MvdDigiPixel(iPoint, detID, point->GetDetName(),myFePixels[iPix].GetFE(),
// 														      myFePixels[iPix].GetCol(), myFePixels[iPix].GetRow(),
// 										      				  myFePixels[iPix].GetCharge());
// 		}
// 	  }
//     }   // Loop over MCPoints
// 
//   // Event summary
//   std::cout << "-I- MvdHybridHitProducer: " << nPoints << " MvdPoints, "
//        << pixelHits << " Hits created." << " " << iPixel<< std::endl;
// 
// }


void MvdHybridHitProducer::GetLocalHitPoints(MvdPoint* myPoint, CbmGeoVector& myHitIn, CbmGeoVector& myHitOut)
{
  
  if (fVerbose > 1)
    std::cout << "GetLocalHitPoints" << std::endl;
  TGeoHMatrix trans = GetTransformation(myPoint->GetDetName().Data());
  
  double posIn[3];
  double posOut[3];
  double posInLocal[3];
  double posOutLocal[3];
  
  posIn[0] = myPoint->GetX();
  posIn[1] = myPoint->GetY();
  posIn[2] = myPoint->GetZ();
  
  posOut[0] = myPoint->GetXOut();
  posOut[1] = myPoint->GetYOut();
  posOut[2] = myPoint->GetZOut();
  
  if (fVerbose > 1){
    for (int i = 0; i < 3; i++)
      std::cout << "posIn "<< i << ": " << posIn[i] << std::endl;
  
    trans.Print("");
  }
  
  trans.MasterToLocal(posIn, posInLocal);
  trans.MasterToLocal(posOut, posOutLocal);
  
  if (fVerbose > 1) {
    for (int i = 0; i < 3; i++){
      std::cout << "posInLocal "<< i << ": " << posInLocal[i] << std::endl;
      std::cout << "posOutLocal "<< i << ": " << posOutLocal[i] << std::endl;
    }
  }

  
  //posIn/OutLocal have the center of the coordinate system in the center of the shape
  //typically sensors have their coordinate system centered at the lower left corner
  
  TVector3 offset = GetSensorDimensions(myPoint->GetDetName().Data());
  
  posInLocal[0] += offset.x();
  posInLocal[1] += offset.y();
  //posInLocal[2] += offset.z();
  
  posOutLocal[0] += offset.x();
  posOutLocal[1] += offset.y();
  //posOutLocal[2] += offset.z();
  
  
  myHitIn.setVector(posInLocal);
  myHitOut.setVector(posOutLocal);
    
}

MvdHit MvdHybridHitProducer::CalcGlobalPoint(std::vector<MvdPixel> pixels)
{
  Double_t col = 0, row = 0, charge = 0;
  Double_t colB = 0, rowB = 0;
  int count = 0;
  
  if (pixels.size() == 1){
    if (pixels[0].GetCharge() > 0){
      col = pixels[0].GetCol();
      if (col < 0)
        col -= 0.5;
      else col += 0.5;
      row = pixels[0].GetRow();
      if (row < 0)
        row -= 0.5;
      else row += 0.5;
      count = 1;
      charge = pixels[0].GetCharge();
    }
    colB = col;
    rowB = row;
  }
  else {
    //std::cout << "Multiple Hits!" << std::endl;
    for (uint i = 0; i < pixels.size(); i++){
      //std::cout << "ActCol / Row" << col << " " << row << " added Col/Row " << pixels[i].GetCol() << " " << pixels[i].GetRow() << std::endl;
      if (pixels[i].GetCharge() > 0){
        col += (pixels[i].GetCol()*pixels[i].GetCharge());
        row += (pixels[i].GetRow()*pixels[i].GetCharge());
        charge += pixels[i].GetCharge();
        colB += (pixels[i].GetCol());
        rowB += (pixels[i].GetRow());
        count++;
      }
    }
    if (count > 0){
      colB /= count;
      rowB /= count;
      if (charge > 0){
        col /= charge;
        row /= charge;
      }
      else {
        col = colB;
        row = rowB;
      }
      if (col < 0){
        col -= 0.5;
        colB -= 0.5;
      }
      else {
        col += 0.5;
        colB += 0.5;
      }
      if (row < 0){
        row -= 0.5;
        rowB -= 0.5;
      }
      else {
        row += 0.5;
        rowB += 0.5;
      }
    }
    else col = colB = row = rowB = 0;
  }
  if (fVerbose > 1){
    std::cout << "Col: " << col << " Row: " << row << std::endl;
    std::cout << "ColB: " << colB << " RowB: " << rowB << std::endl;
  }
  double resultD[3];
  resultD[0] = col * fly;
  resultD[1] = row * flx;
  resultD[2] = 0;
  
  double resultFinal[3];
  TVector3 offset = GetSensorDimensions(pixels[0].GetDetName());
  resultD[0] -= offset.x();
  resultD[1] -= offset.y();
  //resultD[2] -= offset.z();
  
  TGeoHMatrix trans = GetTransformation(pixels[0].GetDetName());
  //std::cout << "Transformation for: " << pixels[0].GetDetName() << std::endl;
  //trans.Print("");
  trans.LocalToMaster(resultD, resultFinal);
  //result.setXYZ(resultFinal[0], resultFinal[1], resultFinal[2]);
  TVector3 dpos(flx/sqrt(12.0),fly/sqrt(12.0),0);
  TVector3 pos(resultFinal[0], resultFinal[1], resultFinal[2]);
  
  return (MvdHit(0, 0, pixels[0].GetDetName(), pos, dpos, 0, charge, count));
  
}

//CbmGeoTransform MvdHybridHitProducer::GetTransformation(std::string detName)
TGeoHMatrix MvdHybridHitProducer::GetTransformation(std::string detName)
{
  gGeoManager->cd(detName.c_str());
  TGeoHMatrix* transMat = gGeoManager->GetCurrentMatrix();
  if (fVerbose > 1)
  transMat->Print("");
  return *transMat;
}

TVector3 MvdHybridHitProducer::GetSensorDimensions(std::string detName)
{
  gGeoManager->cd(detName.c_str());
  TGeoVolume* actVolume = gGeoManager->GetCurrentVolume();
  TGeoBBox* actBox = (TGeoBBox*)(actVolume->GetShape());
  TVector3 result;
  result.SetX(actBox->GetDX());
  result.SetY(actBox->GetDY());
  result.SetZ(actBox->GetDZ());
  
  //result.Dump();
  
  return result;
}
// -------------------------------------------------------------------------


ClassImp(MvdHybridHitProducer)