#include "ITrackState.hxx"

///////////////////////////////////////////////////////
ClassImp(COMET::ITrackState);

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

COMET::ITrackState::~ITrackState() {}

COMET::ITrackState::ITrackState(const COMET::ITrackState& 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::ITrackState& COMET::ITrackState::operator=(const COMET::ITrackState& 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::ICorrValues COMET::ITrackState::ProjectState(
    const COMET::IHandle<COMET::IReconState>& proj) {
    ICorrValues values(ITrackState::GetSize());
    values.SetType("EDeposit X Y Z T DX DY DZ Curvature W1 W2 Momentum ");
    const IMEDepositState* eDepositState 
        = dynamic_cast<const IMEDepositState*>(GetPointer(proj));
    int base = 0;
    if (eDepositState) {
        const int offset = eDepositState->GetEDepositIndex();
        for (int i = 0; i < IMEDepositState::GetSize(); ++i) {
            values.SetValue(i+base,
                            eDepositState->
                            GetThis().fValues.GetValue(i+offset));
            for (int j = 0; j < IMEDepositState::GetSize(); ++j) {
                values.SetCovarianceValue(
                    i+base, j+base,
                    eDepositState->
                    GetThis().fValues.GetCovarianceValue(i+offset,
                                                         j+offset));
            }
        }
    }
    const IMPositionState* posState 
        = dynamic_cast<const IMPositionState*>(GetPointer(proj));
    base += IMEDepositState::GetSize();
    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 IMCurvatureState* curvState 
        = dynamic_cast<const IMCurvatureState*>(GetPointer(proj));
    base += IMDirectionState::GetSize();
    if (curvState) {
        const int offset = curvState->GetCurvatureIndex();
        for (int i = 0; i < IMCurvatureState::GetSize(); ++i) {
            values.SetValue(i+base,
                            curvState->GetThis().fValues.GetValue(i+offset));
            for (int j = 0; j < IMCurvatureState::GetSize(); ++j) {
                values.SetCovarianceValue(
                    i+base, j+base,
                    curvState->GetThis().fValues.GetCovarianceValue(i+offset,
                                                                    j+offset));
            }
        }
    }
    const IMWidthState* widthState 
        = dynamic_cast<const IMWidthState*>(GetPointer(proj));
    base += IMCurvatureState::GetSize();
    if (widthState) {
        const int offset = widthState->GetWidthIndex();
        for (int i = 0; i < IMWidthState::GetSize(); ++i) {
            values.SetValue(i+base,
                            widthState->GetThis().fValues.GetValue(i+offset));
            for (int j = 0; j < IMWidthState::GetSize(); ++j) {
                values.SetCovarianceValue(
                    i+base, j+base,
                    widthState->GetThis().fValues.GetCovarianceValue(i+offset,
                                                                    j+offset));
            }
        }
    }
    const IMMomentumState* momState 
        = dynamic_cast<const IMMomentumState*>(GetPointer(proj));
    base += IMCurvatureState::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));
            }
        }
    }
    return values;
}