//-----------------------------------------------------------
// File and Version Information:
// $Id$
//
// Description:
//      Implementation of resCalc
//      Uses existing CDC circle fits (XY) and calculates
//      residua in XY. 
//
// Environment:
//      Software developed for the PANDA Detector at FAIR.
//
// Author List:
//      Felix Boehmer                     (original author)
//      Francesco Cusanno                 (adapted for the alignment manager)
//      Physik Department E18, TUM
//
//-----------------------------------------------------------


#include "TpcCdcFit2DResCalc.h"
#include "TClonesArray.h"
#include "TClass.h"
#include "CdcTrack.h"
#include "TpcCluster.h"
#include "TpcResidual.h"
#include "TpcRefResidualCollection.h"
#include "TpcAlignmentManager.h"

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


TpcCdcFit2DResCalc::TpcCdcFit2DResCalc()
  : fCutVal(false),
  fAlignment(false),
  fMomCut(0)
{
  fNExpectedBranches = 2;
}

TpcCdcFit2DResCalc::~TpcCdcFit2DResCalc() 
{;}

bool TpcCdcFit2DResCalc::init() {
  if(fBranchMap.size() != fNExpectedBranches) {
    std::cout<<"TpcCdcFit2DResCalc::init(): "<<std::endl
        <<"   ERROR: Wrong number of input branches ("
        <<fBranchMap.size()<<")!"<<std::endl;
    return true;
  }
  //check for proper data types:
  std::string cdcTrackRefType("CdcTrack");
  std::string tpcClusterRefType("TpcCluster");
  bool foundCdcTrack=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(cdcTrackRefType)<type.size()) {
      fCdcTrackArray=it->second;
      fCdcTrackBranch = it->first;
      foundCdcTrack=true;
      std::cout<<"TpcCdcFit2DResCalc::init(): "<<std::endl
          <<"   INFO: got "<<type<<" as Object type for CDC Tracks"
          <<std::endl;
    }
    if(type.find(tpcClusterRefType)<type.size()) {
      fTpcClusterArray=it->second;
      foundTpcCluster=true;
      std::cout<<"TpcCdcFit2DResCalc::init(): "<<std::endl
          <<"   INFO: got "<<type<<" as Object type for TPC Clusters"
          <<std::endl;
    }
  }
  if(!(foundCdcTrack&&foundTpcCluster)) {
    std::cout<<"TpcCdcFit2DResCalc::init(): "<<std::endl
        <<"   ERROR: Wrong Class Types in input branches!"<<std::endl;
    return true;
  }
  //all ok
  return false;  
}


//main functionality
int TpcCdcFit2DResCalc::calc() {
  
  //clear results
  for(unsigned int i=0; i<fResults.size(); i++) 
    delete fResults[i];
  
  fResults.clear();
  
  //get instance of alignment manager
  TpcAlignmentManager* alMan = TpcAlignmentManager::getInstance();

  unsigned int nTracks = fCdcTrackArray->GetEntriesFast();
  // //CDC trackfit loop
  for(unsigned int itr=0; itr<nTracks; itr++) {
    CdcTrack* itrack = (CdcTrack*)((*fCdcTrackArray)[itr]);
    //validity - I don't know how it's defined, but I'll use it for now...
    int val = itrack->GetValid();
    int nPoints = itrack->GetNpoint();
    if((val>0) && fCutVal) {
      std::cout<<"TpcCdcFit2DResCalc::calc(): "<<std::endl
          <<"   INFO: CDC Track invalid ("<<val<<")..."
          <<" skipping track"<<std::endl;
      continue;
    }
    if(itrack->GetMom()<fMomCut){
      continue;
    }

    //create residualCollection for this track
    fResults.push_back(new TpcRefResidualCollection(fCdcTrackBranch.Data(),itr));

    double mx  = itrack->GetMx();
    double my  = itrack->GetMy();
    double r = itrack->GetRadius();
    
    //calculate residuals in XY --------------------------
    unsigned int ncl = fTpcClusterArray->GetEntriesFast();
    for(unsigned int icl=0; icl<ncl; icl++) {
      //solve for intersection of vector to point and circle fit ...
      TpcCluster* cl = (TpcCluster*)((*fTpcClusterArray)[icl]);
      TVector3 clPos;
      if(fAlignment){
        clPos = alMan->localToMaster("tpc",cl->pos());
      }
      else {
        clPos = cl->pos();
      }
      //
      TVector3 copy = clPos;
      clPos.SetZ(0.);
      TVector3 M(mx, my, 0.);
      TVector3 diff = clPos-M;
      double dist = diff.Mag() - r;
      //fiducial volume cut
      if(fabs(dist)>fDistCut)
      	continue;
      //save residual 
      diff.SetMag(dist);
      TpcResidual* resi = new TpcResidual();
      resi->setResXYZ(diff);
      resi->setHitPos(copy);
      resi->setHitIndex(icl);
      TVector3 pP = clPos - diff; //reference position on the circle
      resi->setRefPos(pP);
      fResults.back()->addResidual(resi);
    }
    //end residual calculation  -------------------------

  }//end CDC trackfit loop
  
}