//-----------------------------------------------------------
// 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 "TpcMCResCalcCluster.h"
#include "TClonesArray.h"
#include "TClass.h"
#include "TpcPoint.h"
#include "TpcCluster.h"
#include "TpcResidual.h"
#include "TpcRefResidualCollection.h"
#include "McId.h"
#include "McIdCollection.h"


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


bool sortPoints(TpcPoint* p1, TpcPoint* p2){
  // build temporary McIds and use McId comparison operator
  McId id1(p1->GetEventID(), p1->GetTrackID(), p1->GetSecID());
  McId id2(p2->GetEventID(), p2->GetTrackID(), p2->GetSecID());
  return (id1 < id2);
}

bool sortClusters(TpcCluster* c1, TpcCluster* c2){
  // use McId comparison operator
  return (c1->mcId().DominantID() < c1->mcId().DominantID());
}




TpcMCResCalcCluster::TpcMCResCalcCluster() :
  //fNExpectedBranches(2),
  fskipSecondaries(false)
{

}

TpcMCResCalcCluster::~TpcMCResCalcCluster() {
  ;
}

bool TpcMCResCalcCluster::init() {
  if(fBranchMap.size() != 2) {
    std::cout<<"TpcMCResCalcCluster::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");
  bool foundTpcPoint=false;
  bool foundTpcCluster=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<<"TpcMCResCalcCluster::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<<"TpcMCResCalcCluster::init(): "<<std::endl
	       <<"   INFO: got "<<type<<" as Object type for TPC Clusters"
	       <<std::endl;
    }
  }
  if(!(foundTpcPoint&&foundTpcCluster)) {
    std::cout<<"TpcMCResCalcCluster::init(): "<<std::endl
	     <<"   ERROR: Wrong Class Types in input branches!"<<std::endl;
    return true;
  }
  //all ok
  return false;  
}


//main functionality
int TpcMCResCalcCluster::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<TpcCluster*> clusters;
  unsigned int nClusters = fTpcClusterArray->GetEntriesFast();
  for(unsigned int i=0; i<nClusters; i++) {
    clusters.push_back((TpcCluster*)((*fTpcClusterArray)[i]));
  }
  std::sort(clusters.begin(), clusters.end(), sortClusters);


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

  for (unsigned int iCl=0; iCl<nClusters; ++iCl){
    TpcCluster* cl = clusters[iCl];
    currentClusterID = cl->mcId().DominantID();

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

    if (fskipSecondaries && currentClusterID.mcsecID() != 0){
      continue;
    }

    clPos = cl->pos();


    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(cl->index());

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

  } // end loop over clusters

  return 0;

}
ClassImp(TpcMCResCalcCluster)