#ifndef  feature_INC
#define  feature_INC
#include <opencv2/opencv.hpp>
#include <Eigen/Dense>
#include <iostream>

#include "types.h"
#include "camera.h"

#define FEATURE_NOISE  3.5           /* Feature process noise */
#define VIEW_NOISE 6e-3             /*  */
#define INITIAL_VIEW_NOISE 1e-3           /*  */
#define REFLECTION_VIEW_NOISE 6e-3            /*  */
#define FEATURECOVX .0001            /*  */
#define FEATURECOVY .0001           /*  */
#define FEATURECOVRHO 25e-4            /*  */
#define FEATURECOVRHO_MONO 0.5            /*  */
#define RHO_0 1./10.            /*  */
#define RANSAC_LI_THRESHOLD 2e-6            /*  */
#define RANSAC_HI_THRESHOLD 5e-2            /*  */
#define INITDEPTH

using cv::Mat;
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:  Feature
 *  Description:  
 * =====================================================================================
 */
class Feature
{
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
        /* ====================  LIFECYCLE     ======================================= */
        Feature ( int id, const Vector3d &xs, const Vector3d &xr,
                const Vector3d &xbw, const Quaterniond &q, double z, Mat &p);
        Feature ( int id, const Vector3d &xs, const Vector3d &xbw,
                const Quaterniond &q, Mat &p);

        /* ====================  ACCESSORS     ======================================= */
        int id();
        Matrix<double,3,3> P0(measurement_type t);
        Vector3d asVector();
        UTM utm(const Vector3d &xbw, const Quaterniond &q); 
        Mat patch();
        Mat warpedPatch(const Camera &cam, const Quaterniond &q1);
        Mat reflectedPatch(const Camera &cam, const Quaterniond &q1);
        void unicsv(const double time, const Vector3d &xbw, const Quaterniond &qbw) ;

        /* ====================  MUTATORS      ======================================= */
        void asVector(const Vector3d &m);
        void motionModel ( const Vector3d &ang, const Vector3d &vel, const double dt);
        void dx( const Vector3d &del);
        void seen();
        bool since(int N) const;
        void reinitialize(const Vector3d &xs, const Vector3d& xr,
                const Vector3d &xbw, const Quaterniond &q, Mat &p);

        /* ====================  OPERATORS     ======================================= */
        bool sane();
        bool isInlier(const measurement_t &z, const Matrix<double,STATESIZE,STATESIZE> &Pxx,
                const Matrix<double,STATESIZE,3> &Pxy, const Matrix<double,3,3> &Pyy,
                const Vector3d &pos, const Quaterniond &q, double thr);
        Matrix<double,3,STATESIZE> Fx( double dt );
        Matrix<double,6,STATESIZE> Hx( const Vector3d &pos, const Quaterniond &q);
        Matrix<double,6,6> S ( const Matrix<double,STATESIZE,STATESIZE> &Pxx, 
                const Matrix<double,STATESIZE,3> &Pxy, const Matrix<double,3,3> &Pyy, 
                const Vector3d &pos, const Quaterniond &q);
        bool isRansacInlier(const measurement_t &z, const Vector3d &pos, const Quaterniond &q);
        bool inFOV();
        Vector3d findDepth( const Quaterniond &q, double z, const Vector3d &xs,
                const Vector3d &xr);
        Matrix<double,3,3> Fy( const Vector3d &vel, const Vector3d &ang, double dt);
        Matrix<double,6,3> Hy( const Vector3d &pos, const Quaterniond &q);
        Matrix<double,6,1> h( const Vector3d &x, const Quaterniond &q);
        Matrix<double,3,6> L();
        Matrix<double,6,1> measurement_vector(const Vector3d &xs, const Vector3d &xr);
        Matrix<double,4,1> measurement_vector( const Vector3d &xs );

        Matrix<double,3,3> Q(const double dt);
        Matrix<double,6,6> R();
        Vector3d p2x(const Vector3d &p);
        Vector3d x2p(const Vector3d &x);

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

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

    private:
        /* ====================  METHODS       ======================================= */

        /* ====================  DATA MEMBERS  ======================================= */
        int _id;
        Mat _patch;
        unsigned int lastseen;
        Quaterniond q0;
        Vector3d X;
        Vector3d X0;
        Vector3d xb0w;

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


#endif   /* ----- #ifndef feature_INC  ----- */