/*
 * =====================================================================================
 *
 *       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"

void
Body::clamp()
{
#ifdef DOCLAMP
    // Constrain the height
    if (X[2]<MAXHEIGHT) {
        X[2]=MAXHEIGHT;
    } else if (X[2]>MINHEIGHT) {
        X[2]=MINHEIGHT;
    }
#endif     /* -----  DOCLAMP  ----- */
}

/*
 *--------------------------------------------------------------------------------------
 *       Class:  Body
 *      Method:  Body :: dx
 * Description:  Increments the state by dx after a Kalman update.
 *--------------------------------------------------------------------------------------
 */
void
Body::dx ( const Matrix<double,STATESIZE,1> &del )
{
    X += del;
    clamp();
    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,STATESIZE,STATESIZE> &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,STATESIZE,STATESIZE>
Body::Q (double dt)
{
    Matrix<double,STATESIZE,STATESIZE> Q;
    Q = Matrix<double,STATESIZE,STATESIZE>::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;
#if STATESIZE==13
    Q.block<4,4>(6,6) = dt*dt*IMU_NOISE*Matrix<double,4,4>::Identity();
    Q.block<3,3>(10,10) = dt*dt*IMU_RANDOMWALK*Matrix<double,3,3>::Identity();
#else
    Q.block<3,3>(6,6) = dt*dt*IMU_RANDOMWALK*Matrix<double,3,3>::Identity();
#endif
    return Q;
}		/* -----  end of method Body::q  ----- */

/*
 *--------------------------------------------------------------------------------------
 *       Class:  Body
 *      Method:  Body :: H
 * Description:  Returns the Jacobian of the measurement model, H.
 *--------------------------------------------------------------------------------------
 */
Matrix<double,1,STATESIZE>
Body::H ( )
{
    Matrix<double,1,STATESIZE> H;
    H = Matrix<double,1,STATESIZE>::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,STATESIZE,STATESIZE>
#if STATESIZE==13
Body::F ( const Vector3d &ang, double dt )
#else
Body::F ( const Vector3d &ang, const Quaterniond &q, double dt )
#endif
{
    Matrix<double,STATESIZE,STATESIZE> F = Matrix<double,STATESIZE,STATESIZE>::Zero();
#if STATESIZE==13
    Quaterniond q = qhat();
#endif
    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;
#if STATESIZE==13
    double qbw1,qbw2,qbw3,qbw4;
    qbw1 = q.x();
    qbw2 = q.y();
    qbw3 = q.z();
    qbw4 = q.w();
    /* Jacobian generated using symbolic matlab */
    double v1,v2,v3;
    v1 = X(3);
    v2 = X(4);
    v3 = X(5);
    F.block<1,4>(0,6) << 2*qbw1*v1 + 2*qbw2*v2 + 2*qbw3*v3, 
                         2*qbw1*v2 - 2*qbw2*v1 + 2*qbw4*v3, 
                         2*qbw1*v3 - 2*qbw3*v1 - 2*qbw4*v2, 
                         2*qbw2*v3 - 2*qbw3*v2 + 2*qbw4*v1;
    F.block<1,4>(1,6) << 2*qbw2*v1 - 2*qbw1*v2 - 2*qbw4*v3,
                         2*qbw1*v1 + 2*qbw2*v2 + 2*qbw3*v3,
                         2*qbw2*v3 - 2*qbw3*v2 + 2*qbw4*v1,
                         2*qbw3*v1 - 2*qbw1*v3 + 2*qbw4*v2;
    F.block<1,4>(2,6) << 2*qbw3*v1 - 2*qbw1*v3 + 2*qbw4*v2,
                         2*qbw3*v2 - 2*qbw2*v3 - 2*qbw4*v1,
                         2*qbw1*v1 + 2*qbw2*v2 + 2*qbw3*v3,
                         2*qbw1*v2 - 2*qbw2*v1 + 2*qbw4*v3;
    F.block<3,4>(3,6) << -2*gravity_world*qbw3,  2*gravity_world*qbw4, -2*gravity_world*qbw1,  2*gravity_world*qbw2,
                         -2*gravity_world*qbw4, -2*gravity_world*qbw3, -2*gravity_world*qbw2, -2*gravity_world*qbw1,
                          2*gravity_world*qbw1,  2*gravity_world*qbw2, -2*gravity_world*qbw3, -2*gravity_world*qbw4;
    F.block<4,4>(6,6) =  omega(ang);
    F.block<3,3>(3,10) = -Matrix<double,3,3>::Identity();
#else
    F.block<3,3>(3,6) = -Matrix<double,3,3>::Identity();
#endif
    F *= dt;
    F += Matrix<double,STATESIZE,STATESIZE>::Identity();
    return F;
}		/* -----  end of method Body::F  ----- */

/*
 *--------------------------------------------------------------------------------------
 *       Class:  Body
 *      Method:  Body :: omega
 * Description:  Returns the Omega(w) used for the motion model of the
 * quaternion.
 *--------------------------------------------------------------------------------------
 */
Matrix<double,4,4>
Body::omega ( const Vector3d &x )
{
    Matrix<double,4,4> Omega;
    Omega.block<3,3>(0,0) = -skewSymmetric(x);
    Omega.block<3,1>(0,3) = x;
    Omega.block<1,3>(3,0) = x.transpose();
    Omega(3,3) = 0;
    return Omega;
}		/* -----  end of method Body::omega  ----- */

/*
 *--------------------------------------------------------------------------------------
 *       Class:  Body
 *      Method:  Body :: motionModel
 * Description:  Propagates the motion model of the body into vector X.
 *--------------------------------------------------------------------------------------
 */
void
#if STATESIZE==13
Body::motionModel ( const Vector3d &acc, const Vector3d &ang, const double dt)
#else
Body::motionModel ( const Vector3d &acc, const Vector3d &ang, const Quaterniond &q, const double dt)
#endif
{
    Vector3d bias = accelerometer_bias();

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

#if STATESIZE==13
    Quaterniond q = qhat();
#endif
    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 STATESIZE==13
    X.segment<4>(6) += 0.5*omega(ang)*X.segment<4>(6);
#endif
    clamp();
    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 )
{
#if STATESIZE==13
    X.segment<3>(10) = b;
#else
    X.segment<3>(6) = b;
#endif
    return ;
}		/* -----  end of method State::accelerometer_bias  ----- */

Vector3d
Body::accelerometer_bias (  )
{
    Vector3d bs;
#if STATESIZE==13
    bs = X.segment<3>(10);
#else
    bs = X.segment<3>(6);
#endif
    return bs ;
}		/* -----  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,STATESIZE,1>
Body::asVector (  )
{
    return X;
}		/* -----  end of method Body::asVector  ----- */

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


/*
 *--------------------------------------------------------------------------------------
 *       Class:  Body
 *      Method:  Body :: qhat
 * Description:  Returns the current quaternion estimate.
 *--------------------------------------------------------------------------------------
 */
Quaterniond
Body::qhat ( )
{
#if STATESIZE==13
    Quaterniond qbw(X(9),X(6),X(7),X(8));
#else
    fprintf(stderr, "Quaternion is not being estimated, quitting.\n");
    exit(1);
#endif
    return qbw;
}		/* -----  end of method Body::qhat  ----- */