//-----------------------------------------------------------
// File and Version Information:
// $Id$
//
// Description:
//      Alignment of TpcSPHits
//      
//
// Environment:
//      Software developed for the PANDA Detector at FAIR.
//
// Author List:
//      Felix Boehmer    TUM            (original author)
//
//
//-----------------------------------------------------------


#include "TpcAlignmentTask.h"

#include "TpcSPHit.h"
#include "TpcAlignmentManager.h"
#include "TClonesArray.h"
#include "FairRootManager.h"
#include "FairRunAna.h"
#include "FairRuntimeDb.h"
#include "TpcDigiPar.h"


ClassImp(TpcAlignmentTask)

TpcAlignmentTask::TpcAlignmentTask()
  : FairTask("Tpc alignment task",0),
  fRecoHitBranchName("TpcSPHit"),
  fRecoHitOutName("unset"),
  fCopy(false),
  fRecoHitArray(NULL),
  fRecoHitOutArray(NULL)
{;}


TpcAlignmentTask::~TpcAlignmentTask()
{;}


InitStatus
TpcAlignmentTask::Init()
{
  //Get ROOT Manager
  FairRootManager* ioman= FairRootManager::Instance();
  if(ioman==0){
    Error("TpcAlignmentTask::Init","RootManager not instantiated!");
    return kERROR;
  }

  // get input arrays
  fRecoHitArray=(TClonesArray*) ioman->GetObject(fRecoHitBranchName);
  if(fRecoHitArray==0)
    Fatal("TpcAlignmentTask::Init","Input array not found! Aborting");
  

  // create and register output arrays
  if(fCopy) {
    fRecoHitOutArray = new TClonesArray("TpcSPHit");
    ioman->Register(fRecoHitOutName,"tpc",fRecoHitOutArray,true);
  }

  return kSUCCESS;
}


void
TpcAlignmentTask::Exec(Option_t* opt)
{
  //std::cout<<"TpcAlignmentTask::Exec()"<<std::endl;
  // Reset output Arrays
  if(fCopy && fRecoHitOutArray==0) Fatal("TpcAlignmentTask::Exec()","No copy array");
  if(fCopy)
    fRecoHitOutArray->Delete();
  
  unsigned int nHits = fRecoHitArray->GetEntriesFast();
  for(unsigned int iHit=0; iHit<nHits; iHit++) {  //loop over recohits
    TpcSPHit* ih = (TpcSPHit*) fRecoHitArray->At(iHit);
    //copy?
    if(fCopy) {
      unsigned int nOut = fRecoHitOutArray->GetEntriesFast();
      ih = new ((*fRecoHitOutArray)[nOut]) TpcSPHit(*ih);
    }
    
    TMatrixDSym covSym(3);
    //TVectorD coord;
    covSym = ih->getRawHitCov();
    TVectorD coord = ih->getRawHitCoord();
    TVector3 pos(coord[0],coord[1],coord[2]); //why?
    
    pos = TpcAlignmentManager::getInstance()->localToMaster("tpc",pos);
    TMatrixD cov = TpcAlignmentManager::getInstance()->localToMaster("tpc",covSym);
    coord[0] = pos.X();
    coord[1] = pos.Y();
    coord[2] = pos.Z();
    //Can't be done by ROOT itself, would have to be implemented by hand
    //std::cout<<" ... asserting master covariance is still symmetric ... "<<std::endl;
    //assert(cov.IsSymmetric());
    TMatrixDSym newCov(3,cov.GetMatrixArray());
    ih->setHitCov(newCov);
    ih->setHitCoord(coord);
  } //end loop over recohits
  if(fVerbose){
    std::cout<<"TpcAlignmentTask::Exec() ... done"<<std::endl;
  }
}


void 
TpcAlignmentTask::SetParContainers() {

  std::cout<<"TpcAlignmentTask::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");

  // Get Tpc digitisation parameter container
  fPar= (TpcDigiPar*) db->getContainer("TpcDigiPar");
  if (! fPar ) Fatal("SetParContainers", "TpcDigiPar not found");
}