#ifndef  body_INC
#define  body_INC
#include <Eigen/Dense>
#include <iostream>
#include "types.h"

using Eigen::Matrix;
using Eigen::Vector3d;
using Eigen::Quaternion;

/*
 * =====================================================================================
 *        Class:  Body
 *  Description:  State representation of body.
 * =====================================================================================
 */
class Body
{
    public:
        /* ====================  LIFECYCLE     ======================================= */
        Body (){};                             /* constructor */

        /* ====================  ACCESSORS     ======================================= */

        /* ====================  MUTATORS      ======================================= */
        void accelerometer_bias( const Vector3d &b);
        void enu( const UTM &utm);
        void update(const Matrix<double,9,1> &dx);
        void vel(const Matrix<double,3,1> &v);

        /* ====================  OPERATORS     ======================================= */
        Matrix<double,9,9> F(const Vector3d &ang, const Quaternion<double> &q);
        Matrix<double,1,9> H();
        Matrix<double,1,1> h();
        void motionModel ( const Vector3d &acc, const Vector3d &ang, 
                const Quaternion<double> &q, const double dt);
        Matrix<double,9,9> Q(double dt);
        Matrix<double,1,1> R();
        Matrix<double,1,1> S(const Matrix<double,9,9> &P);
        void skewSymmetric(const Vector3d &x, Matrix<double,3,3> &y);
        void unicsv();

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

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

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

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

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


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