// from ROOT
#include "TRotation.h"
#include "TVector3.h"

// form kalman
#include "hkaltracksite.h"
#include "hkaltrackstate.h"

#include <iostream>
using namespace std;
#include <cstdlib>

ClassImp(HKalTrackSite)

//_HADES_CLASS_DESCRIPTION
///////////////////////////////////////////////////////////////////////////////
//
// Class: HKalTrackSite
//
// A track site stores information about a measurement hit and parameters of
// the Kalman filter track states (prediction, filtering, smoothing, inverse filtering)
// for this hit.
// The hit object for a site may be replaced with the setHit(const HKalMdcHit &newhit)
// function.
// The site provides read/write access to the state vectors and the matrices for all
// track states via the setState* functions.
// The dimension of the measurement vector and track state vector must be defined
// when building the object and may not be changed later on.
//
//-----------------------------------------------------------------------
///////////////////////////////////////////////////////////////////////////////


//  -----------------------------------
//  Ctors and Dtor
//  -----------------------------------

HKalTrackSite::HKalTrackSite(Int_t measDim, Int_t stateDim) : TObject() {
    // Creates a dummy hit and track states.
    // measDim:  dimension of measurement vector.
    // stateDim: dimension of the track state vectors.

    trackStates = new HKalTrackState*[kNumKalStates];
    for(Int_t stateType = 0; stateType < kNumKalStates; stateType++) {
        trackStates[stateType] = new HKalTrackState((kalFilterTypes)stateType, measDim, stateDim);
    }
    hits = new TObjArray();
    hits->Add(new HKalMdcHit(measDim)); // add a dummy hit to site
}

HKalTrackSite::~HKalTrackSite() {
    delete hits;
    delete [] trackStates;
}

//  -----------------------------------
//  Implementation of private methods
//  -----------------------------------

HKalMdcHit* HKalTrackSite::getHit(Int_t idx) {
    // Get hit at index idx that this site contains.
    if(idx >= hits->GetEntries() || idx < 0 || hits->GetEntries() == 0) {
        Error("getHit()", Form("No hit at index %i. Only %i hits stored. Returning first hit object.", idx, hits->GetEntries()));
        return (HKalMdcHit*)hits->At(0);
    } else {
        return (HKalMdcHit*)hits->At(idx);
    }
}

void HKalTrackSite::transformHit(const TRotation *transMat) {
    // Transform the measurement hit coordinates and layer vectors
    // using the rotation matrix transMat.

    for(Int_t i = 0; i < hits->GetLast() + 1; i++) {
        getHit(i)->transformHit(transMat);
    }
    getHit()->transformLayer(transMat);
}

void HKalTrackSite::transformStates(const TRotation *transMat) {
    // Transform the track state vectors using the rotation matrix transMat.
    // Since information about the measurement layer is needed, make sure
    // the hit has not been transformed yet.

    for(Int_t stateType = 0; stateType < kNumKalStates; stateType++) {
        trackStates[stateType]->transform(transMat, getHit(0)->getMeasLayer());
    }
}

//  -----------------------------------
//  Implementation of public methods
//  -----------------------------------

void HKalTrackSite::addHit(HKalMdcHit *newhit) {
    // Add a new hit to this site.
    hits->Add(newhit);
}

void HKalTrackSite::calcMeasVecFromState(TVectorD &measVec, kalFilterTypes stateType) const {
    // Projects the track parameters of a track state at this site
    // on a hit coordinate vector.
    // measVec:   returns the hit coordinate vector
    // stateType: track state type (predicted, filtered, smoothed, invFiltered)

    trackStates[stateType]->calcMeasVec(measVec);
}

void HKalTrackSite::clearHits() {
    hits->Clear();
}

void HKalTrackSite::sortHits() {
    hits->Sort();
}

HKalMdcHit const& HKalTrackSite::getHit(Int_t idx) const {
    // Get hit at index idx that this site contains.
    if(idx >= hits->GetEntries() || idx < 0 || hits->GetEntries() == 0) {
        Error("getHit()", Form("No hit at index %i. Only %i hits stored. Returning first hit object.", idx, hits->GetEntries()));
        return (const HKalMdcHit&) (* (HKalMdcHit*)hits->At(0));
    } else {
        return (const HKalMdcHit&) (* (HKalMdcHit*)hits->At(idx));
    }
}

Double_t HKalTrackSite::getHitsTotalWeight() {
    // Returns the sum of the weights from all hits in this site.
    Double_t totalWeight = 0.;
    for(Int_t i = 0; i < getNcompetitors(); i++) {
        totalWeight += getHit(i)->getWeight();
    }
    return totalWeight;
}

void HKalTrackSite::getPosAndDirFromState(TVector3 &pos, TVector3 &dir, kalFilterTypes stateType) const {
    // Projects the track parameters of a track state at this site
    // on a hit coordinate and a direction vector.
    // pos:       returns the hit coordinate vector
    // dir:       returns the track direction vector
    // stateType: track state type (predicted, filtered, smoothed, invFiltered)

    HKalPlane plane = getHit().getMeasLayer();
    trackStates[stateType]->calcPosAndDirAtPlane(pos, dir, plane);
}

void HKalTrackSite::print(const Option_t *opt) const {
    // Prints information about the hit and track states for this site.
    // opt: Determines what information about the object will be printed.
    // If opt contains:
    // "Hit": print information about hit's measurement coordinates and layer
    // "Pred": print state vector and used matrices of the predicted state
    // "Filt": print state vector and used matrices of the filtered state
    // "Smoo": print state vector and used matrices of the smoothed state
    // "Inv":  print state vector and used matrices of the inverse filtered state
    // If opt is empty all of the above will be printed.
    // Only the state vector and covariance matrix are printed for all states.
    // The content of unused matrices will not be printed:
    // The propagator and process noise matrices are always stored in the filtered state.
    // The projector matrix is stored in class HKalSystem.
    // idx: Index of hit object in site.

    cout<<"**** Print content of site ****"<<endl;
    TString stropt(opt);
    if(stropt.Contains("Hit", TString::kIgnoreCase)  || stropt.IsNull()) {
        for(Int_t i = 0; i < getNcompetitors(); i++) {
            cout<<"Printing hit number "<<i<<" in site."<<endl;
            getHit(i).print("Hit");
        }
    }
    if(stropt.Contains("Pred", TString::kIgnoreCase) || stropt.IsNull()) {
        trackStates[kPredicted]  ->print("SFCQ");
    }
    if(stropt.Contains("Filt", TString::kIgnoreCase) || stropt.IsNull()) {
        trackStates[kFiltered]   ->print("SC");
    }
    if(stropt.Contains("Smoo", TString::kIgnoreCase) || stropt.IsNull()) {
        trackStates[kSmoothed]   ->print("SC");
    }
    if(stropt.Contains("Inv", TString::kIgnoreCase)  || stropt.IsNull()) {
        trackStates[kInvFiltered]->print("SC");
    }
    cout<<"**** End print of site ****"<<endl;
}

void HKalTrackSite::transform(const TRotation *transMat) {
    // Transform the track state vectors and measurement hit coordinates
    // and the hit's measurement layer using the rotation matrix transMat.

#ifdef kalDebug
    if(transMat->IsIdentity()) {
        Warning("transform()", "Rotation matrix for coordinate transformation not set.");
    }
#endif
    // Track states must be transformed before the hit!
    transformStates(transMat);
    transformHit(transMat);
}