//-----------------------------------------------------------
// File and Version Information:
// $Id$
//
// Description:
//      Implementation of resCalc
//      Uses MC Points and calculates
//      residua in XYZ.
//
// Environment:
//      Software developed for the PANDA Detector at FAIR.
//
// Author List:
//      Johannes Rauch
//      Physik Department E18, TUM
//
//-----------------------------------------------------------


#include "TpcMCResCalcSPHit.h"
#include "TClonesArray.h"
#include "TClass.h"
#include "TpcPoint.h"
#include "TpcCluster.h"
#include "TpcSPHit.h"
#include "TpcResidual.h"
#include "TpcRefResidualCollection.h"
#include "McId.h"
#include "McIdCollection.h"
#include "TpcMCResCalcCluster.h"

#include <string>
#include <iostream>
#include <cmath>


bool sortHitPairs(std::pair<TpcSPHit*,TpcCluster*> p1,std::pair<TpcSPHit*,TpcCluster*> p2){
  return (p1.second->mcId().DominantID() < p2.second->mcId().DominantID());
}

ClassImp(TpcMCResCalcSPHit)

TpcMCResCalcSPHit::TpcMCResCalcSPHit() :
  //fNExpectedBranches(3),
  fskipSecondaries(false)
{

}

TpcMCResCalcSPHit::~TpcMCResCalcSPHit() {
  ;
}

bool TpcMCResCalcSPHit::init() {
  if(fBranchMap.size() != 3) {
    std::cout<<"TpcMCResCalcSPHit::init(): "<<std::endl
	     <<"   ERROR: Wrong number of input branches ("
	     <<fBranchMap.size()<<")!"<<std::endl;
    return true;
  }
  //check for proper data types:
  std::string tpcPointRefType("TpcPoint");
  std::string tpcClusterRefType("TpcCluster");
  std::string tpcSPHitRefType("TpcSPHit");
  bool foundTpcPoint=false;
  bool foundTpcCluster=false;
  bool foundSPHit=false;

  //identify the needed branches in the map
  std::map<TString, TClonesArray*>::const_iterator it;
  for(it=fBranchMap.begin(); it!=fBranchMap.end(); it++) {
    std::string type((it->second)->GetClass()->GetName());
    if(type.find(tpcPointRefType)<type.size()) {
      fTpcPointArray=it->second;
      fTpcPointBranch = it->first;
      foundTpcPoint=true;
      std::cout<<"TpcMCResCalcSPHit::init(): "<<std::endl
	       <<"   INFO: got "<<type<<" as Object type for Tpc Points"
	       <<std::endl;
    }
    if(type.find(tpcClusterRefType)<type.size()) {
      fTpcClusterArray=it->second;
      foundTpcCluster=true;
      std::cout<<"TpcMCResCalcSPHit::init(): "<<std::endl
	       <<"   INFO: got "<<type<<" as Object type for TPC Clusters"
	       <<std::endl;
    }
    if(type.find(tpcSPHitRefType)<type.size()) {
      fSPHitArray=it->second;
      foundSPHit=true;
      std::cout<<"TpcMCResCalcSPHit::init(): "<<std::endl
         <<"   INFO: got "<<type<<" as Object type for TPC SPHit"
         <<std::endl;
    }
  }
  if(!(foundTpcPoint&&foundTpcCluster&&foundSPHit)) {
    std::cout<<"TpcMCResCalcSPHit::init(): "<<std::endl
	     <<"   ERROR: Wrong Class Types in input branches!"<<std::endl;
    return true;
  }
  //all ok
  return false;  
}


//main functionality
int TpcMCResCalcSPHit::calc() {
  
  //clear results
  for(unsigned int i=0; i<fResults.size(); i++) 
    delete fResults[i];
  
  fResults.clear();
  

  // create, fill and sort vectors
  std::vector<TpcPoint*> points;
  unsigned int nPoints = fTpcPointArray->GetEntriesFast();
  for(unsigned int i=0; i<nPoints; i++) {
    points.push_back((TpcPoint*)((*fTpcPointArray)[i]));
  }
  std::sort(points.begin(), points.end(), sortPoints);


  std::vector< std::pair<TpcSPHit*,TpcCluster*> > recoPoints;
  unsigned int nClusters = fTpcClusterArray->GetEntriesFast();
  unsigned int nHits = fSPHitArray->GetEntriesFast();
  for(unsigned int i=0; i<nHits; i++) {
    std::pair<TpcSPHit*,TpcCluster*> hitCl;
    hitCl.first = (TpcSPHit*)(fSPHitArray->At(i));
    hitCl.second = (TpcCluster*)fTpcClusterArray->At(hitCl.first->getClusterID());
    recoPoints.push_back(hitCl);
    //clusters.push_back((TpcCluster*)((*fTpcClusterArray)[i]));
  }
  std::sort(recoPoints.begin(), recoPoints.end(), sortHitPairs);
  std::vector<TpcCluster*> clusters;

  // loop over clusters
  McId currentTrackID, currentClusterID;
  TVector3 clPos, ptPos, ptDir, POCA, res3D;
  unsigned int itr(0); // track index

  for (unsigned int iHit=0; iHit<nHits; ++iHit){
    std::pair<TpcSPHit*,TpcCluster*>  hitPair= recoPoints[iHit];
    currentClusterID = hitPair.second->mcId().DominantID();

    if (iHit==0 || currentClusterID != currentTrackID){
      currentTrackID = currentClusterID;
      fResults.push_back(new TpcRefResidualCollection(fTpcPointBranch.Data(), currentTrackID.mctrackID()));
    }

    if (fskipSecondaries && currentClusterID.mcsecID() != 0){
      continue;
    }
    TVectorD hitCoord(3);
    hitCoord = ((GFAbsRecoHit*) hitPair.first)->getRawHitCoord();
    
    clPos.SetXYZ(hitCoord[0],hitCoord[1],hitCoord[2]);


    double mindist(1.e99);
    int minId(-1);

    // look for the MC Point with minimum distance to the cluster
    for (unsigned int iPt=0; iPt<nPoints; ++iPt){
      TpcPoint* pt = points[iPt];
      McId idPt(pt->GetEventID(), pt->GetTrackID(), pt->GetSecID());
      if (idPt != currentTrackID) continue;

      pt->Position(ptPos);
      double dist = (clPos - ptPos).Mag();

      if (dist < mindist){
        mindist = dist;
        minId = iPt;
      }
    }

    // calc Residuum
    if (minId==-1) continue;
    points[minId]->Position(ptPos);
    points[minId]->Momentum(ptDir);
    ptDir.SetMag(1.);

    // local straight line approx. of the track, calc POCA on line
    POCA = ptPos + ptDir * (ptDir * (clPos - ptPos));

    res3D = clPos - POCA;

    TpcResidual* resi = new TpcResidual();
    resi->setResXYZ(res3D);
    resi->setRefPos(POCA);
    resi->setHitPos(clPos);
    resi->setHitIndex(hitPair.second->index());

    fResults.back()->addResidual(resi);

  } // end loop over clusters

  return 0;

}