#include <cmath>

#include "IPIDState.hxx"
#include "HEPUnits.hxx"

///////////////////////////////////////////////////////
ClassImp(COMET::IPIDState);

COMET::IPIDState::IPIDState(): IMReconState(this) {
    std::copy(fLocalNames.begin(), fLocalNames.end(), 
              std::back_inserter(fFieldNames));
    Init();
}

COMET::IPIDState::IPIDState(const COMET::ITrackState& tstate): IMReconState(this) {

    std::copy(fLocalNames.begin(), fLocalNames.end(), 
              std::back_inserter(fFieldNames));


    Init();

    // retrieve the position and it's covariance. 
    for(int i = 0;i < IMPositionState::GetSize(); ++i) {
        SetValue(i+GetPositionIndex(),
                 tstate.GetValue(i+tstate.GetPositionIndex()));
        for(int j = 0;j < IMPositionState::GetSize(); ++j) {
            SetCovarianceValue(i+GetPositionIndex(),
                               j+GetPositionIndex(),
                               tstate.GetCovarianceValue(
                                   i+tstate.GetPositionIndex(), 
                                   j+tstate.GetPositionIndex()));
        }
    }  
    
    // retrieve the direction and it's covariance.
    for(int i = 0;i < IMDirectionState::GetSize();++i){
        SetValue(i+GetDirectionIndex(),
                 tstate.GetValue(i+tstate.GetDirectionIndex()));
        for(int j = 0;j < IMDirectionState::GetSize();++j){
            SetCovarianceValue(i+GetDirectionIndex(),
                               j+GetDirectionIndex(),
                               tstate.GetCovarianceValue(
                                   i+tstate.GetDirectionIndex(),
                                   j+tstate.GetDirectionIndex()));
            SetCovarianceValue(i+GetPositionIndex(),
                               j+GetDirectionIndex(),
                               tstate.GetCovarianceValue(
                                   i+tstate.GetPositionIndex(),
                                   j+tstate.GetDirectionIndex()));
            SetCovarianceValue(i+GetDirectionIndex(),
                               j+GetPositionIndex(),
                               tstate.GetCovarianceValue(
                                   i+tstate.GetDirectionIndex(),
                                   j+tstate.GetPositionIndex()));
        }
    }  
    
    // If curvature is available, then convert to momentum.  This is bogus
    // since it depends on a magnetic field, so the momentum is initialized
    // assuming a constant 0.2T field, but is set as a free parameter.  be
    // free (by default).
    double B=0.2;
    double factor = -0.3*B;
    double p = 0;
    if (!tstate.IsFree(tstate.GetCurvatureIndex())) {
        p=std::fabs(factor*1/tstate.GetValue(tstate.GetCurvatureIndex()));
    }
    
    SetValue(GetMomentumIndex(),p);
    SetFree(GetMomentumIndex());

    // If the curvature is available, then convert to charge.
    double q = 0;
    if (!tstate.IsFree(tstate.GetCurvatureIndex())
        && tstate.GetValue(tstate.GetCurvatureIndex())>1E-15) {
        q=-tstate.GetValue(tstate.GetCurvatureIndex())
            /std::fabs(tstate.GetValue(tstate.GetCurvatureIndex()));
    }
    SetValue(GetChargeIndex(),q);
    if (tstate.IsFree(tstate.GetCurvatureIndex()) || std::fabs(q) < 0.1) {
        SetFree(GetChargeIndex());
    }
    else if (tstate.IsFixed(tstate.GetCurvatureIndex())) {
        SetFixed(GetChargeIndex());
    }
    else {
        double cv = tstate.GetCovarianceValue(tstate.GetCurvatureIndex(),
                                              tstate.GetCurvatureIndex());
        SetCovarianceValue(
            GetChargeIndex(),GetChargeIndex(),
            std::fabs(cv/tstate.GetValue(tstate.GetCurvatureIndex())));
    }

}


COMET::IPIDState::IPIDState(const COMET::IShowerState& tstate): IMReconState(this) {

    std::copy(fLocalNames.begin(), fLocalNames.end(), 
              std::back_inserter(fFieldNames));

    Init();

    // retrieve the position and it's covariance. 
    for(int i = 0;i < IMPositionState::GetSize(); ++i) {
        SetValue(i+GetPositionIndex(),
                 tstate.GetValue(i+tstate.GetPositionIndex()));
        for(int j = 0;j < IMPositionState::GetSize(); ++j) {
            SetCovarianceValue(i+GetPositionIndex(),
                               j+GetPositionIndex(),
                               tstate.GetCovarianceValue(
                                   i+tstate.GetPositionIndex(), 
                                   j+tstate.GetPositionIndex()));
        }
    }  
    
    // retrieve the direction and it's covariance.
    for(int i = 0;i < IMDirectionState::GetSize();++i){
        SetValue(i+GetDirectionIndex(),
                 tstate.GetValue(i+tstate.GetDirectionIndex()));
        for(int j = 0;j < IMDirectionState::GetSize();++j){
            SetCovarianceValue(i+GetDirectionIndex(),
                               j+GetDirectionIndex(),
                               tstate.GetCovarianceValue(
                                   i+tstate.GetDirectionIndex(),
                                   j+tstate.GetDirectionIndex()));
            SetCovarianceValue(i+GetPositionIndex(),
                               j+GetDirectionIndex(),
                               tstate.GetCovarianceValue(
                                   i+tstate.GetPositionIndex(),
                                   j+tstate.GetDirectionIndex()));
            SetCovarianceValue(i+GetDirectionIndex(),
                               j+GetPositionIndex(),
                               tstate.GetCovarianceValue(
                                   i+tstate.GetDirectionIndex(),
                                   j+tstate.GetPositionIndex()));
        }
    }  
    
    // Set momentum and charge
    double p = tstate.GetEDeposit(); // Use the deposited energy (bogus)
    double q = 0;                    // Don't have any curvature.
    
    SetValue(GetMomentumIndex(),p);
    SetFree(GetMomentumIndex());

    SetValue(GetChargeIndex(),q);
    SetFree(GetChargeIndex());
}

