/*
 * =====================================================================================
 *
 *       Filename:  body.cpp
 *
 *    Description:  Method definitions for body class.
 *
 *        Version:  1.0
 *        Created:  03/17/2017 08:07:35 PM
 *       Revision:  none
 *       Compiler:  gcc
 *
 *         Author:  Martin Miller (MHM), miller7@illinois.edu
 *   Organization:  Aerospace Robotics and Controls Lab (ARC)
 *
 * =====================================================================================
 */
#include "body.h"

/*
 *--------------------------------------------------------------------------------------
 *       Class:  Body
 *      Method:  Body :: dx
 * Description:  Increments the state by dx after a Kalman update.
 *--------------------------------------------------------------------------------------
 */
void
Body::dx ( const Matrix<double,9,1> &del )
{
    X += del;
    // Constrain the height
    if (X[2]<-1.5) {
        X[2]=-1.;
    } else if (X[2]>-0.3) {
        X[2]=-0.3;
    }
    return ;
}		/* -----  end of method Body::dx  ----- */

void
Body::vel ( const Matrix<double,3,1> &v )
{
    X.segment<3>(3) = v;
    return ;
}		/* -----  end of method Body::vel  ----- */

/*
 *--------------------------------------------------------------------------------------
 *       Class:  Body
 *      Method:  Body :: S
 * Description:  Returns the matrix S=HPH.T+R. HPH.T is trivial, so Body::H() is
 * not called explicitly.
 *--------------------------------------------------------------------------------------
 */
Matrix<double,1,1>
Body::S ( const Matrix<double,9,9> &Pxx )
{
    Matrix<double,1,1> S;
    S << Pxx(2,2);
    return S+R();
}		/* -----  end of method Body::S  ----- */

/*
 *--------------------------------------------------------------------------------------
 *       Class:  Body
 *      Method:  Body :: pos
 * Description:  Stores the UTM position in NED format.
 *--------------------------------------------------------------------------------------
 */
void
Body::pos ( const UTM &utm )
{
    utm_c = utm.zone_c;
    utm_i = utm.zone_i;
    X[0] = utm.northing;
    X[1] = utm.easting;
    X[2] = -utm.up;
    return ;
}		/* -----  end of method Body::pos  ----- */


/*
 *--------------------------------------------------------------------------------------
 *       Class:  Body
 *      Method:  Body :: R
 * Description:  Returns R matrix of measurement noise for height measurement.
 *--------------------------------------------------------------------------------------
 */
Matrix<double,1,1> 
Body::R()
{
    Matrix<double,1,1> R;
    R << R_HEIGHT;
    return R;
}

/*
 *--------------------------------------------------------------------------------------
 *       Class:  Body
 *      Method:  Body :: Q
 * Description:  Returns Q matrix of process noise for the body.
 *--------------------------------------------------------------------------------------
 */
Matrix<double,9,9>
Body::Q (double dt)
{
    Matrix<double,9,9> Q;
    Q = Matrix<double,9,9>::Zero();
    Q.block<3,3>(0,0) = 0.25*dt*dt*dt*dt*Matrix<double,3,3>::Identity();
    Q.block<3,3>(3,0) = 0.5*dt*dt*dt*Matrix<double,3,3>::Identity();
    Q.block<3,3>(0,3) = 0.5*dt*dt*dt*Matrix<double,3,3>::Identity();
    Q.block<3,3>(3,3) = dt*dt*Matrix<double,3,3>::Identity();
    Q *= IMU_NOISE;
    Q.block<3,3>(6,6) = dt*dt*IMU_RANDOMWALK*Matrix<double,3,3>::Identity();
    return Q;
}		/* -----  end of method Body::q  ----- */

/*
 *--------------------------------------------------------------------------------------
 *       Class:  Body
 *      Method:  Body :: H
 * Description:  Returns the Jacobian of the measurement model, H.
 *--------------------------------------------------------------------------------------
 */
Matrix<double,1,9>
Body::H ( )
{
    Matrix<double,1,9> H;
    H = Matrix<double,1,9>::Zero();
    H(2) = 1.;
    return H ;
}		/* -----  end of method Body::H  ----- */


/*
 *--------------------------------------------------------------------------------------
 *       Class:  Body
 *      Method:  Body :: h
 * Description:  Returns the predicted measurement vector, h.
 *--------------------------------------------------------------------------------------
 */
