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

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

#define MAXFEATURES 40
#define MINFEATURES 30
#define COVBIAS 2e-5
//#define FASTMOTIONMODEL // Uncomment this to perform motion model update on all features at once
#define DORANSAC            /*  */
//#define INLIERTEST            /*  */
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;
using std::vector;

/*
 * =====================================================================================
 *        Class:  State
 *  Description:  
 * =====================================================================================
 */
class State
{
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
        /* ====================  LIFECYCLE     ======================================= */
        State ();                             /* constructor */

        /* ====================  ACCESSORS     ======================================= */
        Matrix<double,Dynamic,1> asVector();
        bool exists(int id);
        int rowById(int id);
        int iById(int id);
        Feature *featureById(int id);
#if STATESIZE==13
        MatrixXd F(const Vector3d &w, double dt);
        MatrixXd H ( const vector<measurement_t> &z );
        MatrixXd blockSI( const vector<measurement_t> &z );
        Quaterniond qhat();
        void featuresAsMeasurements(vector<measurement_t> &yk, const Camera &cam);
#else
        MatrixXd F(const Quaterniond &q, const Vector3d &w, double dt);
        MatrixXd H ( const Quaterniond &q, const vector<measurement_t> &z );
        MatrixXd blockSI( const vector<measurement_t> &z, const Quaterniond &q);
        void featuresAsMeasurements(vector<measurement_t> &yk, const Camera &cam,
                const Quaterniond &q);
#endif
        int Hrows( const vector<measurement_t> &z );
        MatrixXd Q(double dt);
        MatrixXd R( const vector<measurement_t> &z );
        Matrix<double,STATESIZE,STATESIZE> Pxx();
        Matrix<double,STATESIZE,3> Pxy(int id);
        Matrix<double,3,3> Pyy(int id);
        Matrix<double,Dynamic,6> L();

        /* ====================  MUTATORS      ======================================= */
        void accelerometer_bias(const Vector3d &b);
        void asVector(const Matrix<double,Dynamic,1> &m);
        void pos(const UTM &utm);
#if STATESIZE==13
        void doMeasurements( vector<measurement_t> &z, Vision &viz, Camera &cam);
        void handle_measurements( const vector<measurement_t> & z);
        void kalmanUpdate( const MatrixXd &h, const MatrixXd &S, 
                const vector<measurement_t> &z);
        void motionModel(const Vector3d &acc, const Vector3d &ang, const double dt);
        MatrixXd partialUpdate( const MatrixXd &h, const MatrixXd &S, 
                const vector<measurement_t> &z);
        void Pkk1 ( const Vector3d &ang, const double dt);
        void ransacUpdate(vector<measurement_t> &z);
        void quaternion_covariance(const Matrix<double,4,4> &covq);
        void qhat(const Quaterniond &q);
#else
        void doMeasurements( vector<measurement_t> &z, Vision &viz,
                const Quaterniond &q, Camera &cam);
        void handle_measurements( const vector<measurement_t> & z, const Quaterniond &q);
        void kalmanUpdate( const MatrixXd &h, const MatrixXd &S, 
                const vector<measurement_t> &z, const Quaterniond &q);
        void motionModel(const Vector3d &acc, const Vector3d &ang,
                const Quaterniond &q, const double dt);
        MatrixXd partialUpdate( const MatrixXd &h, const MatrixXd &S, 
                const vector<measurement_t> &z, const Quaterniond &q );
        void Pkk1 ( const Vector3d &ang, const Quaterniond &q, const double dt);
        void ransacUpdate(vector<measurement_t> &z, const Quaterniond &q);
#endif
        void initializePi(int i, const Matrix<double,3,3> &Pi);
        void position_covariance(const Matrix<double,3,3> &p);
        std::list<Feature *>::iterator removeFeature(std::list<Feature *>::iterator &i, bool bl);
        std::list<Feature *>::iterator removeFeature ( int id, bool bl );
        void velocity_covariance(const Matrix<double,3,3> &p);
        void vel(const Matrix<double,3,1> &v);

        /* ====================  OPERATORS     ======================================= */
#if STATESIZE==13
        Matrix<double,Dynamic,1> innovation( const vector<measurement_t> &z);
#else
        Matrix<double,Dynamic,1> innovation( const vector<measurement_t> &z, const Quaterniond &q);
#endif
        void unicsv(const double time);

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

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

    private:
        /* ====================  METHODS       ======================================= */
#if STATESIZE==13
        void addFeatures(vector<measurement_t> &F, double z);
#else
        void addFeatures(vector<measurement_t> &F, const Quaterniond &q, double z);
#endif
        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;
        vector<int> blacklist;


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



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