COMET::IPIDState::IPIDState(const COMET::IPIDState& init)
  : IMReconState(this) {
    std::copy(fLocalNames.begin(), fLocalNames.end(), 
              std::back_inserter(fFieldNames));
    Init();

    for (int i=0; i<GetSize(); ++i) {
        SetValue(i,init.GetValue(i));
    }

    for (int i=0; i<GetSize(); ++i) {
        for (int j=0; j<GetSize(); ++j) {
            SetCovarianceValue(i,j,init.GetCovarianceValue(i,j));
        }
    }
    
}

COMET::IPIDState& COMET::IPIDState::operator=(const COMET::IPIDState& rhs) {
    if (this == &rhs) return *this;

    for (int i=0; i<GetSize(); ++i) {
        SetValue(i,rhs.GetValue(i));
    }

    for (int i=0; i<GetSize(); ++i) {
        for (int j=0; j<GetSize(); ++j) {
            SetCovarianceValue(i,j,rhs.GetCovarianceValue(i,j));
        }
    }
    
    return *this;
}

COMET::IPIDState::~IPIDState() {}

COMET::ICorrValues COMET::IPIDState::ProjectState(
    const COMET::IHandle<COMET::IReconState>& proj) {
    ICorrValues values(IPIDState::GetSize());
    values.SetType("X Y Z T DX DY DZ Momentum Charge");
    const IMPositionState* posState 
        = dynamic_cast<const IMPositionState*>(GetPointer(proj));
    int base = 0;
    if (posState) {
        const int offset = posState->GetPositionIndex();
        for (int i = 0; i < IMPositionState::GetSize(); ++i) {
            values.SetValue(i+base, 
                            posState->GetThis().fValues.GetValue(i+offset));
            for (int j = 0; j < IMPositionState::GetSize(); ++j) {
                values.SetCovarianceValue(
                    i+base, j+base,
                    posState->GetThis().fValues.GetCovarianceValue(i+offset,
                                                                   j+offset));
            }
        }
    }
    const IMDirectionState* dirState 
        = dynamic_cast<const IMDirectionState*>(GetPointer(proj));
    base += IMPositionState::GetSize();
    if (dirState) {
        const int offset = dirState->GetDirectionIndex();
        for (int i = 0; i < IMDirectionState::GetSize(); ++i) {
            values.SetValue(i+base, 
                            dirState->GetThis().fValues.GetValue(i+offset));
            for (int j = 0; j < IMDirectionState::GetSize(); ++j) {
                values.SetCovarianceValue(
                    i+base, j+base,
                    dirState->GetThis().fValues.GetCovarianceValue(i+offset,
                                                                   j+offset));
            }
        }
    }
    const IMMomentumState* momState 
        = dynamic_cast<const IMMomentumState*>(GetPointer(proj));
    base += IMDirectionState::GetSize();
    if (momState) {
        const int offset = momState->GetMomentumIndex();
        for (int i = 0; i < IMMomentumState::GetSize(); ++i) {
            values.SetValue(i+base,
                            momState->GetThis().fValues.GetValue(i+offset));
            for (int j = 0; j < IMMomentumState::GetSize(); ++j) {
                values.SetCovarianceValue(
                    i+base, j+base,
                    momState->GetThis().fValues.GetCovarianceValue(i+offset,
                                                                    j+offset));
            }
        }
    }
    const IMChargeState* chgState 
        = dynamic_cast<const IMChargeState*>(GetPointer(proj));
    base += IMMomentumState::GetSize();
    if (chgState) {
        const int offset = chgState->GetChargeIndex();
        for (int i = 0; i < IMChargeState::GetSize(); ++i) {
            values.SetValue(i+base,
                            chgState->GetThis().fValues.GetValue(i+offset));
            for (int j = 0; j < IMChargeState::GetSize(); ++j) {
                values.SetCovarianceValue(
                    i+base, j+base,
                    chgState->GetThis().fValues.GetCovarianceValue(i+offset,
                                                                    j+offset));
            }
        }
    }
    return values;
}