/*
 * =====================================================================================
 *
 *       Filename:  main.cpp
 *
 *    Description:  Main code for reflections SLAM. While the code is primarily
 *    designed to run in ROS, an attempt is being made to allow some
 *    functionality without it. However, no guarantees are made that the two
 *    will produce the same outputs for a given input.
 *
 *        Version:  1.0
 *        Created:  03/17/2017 04:12:33 PM
 *       Revision:  none
 *       Compiler:  gcc
 *
 *         Author:  Martin Miller (MHM), miller7@illinois.edu
 *   Organization:  Aerospace Robotics and Controls Lab (ARC)
 *
 * =====================================================================================
 */

#include "main.h"
bool seenutm=false;
bool seenpva=false;
using std::cout;
using std::endl;
using std::cerr;

#ifdef  USE_ROS

#else      /* -----  not USE_ROS  ----- */
void
covCallback(const message &msg, Matrix<double,9,9> &P, const Quaternion<double> &q)
{
    if (seenutm==false || seenpva==false) {
        Matrix3d Rbw(q.toRotationMatrix());
        Matrix3d Renuned;
        Renuned << 0.,1.,0.,1.,0.,0.,0.,0.,-1;
        P.block(0,0,3,3) = Renuned*msg.covariance.position*Renuned.transpose();
        P.block(3,3,3,3) = Rbw.transpose()*msg.covariance.velocity*Rbw;
    }
}

void
imgCallback(const message *msg)
{
    return;
}

void 
imuCallback(const message &msg, Matrix<double,9,1> &X, 
        Matrix<double,9,9> &P, const Quaternion<double> &q,
        const timestamp dt)
{
    if (!seenutm || !seenpva || (dt.secs==0 && dt.nsecs==0)) return;
    Vector3d acc(msg.linear_acceleration.y,msg.linear_acceleration.x,-msg.linear_acceleration.z);
    Vector3d ang(msg.angular_velocity.y,msg.angular_velocity.x,-msg.angular_velocity.z);
    // WARNING: This is the bias for 1112-1
    Vector3d ang_bias(-2.795871394666666222e-03, 6.984255690000021506e-03, 1.418145565750002614e-03);
    ang-=ang_bias;
    Body body;
    State mu;
    double dtf = dt.secs+dt.nsecs*1e-9;

    Matrix<double,9,9> F;
    F = body.F(X,ang,q);
    mu.Pkk1(P,F,body.Q(dtf));
    body.motionModel(X,acc,ang,q,dtf);
}

void
pvaCallback(const message &msg, Matrix<double,9,1> &X, Quaternion<double> &q)
{
    // Update quaternion
    using Eigen::AngleAxisd;
    double roll, pitch, yaw;
    attitude_t A=msg.attitude;
    roll = M_PI*msg.attitude.roll/180.;
    pitch = M_PI*msg.attitude.pitch/180.;
    yaw = M_PI*(msg.attitude.yaw)/180.;
    Matrix3d R;
    R = AngleAxisd(yaw, Vector3d::UnitZ())
      * AngleAxisd(roll, Vector3d::UnitX())
      *  AngleAxisd(pitch, Vector3d::UnitY());
    q = Quaternion<double>(R);

    // Set body velocity
    if (!seenpva) {
        Vector3d vw(msg.velocity.north,msg.velocity.east,-msg.velocity.up);
        X.segment(3,3) = R.transpose()*vw;
        seenpva=true;
    } 
    return;
}

void
utmCallback(const message &msg, Matrix<double,9,1> &X, Matrix<double,9,9> &P)
{
    if (!seenutm) {
        seenutm=true;
        X[0] = msg.utm.northing;
        X[1] = msg.utm.easting;
        X[2] = -msg.utm.up;
    } else { // Perform Z measurement
        State mu;
        Body body;
        Matrix<double,1,1> z,h;
        Matrix<double,1,9> H;
        z(0) = -msg.utm.up;
        h = body.h(X);
        body.H(H);
        mu.update(X,P,H,h,z,body.R());
    }
    return;
}
#endif     /* -----  not USE_ROS  ----- */

