/*
 * =====================================================================================
 *
 *       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();
#if STATESIZE==13
    normalizeQuaternion();
#endif
    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,3> Ga;
    Ga.block<3,3>(0,0) = 0.5*dt*dt*Matrix3d::Identity();
    Ga.block<3,3>(3,0) = dt*Matrix3d::Identity();
    Ga.block<STATESIZE-6,3>(6,0) = Matrix<double,STATESIZE-6,3>::Zero();

    Matrix<double,STATESIZE,3> Gb;
    if (donew) {
        Gb.block<3,3>(0,0) = -0.5*dt*dt*Matrix3d::Identity();
        Gb.block<3,3>(3,0) = -dt*Matrix3d::Identity();
        Gb.block<STATESIZE-6,3>(6,0) = Matrix<double,STATESIZE-6,3>::Zero();
        Gb.block<3,3>(STATESIZE-3,0) = dt*Matrix3d::Identity();
    } else {
        Gb.block<STATESIZE-3,3>(0,0) = Matrix<double,STATESIZE-3,3>::Zero();
        Gb.block<3,3>(STATESIZE-3,0) = 0.5*dt*dt*Matrix3d::Identity();
    }

    Matrix<double,STATESIZE,3> Gw;
    if (donew) {
        Gw.block<3,3>(0,0) = 0.5*dt*dt*skewSymmetric(vel());
        Gw.block<3,3>(3,0) = dt*skewSymmetric(vel());
        Gw.block<3,3>(STATESIZE-3,0) = Matrix3d::Zero();
    } else {
        Gw = Matrix<double,STATESIZE,3>::Zero();
    }

#if STATESIZE==13
    Quaterniond q = qhat();
    Gw.block<4,3>(6,0) = 0.5*dt*qomega(q);
#endif
    Matrix<double,STATESIZE,STATESIZE> Q;
    Q = Matrix<double,STATESIZE,STATESIZE>::Zero();
    if (donew) {
        Q += acc_std*acc_std*Ga*Ga.transpose();
        Q += ang_std*ang_std*Gw*Gw.transpose();
        Q += acc_bias_std*acc_bias_std*Gb*Gb.transpose();
    } else {
        Q += (acc_bias_std+ang_std+acc_std)*(acc_std+acc_bias_std+ang_std)*Ga*Ga.transpose();
        Q += ang_std*ang_std*Gw*Gw.transpose();
        Q += acc_bias_std*acc_bias_std*Gb*Gb.transpose();
    }

    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);
    double gw = -gravity_world[2];
    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*gw*qbw3,  2*gw*qbw4, -2*gw*qbw1,  2*gw*qbw2,
                         -2*gw*qbw4, -2*gw*qbw3, -2*gw*qbw2, -2*gw*qbw1,
                          2*gw*qbw1,  2*gw*qbw2, -2*gw*qbw3, -2*gw*qbw4;
    F.block<4,4>(6,6) =  0.5*omega(ang);
#endif
    F.block<3,3>(3,STATESIZE-3) = -Matrix<double,3,3>::Identity();
    F *= dt;
    F += Matrix<double,STATESIZE,STATESIZE>::Identity();
    return F;
}		/* -----  end of method Body::F  ----- */

Matrix<double,4,3>
Body::qomega ( const Quaterniond &q )
{
    Matrix<double,4,3> qom;
    qom << q.w(), -q.z(), q.y(),
           q.z(), q.w(), -q.x(),
           -q.y(), q.x(), q.w(),
           -q.x(),-q.y(),-q.z();

    return qom ;
}		/* -----  end of method Body::qomega  ----- */

/*
 *--------------------------------------------------------------------------------------
 *       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;
    Matrix<double,6,1> dX;
    dX = (A*X.segment<3>(3)+b)*dt;

    X.segment<6>(0) += (A*X.segment<3>(3)+b)*dt;
#if STATESIZE==13
    Matrix<double,4,1> dq;
    dq = dt*0.5*omega(ang)*X.segment<4>(6);
    X.segment<4>(6) += dq;
    normalizeQuaternion();
#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 ( const double time )
{
#ifdef FEATUREMAP
    printf("0,");
#endif
#if STATESIZE==13
    printf("%f,%d,%c,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f", time,utm_i, utm_c,
            X[0], X[1], -X[2],
            X[3], X[4], X[5],
            X[6], X[7], X[8],X[9],
            X[10],X[11],X[12]);
#else
    printf("%f,%d,%c,%f,%f,%f,%f,%f,%f,%f,%f,%f", time,utm_i, utm_c,
            X[0], X[1], -X[2],
            X[3], X[4], X[5],
            X[6], X[7], X[8]);
#endif
    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.
 *--------------------------------------------------------------------------------------
 */
#if STATESIZE==13
Quaterniond
Body::qhat ( )
{
    Quaterniond qbw(X(9),X(6),X(7),X(8));
    return qbw;
}		/* -----  end of method Body::qhat  ----- */

void
Body::qhat ( const Quaterniond &q )
{
    X[6] = q.x();
    X[7] = q.y();
    X[8] = q.z();
    X[9] = q.w();
    return ;
}		/* -----  end of method Body::qhat  ----- */

void
Body::normalizeQuaternion (  )
{
    Quaterniond q = qhat();
    Matrix<double,4,1> qv;
    qv[0] = q.x();
    qv[1] = q.y();
    qv[2] = q.z();
    qv[3] = q.w();
    qv /= qv.norm();
    X.segment<4>(6) = qv;
    //qhat(q.normalized());
    return ;
}		/* -----  end of method Body::normalizequaternion  ----- */

#endif