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

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

#define MAXFEATURES 30
//#define FASTMOTIONMODEL // Uncomment this to perform motion model update on all features at once
#define FULLS // Comment out to treat each measurement independently.
using Eigen::Dynamic;
using Eigen::Matrix;
using Eigen::MatrixXd;
using Eigen::Quaterniond;
using Eigen::Vector2d;
using Eigen::Vector3d;
using std::cout;
using std::cerr;
using std::endl;
/*
 * =====================================================================================
 *        Class:  State
 *  Description:  
 * =====================================================================================
 */
class State
{
    public:
        /* ====================  LIFECYCLE     ======================================= */
        State ();                             /* constructor */

        /* ====================  ACCESSORS     ======================================= */
        bool exists(int id);
        int rowById(int id);
        Feature *featureById(int id);
        MatrixXd F(const Quaterniond &q, const Vector3d &w, double dt);
        MatrixXd H ( const Quaterniond &q, const std::vector<measurement_t> &z );
        int Hrows( const std::vector<measurement_t> &z );
        MatrixXd Q(double dt);
        MatrixXd R( const std::vector<measurement_t> &z );
        Matrix<double,9,9> Pxx();
        Matrix<double,Dynamic,6> L();

        /* ====================  MUTATORS      ======================================= */
        void accelerometer_bias(const Vector3d &b);
        void pos(const UTM &utm);
        void handle_measurements( const std::vector<measurement_t> & z, const Quaterniond &q);
        void initializePi(int i, const Matrix<double,3,3> &Pi);
        void kalmanUpdate( const MatrixXd &h, const MatrixXd &S, 
                const std::vector<measurement_t> &z, const Quaterniond &q);
        void motionModel(const Vector3d &acc, const Vector3d &ang,
                const Quaterniond &q, const double dt);
        void Pkk1 ( const Vector3d &ang, const Quaterniond &q, const double dt);
        void position_covariance(const Matrix<double,3,3> &p);
        std::list<Feature *>::iterator removeFeature(std::list<Feature *>::iterator &i, int j);
        void velocity_covariance(const Matrix<double,3,3> &p);
        void vel(const Matrix<double,3,1> &v);

        /* ====================  OPERATORS     ======================================= */
        Matrix<double,Dynamic,1> innovation( const std::vector<measurement_t> &z, const Quaterniond &q);
        void unicsv();

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

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

    private:
        /* ====================  METHODS       ======================================= */
        void addFeatures(std::vector<measurement_t> &F, const Quaterniond &q, double z);
        void expandP ( const Matrix3d &Pi);
        void shrinkP( int i );

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


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



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