#ifndef  body_INC
#define  body_INC
#include <Eigen/Dense>
#include <iostream>
#include "types.h"
#define R_HEIGHT 2.5e-3            /* measurement noise of height measurement */
#define ACC_STD 28e-3
#define ANG_STD 9e-3
#define ACC_BIAS_STD 22e-3
#define ANG_BIAS_STD 3e-3
#define IMU_NOISE 800e-6            /* IMU process noise */
#define IMU_RANDOMWALK 50e-6      /* Bias process noise */
//#define DOCLAMP
#define MAXHEIGHT -1.5 /* Notion of max and min is reversed due to z pointing down */
#define MINHEIGHT -0.3            /*  */


using Eigen::Matrix;
using Eigen::Matrix3d;
using Eigen::Matrix4d;
using Eigen::Vector3d;
using Eigen::Vector4d;
using Eigen::Quaterniond;
using std::cout;
using std::cerr;
using std::endl;

/*
 * =====================================================================================
 *        Class:  Body
 *  Description:  State representation of body.
 * =====================================================================================
 */
class Body
{
    public:
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
        /* ====================  LIFECYCLE     ======================================= */
        Body (){ gravity_world << 0.,0.,9.80655; };                             /* constructor */

        /* ====================  ACCESSORS     ======================================= */
#if STATESIZE==13
        Matrix<double,STATESIZE,STATESIZE> F(const Vector3d &ang, double dt);
        Quaterniond qhat();
        void qhat(const Quaterniond &q);
        void normalizeQuaternion();
#else
        Matrix<double,STATESIZE,STATESIZE> F(const Vector3d &ang, const Quaterniond &q, double dt);
#endif
        Matrix<double,STATESIZE,1> asVector();
        void asVector(const Matrix<double,STATESIZE,1> &m);
        void dx( const Matrix<double,STATESIZE,1> &del);
        Matrix<double,1,STATESIZE> H();
        Matrix<double,STATESIZE,STATESIZE> Q(double dt);
        Matrix<double,1,1> S(const Matrix<double,STATESIZE,STATESIZE> &Pxx);
        Vector3d ned();
        Vector3d vel();
        Vector3d accelerometer_bias( );
        /* ====================  MUTATORS      ======================================= */
        void accelerometer_bias( const Vector3d &b);
        void clamp();
        void pos( const UTM &utm);
        void vel(const Matrix<double,3,1> &v);

        /* ====================  OPERATORS     ======================================= */
        Matrix<double,1,1> h();
#if STATESIZE==13
        void motionModel ( const Vector3d &acc, const Vector3d &ang, const double dt);
#else
        void motionModel ( const Vector3d &acc, const Vector3d &ang, 
                const Quaterniond &q, const double dt);
#endif
        Matrix<double,1,1> R();
        Matrix<double,3,3> skewSymmetric(const Vector3d &x);
        Matrix<double,4,4> omega(const Vector3d &x);
        Matrix<double,4,3> qomega(const Quaterniond &q);
        void unicsv(const double time);

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

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

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

        /* ====================  DATA MEMBERS  ======================================= */
        Matrix<double,STATESIZE,1> X;
        char utm_c;
        int utm_i;
        Vector3d gravity_world;

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


#endif   /* ----- #ifndef body_INC  ----- */