//-----------------------------------------------------------
// File and Version Information:
// $Id$
//
// Description:
//      TPC Helitron/Plawa track fitting routine
//
//
// Environment:
//      Processing of data from FOPI experiment S339
//
// Author List:
//      Paul Buehler                        SMI/OEAW
//
//-----------------------------------------------------------



// This Class' Header ------------------
#include "TpcHelTrackFittingTask.h"

// C++ headers

// Collaborating Class Headers --------
#include "FairRun.h"
#include "FairRunAna.h"
#include "FairRuntimeDb.h"

#include "TVectorD.h"
#include "TH2.h"
#include "TCanvas.h"
#include "TGraph.h"
#include "TMultiGraph.h"
#include "TLatex.h"
#include "TLegend.h"

#include "RKTrackRep.h"
#include "GFTrack.h"
#include "GFTrackCand.h"
#include "RecoHits/GFAbsRecoHit.h"
#include "PseudoSpacePoint.h"
#include "GFKalman.h"

#include "FOPIField.h"
#include "FopiEvent.h"
#include "CdcHit.h" 
#include "HeliHit.h" 
#include "HeliTrack.h" 
#include "PlawaTrack.h"
#include "FopiForwardHit.h"

#include "GFException.h"

#define DEBUG 0


//-----------------------------------------------------------
TpcHelTrackFittingTask::TpcHelTrackFittingTask()
  : fFopiEvBranchName("FopiEvent"),
    fTpcHelMatchParsBranchName("TpcHelMatchPars"),
    fTpcHelTrackBranchName("TpcHelPreFit"),
    fTpcHelTrackFitBranchName("TpcHelPostFit"),
    fPersistence(kTRUE),
    fVerbose(kFALSE),
    frep(0)
{	

}

//-----------------------------------------------------------
TpcHelTrackFittingTask::~TpcHelTrackFittingTask(){
}


//-----------------------------------------------------------
InitStatus
TpcHelTrackFittingTask::Init() {

  //Get ROOT Manager ------------------
  FairRootManager* ioman= FairRootManager::Instance();
  if(ioman==0)
    {
      Error("TpcHelTrackFittingTask::Init","RootManager not instantiated!");
      return kERROR;
    }
  
  // Get input ------------------------
  fFopiEventArray=(TClonesArray*) ioman->GetObject(fFopiEvBranchName);
  if(fFopiEventArray==0) {
    Error("TpcHelTrackFittingTask::Init","FOPI event not found!");
    return kERROR;
  } 
  fMatchingPars=(TClonesArray*) ioman->GetObject(fTpcHelMatchParsBranchName);
  if(fMatchingPars==0) {
    Error("TpcHelTrackFittingTask::Init",
      "Matching parameters array not found!!");
    return kERROR;
  }  
  fTpcHelTracks=(TClonesArray*) ioman->GetObject(fTpcHelTrackBranchName);
  if(fTpcHelTracks==0) {
    Error("TpcHelTrackFittingTask::Init","TpcHelTrack array not found!!");
    return kERROR;
  }  

  MatchingParsVec = new TVectorD(14);

  // prepare trees for output
  fTpcHelTrackFits = new TClonesArray("GFTrack");
  ioman->Register(fTpcHelTrackFitBranchName,"Tpc",fTpcHelTrackFits,fPersistence);

  return kSUCCESS;
}

//-----------------------------------------------------------
void TpcHelTrackFittingTask::SetParContainers() {

  //  std::cout<<"TpcResidualTask::SetParContainers"<<std::endl;
  std::cout.flush();

  // Get run and runtime database
  FairRun* run = FairRun::Instance();
  if ( ! run ) Fatal("SetParContainers", "No analysis run");

  FairRuntimeDb* db = run->GetRuntimeDb();
  if ( ! db ) Fatal("SetParContainers", "No runtime database");

}

