//_____________________________________________________________________________
///
/// \class FitStateIterating
///
/// FitStateIterating is the main state of the fitter - repeating
/// iterations with a fixed number of planes until the converges or 
/// diverges.
///
/// \author Sergei avva@fnal.gov
///

#include <string>
#include <cmath>

#include "Algorithm/AlgConfig.h"
#include "MessageService/MsgService.h"

#include "CandFitTrackSA/ConstFT.h"
#include "CandFitTrackSA/DataFT.h"
#include "CandFitTrackSA/MatrixCalculator.h"

#include "CandFitTrackSA/FitStateFactory.h"
#include "CandFitTrackSA/FitStateIterating.h"
#include "CandFitTrackSA/FitContext.h"

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

using namespace ConstFT;

// The ID of class Line
static const std::string ITERATING_FIT_STATE = "Iterating";

// Create an anonymous namespace
// to make the function invisible from other modules
namespace {

FitState* CreateIteratingFS() { return new FitStateIterating; }

// register block
bool registered = FitStateFactory::Instance().RegisterFitState(
                                                    ITERATING_FIT_STATE,
                                                            CreateIteratingFS);
}  // namespace


///
/// Name() - return name of the state 
///
const std::string& FitStateIterating::Name() const
{
    return ITERATING_FIT_STATE;
}    

///
/// Iterate method
///
void FitStateIterating::Iterate(FitContext& context) const
{
    context.fNIterationsStep++;
    context.fNIterationsTotal++;
    
    // check if max number of iterations reached 
    if ( context.fNIterationsStep > context.fNMaxIterations ) {
        // max number of iterations -> switch to Diverged state
        context.fConvergenceMaster.SetDiverged();
        context.SetState(FitStateFactory::Instance().GetFitState("Diverged"));
        context.Print();
        MSG("FitTrackSA",Msg::kInfo) << "Exceeded max # of iterations.\n";
        MSG("FitTrackSA",Msg::kInfo) << "Switched from Iterating to Diverged\n";
        return;
    }

    // protect from bad q/p values
    // should be made configurable
    if ( fabs(context.fCurrentFit(kQoverP))>4. ) {
        MSG("FitTrackSA",Msg::kInfo) << "Bad value of q/p = " 
            << context.fCurrentFit(kQoverP) << "\n";
        context.fCurrentFit(kQoverP) = 
            4.*TMath::Sign(1., context.fCurrentFit(kQoverP));
    }
        
    context.fNPlanesFit = context.fData.SetInitial(context.fCurrentFit, 
                context.fConvergenceMaster.GetNPlanesCur(), *context.fSwimmer);
    
    if ( context.fNPlanesFit < context.fConvergenceMaster.GetNPlanesMin() ) {
        context.fConvergenceMaster.SetDiverged();
        context.SetState(FitStateFactory::Instance().GetFitState("Diverged"));
        context.Print();
        MSG("FitTrackSA",Msg::kInfo) << "fNPlanesFit is smaller than fMinPlanes.\n";
        MSG("FitTrackSA",Msg::kInfo) << "Switched from Iterating to Diverged\n";
        return;        
    }
        
    //    
    context.fMatCalc.Solve(context.fData);
    context.fCurrentFit = context.fMatCalc.GetTrackOut();        
    Double_t dchi2 = context.fMatCalc.GetDChi2();

    //MSG("FitTrackSA",Msg::kInfo) << "new dchi2=" << dchi2 << "\n";

    if ( dchi2 < context.fConvergenceCond &&
            context.fNPlanesFit == context.fConvergenceMaster.GetNPlanesCur() ) {
        // excellent - fit for NPlanesToFit converged!
        context.fDChi2 = dchi2;
        // switching to Converged state
        context.fConvergenceMaster.SetConverged();
        context.SetState(FitStateFactory::Instance().
                                                GetFitState("Converged"));
        context.Print();
        MSG("FitTrackSA",Msg::kInfo) << "Switched from Iterating to Converged\n";
        return;
    }
    
    if ( dchi2 < context.fConvergenceCond &&
            context.fNPlanesFit != context.fConvergenceMaster.GetNPlanesCur() ) {
            //context.fNPlanesFit != context.fNPlanesToFit ) {
        // fit converged, but not to requested number of planes
        // switching to ConvergedPartial state
        context.fDChi2 = dchi2;
        context.fConvergenceMaster.SetDiverged();
        context.SetState(FitStateFactory::Instance().
                                            GetFitState("Diverged"));
        MSG("FitTrackSA",Msg::kInfo) << "#Planes fit != #Planes requested\n";
        MSG("FitTrackSA",Msg::kInfo) << "Switched from Iterating to Diverged\n";
        context.Print();
        return;
    }
    
    // check if the iterations are converging
    if ( dchi2 < context.fDChi2 ) {
        // good - fit is converging
        context.fDChi2 = dchi2;
        context.Print();
        return;
    } else {
        // fit is not converging - bump count
        context.fNTriesDiverges++;
        context.fDChi2 = dchi2;
        
        if ( context.fNTriesDiverges > context.fNMaxDiverging ) {
            // reached maximal count of diverging steps -
            // switch to Diverged state
            context.fConvergenceMaster.SetDiverged();
            context.SetState(FitStateFactory::Instance().
                                                    GetFitState("Diverged"));
            context.Print();
            MSG("FitTrackSA",Msg::kInfo) << "Delta(Chi2) is not decreasing\n";
            MSG("FitTrackSA",Msg::kInfo) << "Switching from Iterating to Diverged\n";
            return;
        }
        return;
    }        
    
    assert(kFALSE && "Should never get here!!");
}