int 
parseLine(char *line, message *data)
{
    char *time_string;
    char *msg_type;
    double t;
    int s,ns;
    time_string = strsep(&line, ",");
    msg_type = strsep(&line, ",");
    sscanf(time_string,"%lf", &t);
    s = (int) t;
    ns = (int) ((t-s)*1e9);
    data->stamp.secs=s;
    data->stamp.nsecs=ns;

    if (!strcmp(msg_type,"bestutm")) {
        data->msg_type = BESTUTM;
        sscanf(line,"%d,%c,%lf,%lf,%lf", 
                &data->utm.zone_i, 
                &data->utm.zone_c, 
                &data->utm.northing, 
                &data->utm.easting, 
                &data->utm.up);
    } else if (!strcmp(msg_type,"IMG")) {
        data->msg_type = IMG;
        char *lfn,*rfn;
        lfn = strsep(&line, ",");
        sscanf(lfn, "%s", &data->image_names);
        rfn = strsep(&line, ",");
        sscanf(rfn, "%s", &data->image_names+1);
    } else if (!strcmp(msg_type,"inscovs")) {
        double pos[9];
        double att[9];
        double vel[9];
        data->msg_type = INSCOVS;
        sscanf(line,"%lf,%lf,%lf,\
                     %lf,%lf,%lf,\
                     %lf,%lf,%lf,\
                     %lf,%lf,%lf,\
                     %lf,%lf,%lf,\
                     %lf,%lf,%lf,\
                     %lf,%lf,%lf,\
                     %lf,%lf,%lf,\
                     %lf,%lf,%lf",
                     pos, pos+1, pos+2,
                     pos+3, pos+4, pos+5,
                     pos+6, pos+7, pos+8,
                     att, att+1, att+2,
                     att+3, att+4, att+5,
                     att+6, att+7, att+8,
                     vel, vel+1, vel+2,
                     vel+3, vel+4, vel+5,
                     vel+6, vel+7, vel+8);
        data->covariance.position << pos[0], pos[1], pos[2],
                                     pos[3], pos[4], pos[5],
                                     pos[6], pos[7], pos[8];
        data->covariance.attitude << att[0], att[1], att[2],
                                     att[3], att[4], att[5],
                                     att[6], att[7], att[8];
        data->covariance.velocity << vel[0], vel[1], vel[2],
                                     vel[3], vel[4], vel[5],
                                     vel[6], vel[7], vel[8];
    } else if (!strcmp(msg_type,"inspvas")) {
        data->msg_type = INSPVAS;
        sscanf(line,"%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf",
                &data->position.latitude,
                &data->position.longitude,
                &data->position.altitude,
                &data->velocity.north,
                &data->velocity.east,
                &data->velocity.up,
                &data->attitude.roll,
                &data->attitude.pitch,
                &data->attitude.yaw);
    } else if (!strcmp(msg_type,"rawimus")) {
        data->msg_type = RAWIMUS;
        double acc_y, ang_y;
        sscanf(line,"%lf,%lf,%lf,%lf,%lf,%lf",
                &data->linear_acceleration.z,
                &acc_y,
                &data->linear_acceleration.x,
                &data->angular_velocity.z,
                &ang_y,
                &data->angular_velocity.x);
        data->linear_acceleration.y = -acc_y;
        data->angular_velocity.y = -ang_y;
    } else {
        fprintf(stderr, "Message type %s is unknown, quitting.\n", msg_type);
        exit(1);
    }
}

timestamp 
update_dt(const timestamp t, timestamp *t_old)
{
    double dtf=0;
    if (t_old->secs!=0 || t_old->nsecs!=0) {
        double tf = t.secs+1e-9*t.nsecs;
        double t_oldf = t_old->secs+1e-9*t_old->nsecs;
        dtf = tf-t_oldf;
    }
    t_old->secs=t.secs;
    t_old->nsecs=t.nsecs;

    timestamp dt;
    dt.secs = (int) dtf;
    dt.nsecs = (int)((dtf-(double)dt.secs)*1e9);
    
    return dt;
}

int 
main(int argc, char **argv)
{
    Quaternion<double> qbw;
    Matrix<double,9,1> bodyStateVector;
    Matrix<double,9,9> P;
    // bias in FRD coordinates
    bodyStateVector.segment(6,3) << -0.03713532, 1.125*0.01465135, -1*-0.00709229;
    timestamp dt{0,0};
    timestamp t_old{0,0};

#ifdef  USE_ROS
    
#else      /* -----  not USE_ROS  ----- */
printf("UTM-Zone,UTM-Ch,UTM-North,UTM-East,Altitude,Vel-X,Vel-Y,Vel-Z,Bias-X,Bias-Y,Bias-Z\n");
// Read sensors from file
char line[MAXLINE];
while (scanf("%s", line)!=EOF) {
    message msg;
    parseLine(line, &msg);
    
    switch ( msg.msg_type ) {
        case BESTUTM:	
            dt = update_dt(msg.stamp, &t_old);
            utmCallback(msg,bodyStateVector,P);
            break;

        case IMG:	
            imgCallback(&msg);
            break;

        case INSPVAS:	
            dt = update_dt(msg.stamp, &t_old);
            pvaCallback(msg, bodyStateVector, qbw);
            //std::cout << bodyStateVector.transpose() << std::endl;
            if (seenutm)
                printf("%d,%c,%f,%f,%f,%f,%f,%f,%f,%f,%f\n", msg.utm.zone_i,msg.utm.zone_c,
                        bodyStateVector[0],bodyStateVector[1],-bodyStateVector[2],
                        bodyStateVector[3],bodyStateVector[4],bodyStateVector[5],
                        bodyStateVector[6],bodyStateVector[7],bodyStateVector[8]);
            break;

        case RAWIMUS:	
            dt = update_dt(msg.stamp, &t_old);
            imuCallback(msg, bodyStateVector, P, qbw, dt);
            break;

        case INSCOVS:
            covCallback(msg, P, qbw);

        default:	
            break;
    }				/* -----  end switch  ----- */
}
#endif     /* -----  not USE_ROS  ----- */
	return 0;
}