//-----------------------------------------------------------
void TpcHelTrackFittingTask::Exec(Option_t* opt) {
  if (fVerbose) fprintf(stderr,"TpcHelTrackFittingTask:Exec\n");

	// Initialisations
  TVector3 pos0, mom0;
  TVector3 pos, mom;
  TMatrixDSym cov0;  
  TMatrixDSym cov;  
    
  GFDetPlane plane0 = GFDetPlane(
    TVector3(0.,0.,0.),
    TVector3(1.,0.,0.),
    TVector3(0.,1.,0.));
  GFDetPlane plane1;
  GFAbsTrackRep *rep = NULL;
  
  // clean up result buffer
  if(fTpcHelTrackFits==0) 
    Fatal("TpcHelMatchingTask::Exec()","No fTpcHelTrackFits");
  fTpcHelTrackFits->Delete();

  // get FOPI event
  FopiEvent *fopiEv = (FopiEvent*) fFopiEventArray->At(0);
  if (fopiEv == 0) return;
	
  Int_t nrun = fopiEv->GetRunNum();
	Int_t nspill = fopiEv->GetSpillNum();
	Int_t nevent = fopiEv->GetEvtNum();
  if (fVerbose) {
    fprintf(stderr,"Event: %i %i %i\n",nrun,nspill,nevent);
  }
  
  // how many tracks are there
  Int_t NumMatchPars = fMatchingPars->GetEntries();
  if (fVerbose)
    fprintf(stderr,"Number of track combinations: %i\n",NumMatchPars);
  
  // prepare fitter
  GFKalman fitter;
  fitter.setNumIterations(5);
  
   // loop over matching Pars
  Int_t ntracks = -1;
  for (Int_t ii=0; ii<NumMatchPars; ii++) {
    MatchingParsVec = (TVectorD*) fMatchingPars->At(ii);
    Double_t *MatchingPars = MatchingParsVec->GetMatrixArray();
    
    // is there are corresponding matched track?
    Int_t trkindex = (Int_t)MatchingPars[0];
    if (trkindex >= 0) {
        
      // load track
      GFTrack *trk = (GFTrack*) fTpcHelTracks->At(trkindex);
      trk->setCardinalRep(frep);
      
      // read initial conditions
      rep = trk->getCardinalRep();
      try {
      	rep->getPosMom(plane0,pos0,mom0);
      }
      catch (GFException& e){
        e.what();
      }

      // fit track
      fitter.setInitialDirection(1);
      try {
        fitter.processTrack(trk);
      }
      catch (GFException& e){
        e.what();
      }
     
      if (fVerbose) {
        
        Double_t chi2 = 0.;
        for (Int_t kk=0; kk<trk->getNumHits(); kk++) {
	  TVectorD raw(3);
	  raw = trk->getHit(kk)->getRawHitCoord();
          TVector3 hitpos(raw[0],raw[1],raw[2]);
          plane1 = GFDetPlane(
            TVector3(0.,0.,hitpos.Z()),
            TVector3(1.,0.,0.),
            TVector3(0.,1.,0.));
          try {
            trk->getPosMomCov(plane1,pos,mom,cov);
          } catch (GFException& e) {
            e.what();
          }
          fprintf(stderr,"hit[%i]: %8.3f/%8.3f %8.3f/%8.3f %8.3f/%8.3f %8.3f\n",
            kk,hitpos.X(),pos.X(),hitpos.Y(),pos.Y(),hitpos.Z(),pos.Z(),
            (hitpos-pos).Mag());
          chi2 += (hitpos-pos).Mag2();
        }
      }
      
      if (fVerbose) {
        fprintf(stderr,"\nTpcHeliTrackFit: TPCTrack: %3.0f HeliTrack: %3.0f\n",
          MatchingPars[1],MatchingPars[2]);
        Int_t nreps = trk->getNumReps();
        for (Int_t kk=0; kk<nreps; kk++) {
          rep = trk->getTrackRep(kk);
          
          fprintf(stderr,"Representation %i: %i\n",kk,rep->getStatusFlag());
          fprintf(stderr,"Chi2: %f/%f\n",rep->getChiSqu(),rep->getRedChiSqu());
          fprintf(stderr,"Charge: %f\n",rep->getCharge());
          fprintf(stderr,"Initial/Fitted momentum: %f/%f\n",
            mom0.Mag(),rep->getMom().Mag());
        }
      }

      // save fitted track on output array
      ntracks++;
      new((*fTpcHelTrackFits)[ntracks]) GFTrack(*trk);       
      
    }
  }
  
}

ClassImp(TpcHelTrackFittingTask)

//-----------------------------------------------------------