Matrix<double,1,1>
Body::h ( )
{
    Matrix<double,1,1> h;
    h[0] = X[2];
    return h;
}		/* -----  end of method Body::h_hat  ----- */

/*
 *--------------------------------------------------------------------------------------
 *       Class:  Body
 *      Method:  Body :: F
 * Description:  Returns the Jacobian of the motion model of the body.
 *--------------------------------------------------------------------------------------
 */
Matrix<double,9,9>
Body::F ( const Vector3d &ang, const Quaterniond &q, double dt )
{
    Matrix<double,9,9> F = Matrix<double,9,9>::Zero();
    Matrix<double,3,3> Rbw(q.toRotationMatrix());
    Matrix<double,3,3> W;
    W = skewSymmetric(ang);
    F.block<3,3>(0,3) = Rbw;
    F.block<3,3>(3,3) = -W;
    F.block<3,3>(3,6) = -Matrix<double,3,3>::Identity();
    F *= dt;
    F += Matrix<double,9,9>::Identity();
    return F;
}		/* -----  end of method Body::F  ----- */

/*
 *--------------------------------------------------------------------------------------
 *       Class:  Body
 *      Method:  Body :: motionModel
 * Description:  Propagates the motion model of the body into vector X.
 *--------------------------------------------------------------------------------------
 */
void
Body::motionModel ( const Vector3d &acc, const Vector3d &ang, const Quaterniond &q, const double dt)
{
    Vector3d bias(X.segment(6,3));
    Vector3d gravity_world(0.,0.,9.80655);

    Matrix<double,6,3> A;
    Matrix<double,6,1> b;
    A = Matrix<double,6,3>::Zero();
    b = Matrix<double,6,1>::Zero();

    Matrix<double,3,3> Rbw(q.toRotationMatrix());
    A.block<3,3>(0,0) = Rbw;
    Matrix<double,3,3> W;
    W = skewSymmetric(ang);
    A.block<3,3>(3,0) = -W;
    b.segment<3>(3) = acc-bias+Rbw.transpose()*gravity_world;

    X.segment<6>(0) += (A*X.segment<3>(3)+b)*dt;
    if (X[2]>-0.3) X[2]=-0.3;
    if (X[2]<-1.3) X[2]=-1.3;
    return ;
}		/* -----  end of method Body::motionModel  ----- */

/*
 *--------------------------------------------------------------------------------------
 *       Class:  Body
 *      Method:  Body :: skewSymmetric
 * Description:  Create the skew symmetric matrix y from the vector x.
 *--------------------------------------------------------------------------------------
 */
Matrix<double,3,3>
Body::skewSymmetric ( const Vector3d &x )
{
    Matrix<double,3,3> y;
    y << 0.,-x[2], x[1], x[2],0.,-x[0],-x[1],x[0],0.;
    return y;
}		/* -----  end of method Body::skewSymmetric  ----- */

void
Body::unicsv ( )
{
    printf("%d,%c,%f,%f,%f,%f,%f,%f,%f,%f,%f", utm_i, utm_c,
            X[0], X[1], -X[2],
            X[3], X[4], X[5],
            X[6], X[7], X[8]);
    return ;
}		/* -----  end of method Body::unicsv  ----- */

    void
Body::accelerometer_bias ( const Vector3d &b )
{
    X.segment<3>(6) = b;
    return ;
}		/* -----  end of method State::accelerometer_bias  ----- */

/*
 *--------------------------------------------------------------------------------------
 *       Class:  Body
 *      Method:  Body :: ned
 * Description:  Returns the body position in NED coordinates.
 *--------------------------------------------------------------------------------------
 */
Vector3d
Body::ned (  )
{
    return X.segment<3>(0);
}		/* -----  end of method Body::ned  ----- */

Vector3d
Body::vel (  )
{
    return X.segment<3>(3);
}		/* -----  end of method Body::vel  ----- */

Matrix<double,9,1>
Body::asVector (  )
{
    return X;
}		/* -----  end of method Body::asVector  ----- */

void
Body::asVector ( const Matrix<double,9,1> &m )
{
    X = m;
    return ;
}		/* -----  end of method Body::asVector  ----- */