#ifndef  state_INC
#define  state_INC
#include <Eigen/Dense>
#include <list>
#include <vector>

#include "body.h"
#include "feature.h"
#include "types.h"

#define MAXFEATURES 30
using Eigen::Matrix;
using Eigen::MatrixXd;
using Eigen::Dynamic;
using Eigen::Vector3d;
using Eigen::Quaterniond;
/*
 * =====================================================================================
 *        Class:  State
 *  Description:  
 * =====================================================================================
 */
class State
{
    public:
        /* ====================  LIFECYCLE     ======================================= */
        State ();                             /* constructor */

        /* ====================  ACCESSORS     ======================================= */
        bool exists(int id);

        /* ====================  MUTATORS      ======================================= */
        void accelerometer_bias(const Vector3d &b);
        void enu(const UTM &utm);
        void feature_update( const std::vector<measurement_t> & z, const Quaterniond &q);
        void initializePi(int i, const Matrix<double,3,3> &Pi);
        void motionModel(const Vector3d &acc, const Vector3d &ang,
                const Quaterniond &q, const double dt);
        void position_covariance(const Matrix<double,3,3> &p);
        void velocity_covariance(const Matrix<double,3,3> &p);
        void vel(const Matrix<double,3,1> &v);
        void Pkk1 ( const Vector3d &ang, const Quaterniond &q, const double dt);
        void update ( const Matrix<double,1,1> &z);

        /* ====================  OPERATORS     ======================================= */
        void unicsv();

    protected:
        /* ====================  METHODS       ======================================= */

        /* ====================  DATA MEMBERS  ======================================= */

    private:
        /* ====================  METHODS       ======================================= */
        void expandP(int Nnew);

        /* ====================  DATA MEMBERS  ======================================= */
        Body *body;
        Matrix<double, Dynamic, Dynamic, 0, 9+3*MAXFEATURES, 9+3*MAXFEATURES> P;
        char utm_c;
        int utm_i;
        std::list<Feature *> features;


}; /* -----  end of class State  ----- */



#endif   /* ----- #ifndef state_INC  ----- */