//_____________________________________________________________________________
///
/// \class FitContext
///
/// FitContext is the Context part of the State design pattern - 
/// it keeps all the info about current status of the fit.
///
/// \author Sergei avva@fnal.gov
///

#include "MessageService/MsgService.h"

#include "CandFitTrackSA/DataFT.h"
#include "CandFitTrackSA/FitContext.h"
#include "CandFitTrackSA/FitState.h"
#include "CandFitTrackSA/FitStateFactory.h"
#include "CandFitTrackSA/Ntp/NtpFitSA.h"
#include "CandFitTrackSA/TracerSA.h"
#include "CandFitTrackSA/TrackEstimatorRange.h"

#include <cmath>

CVSID("$Id: FitContext.cxx,v 1.4 2006/12/01 18:40:45 rhatcher Exp $");

using namespace ConstFT;

///
/// ctor
///
FitContext::FitContext(const AlgConfig& ac, const TrackContext& context,
                         TrackEstimator* estimator, SwimSwimmer* swimmer ) :
    fTrackContext(context), fState(0), 
    fSwimmer(swimmer), fEstimator(estimator),
    fData(ac, context), 
    fConvergenceMaster(), 
    fMatCalc(ac, context),
    fNIterationsStep(0), fNIterationsTotal(0),
    // fNPlanesToFit(0), 
    fNPlanesFit(0),
    fCurrentFit(ConstFT::NTrackParams), fTimesConverged(0), fLastGoodFit(),
    fDChi2(ConstFT::InitialDChi2), fNTriesDiverges(0), fNHitsInViewMin(4),
    fNMaxIterations(100), fNMaxDiverging(1), fConvergenceCond(0.1)
{
    TracerSA trace(
    "FitContext::FitContext(const AlgConfig&,const TrackContext&,SwimSwimmer*)"
    );
    
    Config(ac);
    
    fState = FitStateFactory::Instance().GetFitState(std::string("Initial"));
    
    fConvergenceMaster.SetNHitsInViewMin(fNHitsInViewMin);    
    fConvergenceMaster.BuildMasks(fData);    
}

    
///
/// dtor
///
FitContext::~FitContext()
{
    TracerSA trace("FitContext::~FitContext()");
}


///
/// read configuration parameters from AlgConfig
///
void FitContext::Config(const AlgConfig& ac)
{
    TracerSA trace("FitContext::Config(const AlgConfig&)");
    
    if ( ac.KeyExists("NHitsInViewMin") ) {
        fNHitsInViewMin = ac.GetInt("NHitsInViewMin");
    }    
    if ( ac.KeyExists("NMaxIterations") ) {
        fNMaxIterations = ac.GetInt("NMaxIterations");
    }
    if ( ac.KeyExists("NMaxDiverging") ) {
        fNMaxDiverging = ac.GetInt("NMaxDiverging");
    }
    if ( ac.KeyExists("ConvergenceCond") ) {
        fConvergenceCond = ac.GetDouble("ConvergenceCond");
    }        
}


///
/// do iterations - call Iterate() method of the current FitState
///
void FitContext::Iterate()
{
    TracerSA trace("FitContext::Iterate()");
    fState->Iterate(*this);    
}


///
/// switch to given FitState
///
void FitContext::SetState(const FitState* state)
{
    TracerSA trace("FitContext::SetState(const FitState*)");
    fState = state;    
    MSG("FitTrackSA",Msg::kDebug) << "Switched to " 
        << state->Name() << " state.\n";
}


///
///
///
NtpFitSA    FitContext::MakeNtpFitSA() const
{
    TracerSA trace("FitContext::MakeNtpFitSA()");
    
    NtpFitSA ntp;

    // Fill
    ntp.zdir = fTrackContext.GetDir();
    FillNtpFitSA(ntp);   
    FillNtpPlaneInfo(ntp);
    FillNtpBFieldCalib(ntp);
    FillNtpFitSR(ntp);
            
    return ntp;
}


