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

#ifdef  USE_ROS

#else      /* -----  not USE_ROS  ----- */
void
imgCallback(const message *msg)
{
    return;
}
void 
imuCallback(const message &msg, Eigen::Matrix<double,9,1> &X, const Eigen::Quaternion<double> &q, const timestamp dt)
{
    if (!seenutm || !seenpva || (dt.secs==0 && dt.nsecs==0)) return;
    using Eigen::Vector3d;
    Vector3d acc(msg.linear_acceleration.x,msg.linear_acceleration.y,msg.linear_acceleration.z);
    Vector3d ang(msg.angular_velocity.x,msg.angular_velocity.y,msg.angular_velocity.z);
    Body mu;
    double dtf = dt.secs+dt.nsecs*1e-9;
    mu.motionModel(X,acc,ang,q,dtf);
}

void
pvaCallback(const message &msg, Eigen::Matrix<double,9,1> &X, Eigen::Quaternion<double> &q)
{
    using Eigen::AngleAxisd;
    using Eigen::Vector3d;
    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*(90.-msg.attitude.yaw)/180.;
    Eigen::Matrix3d R;
    R = AngleAxisd(yaw, Vector3d::UnitZ())
      * AngleAxisd(roll, Vector3d::UnitY())
      *  AngleAxisd(pitch, Vector3d::UnitX());
    q = Eigen::Quaternion<double>(R);
    if (!seenpva) {
        Vector3d vw(msg.velocity.east,msg.velocity.north,msg.velocity.up);
        X.segment(3,3) = R.transpose()*vw;
        seenpva=true;
    }
    return;
}

void
utmCallback(const message &msg, Eigen::Matrix<double,9,1> &X)
{
    if (!seenutm) {
        seenutm=true;
        X[0] = msg.utm.easting;
        X[1] = msg.utm.northing;
        X[2] = msg.utm.up;
    }
    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")) {
        data->msg_type = INSCOVS;
    } 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)
{
    Eigen::Quaternion<double> qbw;
    Eigen::Matrix<double,9,1> bodyStateVector;
    bodyStateVector.segment(6,3) << 0.01465135, -0.03713532, -0.00709229;
    timestamp dt{0,0};
    timestamp t_old{0,0};

#ifdef  USE_ROS
    
#else      /* -----  not USE_ROS  ----- */
// 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);
            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\n", msg.utm.zone_i,msg.utm.zone_c,bodyStateVector[1],bodyStateVector[0]);
            break;

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

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