#ifndef KALMAN_FILTER_H
#define KALMAN_FILTER_H
#include <BLAS.h>
#include <vector>

using namespace std;
// this class is designed for error state kalman filter

// error state time update equation implementation
template<int error_state_size>
class TimeUpdater 
{
public:
    TimeUpdater();
    ~TimeUpdater();
    
    // init time updater 
    void init();
    
    // do time update
    void time_update();
    
private:
    CVector<double, error_state_size> error_state_;
};

// measurement update equation implementation
// measurement_size: is the measurement data
template<int measurement_size>
class MeasurementUpdater
{
public:
    MeasurementUpdater();
    ~measurementUpdater();
    
    void measurement_update(CVector<double, measurement_size>& measurement);
    
private:
    CVector<double, measurement_size> measurement_;
};

typedef MeasurementUpdater<4> gnss_measurement_updater;
typedef MeasurementUpdater<2> dmevor_measurement_updater;
typedef MeasurementUpdater<2> ils_measurement_updater;

enum KF_Type
{
    PURE_KF = 0
};

template<int state_size, int error_state_size>
class KalmanFilter {
public:
    KalmanFilter();
    ~KalmanFilter();
    
    void init();
    
    void set_timeupdater();
    void set_measurementupdater();
    
    void set_nominal_state(CVector<double, state_size> nominal_state);
    
    void ; 
    
private:
    
    TimeUpdater<error_state_size> time_updater_;
    
    gnss_measurement_updater gps_measurement_updater_;
    dmevor_measurement_updater dmevor_measurement_updater_;
    ils_measurement_updater ils_measurement_updater_;
    
    bool use_gps_;
    bool use_dmevor_;
    bool use_ils_;
    
    KF_Type kf_type_;
    
    CVector<double, state_size> nominal_state_;
    CVector<double, error_state_size> error_state_;
};
#endif