///
///
///
void    FitContext::FillNtpFitSA(NtpFitSA& ntp) const
{
    TracerSA trace("FitContext::FillNtpFitSA()");
    // 
    if ( fTimesConverged == 0 ) {    
        ntp.fit.pass     = 0;
    } else {
        ntp.fit.pass     = 1;
    }
    ntp.fit.niter   = fNIterationsTotal;
    ntp.fit.ndf     = fLastGoodFit.GetNdof();
    ntp.fit.q       = fLastGoodFit.GetQ();    
    ntp.fit.chi2    = fLastGoodFit.GetChi2();
    if ( TMath::Abs(ntp.fit.ndf) > TinyNumber ) { 
        ntp.fit.rchi2 = ntp.fit.chi2/ntp.fit.ndf;
    } else {
        ntp.fit.rchi2 = -1.;
    }
    
    ntp.fit.cpu     = fCpu;
    ntp.fit.p       = fLastGoodFit.GetP();
    ntp.fit.ep      = fLastGoodFit.GetEP();
    
    ntp.fit.u        = fLastGoodFit.GetFitParameter(kU);
    ntp.fit.eu       = fLastGoodFit.GetFitParameterError(kU);    
    ntp.fit.dudz     = fLastGoodFit.GetFitParameter(kdUdZ);
    ntp.fit.edudz    = fLastGoodFit.GetFitParameterError(kdUdZ);
    ntp.fit.v        = fLastGoodFit.GetFitParameter(kV);
    ntp.fit.ev       = fLastGoodFit.GetFitParameterError(kV);
    ntp.fit.dvdz     = fLastGoodFit.GetFitParameter(kdVdZ);
    ntp.fit.edvdz    = fLastGoodFit.GetFitParameterError(kdVdZ);
    ntp.fit.qp       = fLastGoodFit.GetFitParameter(kQoverP);
    ntp.fit.eqp      = fLastGoodFit.GetFitParameterError(kQoverP);
    
    ntp.fit.z        = fData.GetZVtx();        
        
    // fill direction cosines
    Float_t dcosu, dcosv, dcosz;    
    dcosz = fTrackContext.GetDir()*
                        pow(1. + pow(fLastGoodFit.GetTrackOut(kdUdZ),2)
                            + pow(fLastGoodFit.GetTrackOut(kdVdZ),2),-0.5);
    dcosu = fLastGoodFit.GetTrackOut(kdUdZ)*dcosz;
    dcosv = fLastGoodFit.GetTrackOut(kdVdZ)*dcosz;
    ntp.fit.dcosu    = dcosu;
    ntp.fit.edcosu   = sqrt(fabs(dcosz))*sqrt(pow(dcosu*dcosv*ntp.fit.edvdz,2)+
                            pow((pow(dcosz,2)+pow(dcosv,2))*ntp.fit.edudz,2));
    ntp.fit.dcosv    = dcosv;
    ntp.fit.edcosv   = sqrt(fabs(dcosz))*sqrt(pow(dcosu*dcosv*ntp.fit.edudz,2)+
                            pow((pow(dcosz,2)+pow(dcosu,2))*ntp.fit.edvdz,2));
    ntp.fit.dcosz    = dcosz;

    //
    for ( int i = 0; i < NTrackParams; i++ ) {
        ntp.fit.par[i] = fLastGoodFit.GetFitParameter(i);
        for ( int j = 0; j < NTrackParams; j++ ) {
            ntp.fit.parerr[i][j] = fLastGoodFit.GetFitErrM(i,j);
        }
    }
}


///
///
///
void    FitContext::FillNtpBFieldCalib(NtpFitSA& ntp) const
{
    TracerSA trace("FitContext::FillNtpBFieldCalib()");
    //ntp.bfcalib.bin   =  segment.GetBin();
    ntp.bfcalib.prange = fTrackContext.GetPrange();
    //ntp.bfcalib.pmc    = segment.GetPtrue();
    ntp.bfcalib.umean  = fData.GetUMean();
    ntp.bfcalib.vmean  = fData.GetVMean();        
}


