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

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);
    }
}

int main(int argc, char **argv)
{
#ifdef  USE_ROS
    
#else      /* -----  not USE_ROS  ----- */
// Read sensors from file
char line[MAXLINE];
while (scanf("%s", line)!=EOF) {
    message msg;
    parseLine(line, &msg);
    if (msg.msg_type==INSPVAS) {
    }
}
#endif     /* -----  not USE_ROS  ----- */
	return 0;
}