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


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


bool sortSamples(TpcSample* s1, TpcSample* s2){
  // use McId comparison operator
  return (s1->mcId().DominantID() < s2->mcId().DominantID());
}


ClassImp(TpcMCResCalcSample)

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

}

TpcMCResCalcSample::~TpcMCResCalcSample() {
  ;
}

bool TpcMCResCalcSample::init() {
  if(fBranchMap.size() != 2) {
    std::cout<<"TpcMCResCalcSample::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 tpcSampleRefType("TpcSample");
  bool foundTpcPoint=false;
  bool foundTpcSample=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<<"TpcMCResCalcSample::init(): "<<std::endl
	       <<"   INFO: got "<<type<<" as Object type for Tpc Points"
	       <<std::endl;
    }
    if(type.find(tpcSampleRefType)<type.size()) {
      fTpcSampleArray=it->second;
      foundTpcSample=true;
      std::cout<<"TpcMCResCalcSample::init(): "<<std::endl
	       <<"   INFO: got "<<type<<" as Object type for TPC Samples"
	       <<std::endl;
    }
  }
  if(!(foundTpcPoint&&foundTpcSample)) {
    std::cout<<"TpcMCResCalcSample::init(): "<<std::endl
	     <<"   ERROR: Wrong Class Types in input branches!"<<std::endl;
    return true;
  }
  //all ok
  return false;  
}


//main functionality
int TpcMCResCalcSample::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<TpcSample*> samples;
  unsigned int nsamples = fTpcSampleArray->GetEntriesFast();
  for(unsigned int i=0; i<nsamples; i++) {
    samples.push_back((TpcSample*)((*fTpcSampleArray)[i]));
  }
  std::sort(samples.begin(), samples.end(), sortSamples);


  // loop over samples
  McId currentTrackID, currentSampleID;
  TVector3 dPos, ptPos, ptDir, POCA, res3D;
  unsigned int itr(0); // track index

  for (unsigned int iDigi=0; iDigi<nsamples; ++iDigi){
    TpcSample* s = samples[iDigi];
    currentSampleID = s->mcId().DominantID();

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

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

    TpcDigiMapper::getInstance()->map(s, dPos);


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

    // look for the MC Point with minimum distance to the digi
    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 = (dPos - 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 * (dPos - ptPos));

    res3D = dPos - POCA;

    TpcResidual* resi = new TpcResidual();
    resi->setResXYZ(res3D);
    resi->setRefPos(POCA);
    resi->setHitPos(dPos);
    resi->setHitIndex(s->index());

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

  } // end loop over samples

  return 0;

}