///
///
///
void    FitContext::FillNtpFitSR(NtpFitSA& ntp) const
{
    TracerSA trace("FitContext::FillNtpFitSR()");
    ntp.fitsr.pass      = fTrackContext.GetPassSR();
    ntp.fitsr.niter     = fTrackContext.GetIterationsSR();
    ntp.fitsr.ndf       = fTrackContext.GetNdofSR();
    ntp.fitsr.q         = fTrackContext.GetEMChargeSR();
    ntp.fitsr.chi2      = fTrackContext.GetChi2SR();
    ntp.fitsr.rchi2     = fTrackContext.GetRChi2SR();
    ntp.fitsr.cpu       = fTrackContext.GetCpuSR();
    ntp.fitsr.qp        = fTrackContext.GetQPSR();
    ntp.fitsr.eqp       = fTrackContext.GetEQPSR();
    ntp.fitsr.u         = fTrackContext.GetUSR();
    ntp.fitsr.eu        = fTrackContext.GetUErrorSR();
    ntp.fitsr.v         = fTrackContext.GetVSR();
    ntp.fitsr.ev        = fTrackContext.GetVErrorSR();
    ntp.fitsr.dcosu     = fTrackContext.GetDirCosUSR();
    ntp.fitsr.edcosu    = fTrackContext.GetEDirCosUSR();
    ntp.fitsr.dcosv     = fTrackContext.GetDirCosVSR();
    ntp.fitsr.edcosv    = fTrackContext.GetEDirCosVSR();
    ntp.fitsr.dcosz     = fTrackContext.GetDirCosZSR();
    ntp.fitsr.p         = fTrackContext.GetMomentumSR();
    ntp.fitsr.prange    = fTrackContext.GetMomentumRangeSR();
}


///
///
///
void    FitContext::FillNtpPlaneInfo(NtpFitSA& ntp) const
{
    TracerSA trace("FitContext::FillNtpPlaneInfo()");
    ntp.plane.begin    = fData.GetBegPlaneId().GetPlane();
    ntp.plane.end      = fData.GetEndPlaneId().GetPlane();
    ntp.plane.n        = fData.GetNPlanes();
    ntp.plane.nu       = fData.GetNUPlanes();
    ntp.plane.nv       = fData.GetNVPlanes();
    
    ntp.hitplane.begin = fData.GetBegHit();
    ntp.hitplane.end   = fData.GetEndHit();
    ntp.hitplane.n     = fData.GetNUHits() + fData.GetNVHits();
    ntp.hitplane.nu    = fData.GetNUHits();
    ntp.hitplane.nv    = fData.GetNVHits();

    ntp.fitplane.begin = fData.GetBegHitUsed();
    ntp.fitplane.end   = fData.GetEndHitUsed();
    ntp.fitplane.n     = fData.GetNUHitsUsed() + fData.GetNVHitsUsed();
    ntp.fitplane.nu    = fData.GetNUHitsUsed();
    ntp.fitplane.nv    = fData.GetNVHitsUsed();
}

///
///
///
void FitContext::SetFitParams(const TVectorD& fit) 
{ 
    TracerSA trace("FitContext::SetFitParams(const TVectorD&)");
    fCurrentFit = fit; 
    PrintCurrentFit();
}
    
///
///
///
void FitContext::SetFromLastGoodFit() 
{ 
    TracerSA trace("FitContext::SetFromLastGoodFit()");
    fCurrentFit = fLastGoodFit.GetTrackOut(); 
}

///
/// used by track estimator to set initial track parameters
///
void FitContext::SetLastGoodFitParams(const TVectorD& fit) 
{ 
    TracerSA trace("FitContext::SetLastGoodFitParams(const TVectorD&)");
    fLastGoodFit.SetTrackOut(fit); 
}

///
/// print out FitContext for debugging
///
void FitContext::PrintCurrentFit()
{  
    TracerSA trace("FitContext::PrintCurrentFit()");
     MsgStream *mftsa = &MSGSTREAM("FitTrackSA", Msg::kInfo);

     (*mftsa) << "U=" << fCurrentFit(kU) << "; dU/dZ=" << fCurrentFit(kdUdZ) 
        << "; V=" << fCurrentFit(kV) << "; dV/dZ=" << fCurrentFit(kdVdZ) 
        << "; Q/P=" << fCurrentFit(kQoverP) <<  "\n";
}

///
/// print out FitContext for debugging
///
void FitContext::Print()
{        
    TracerSA trace("FitContext::Print()");
     MsgStream *mftsa = &MSGSTREAM("FitTrackSA", Msg::kInfo);

     (*mftsa) << "Step " << fNIterationsStep << "/" << fNIterationsTotal 
        //<< "; #ToFit=" << fNPlanesToFit
        << "; #ToFit=" << fConvergenceMaster.GetNPlanesCur()
        << "; #Fit=" << fNPlanesFit << "; dchi2=" << fDChi2 
        << "; q/p=" << fCurrentFit(kQoverP) <<  "\n";

}