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

double feature_noise = 3.5;
double view_noise = 0.0006;
double initial_view_noise = 0.0001;
double reflection_view_noise = 0.0006;

double r_height = 0.2025;
double acc_std = 0.002835;
double ang_std = 0.008019;
double acc_bias_std = 0.00891;

double canoecenter = 0.62;
double canoeheight = -0.46;

double ransac_li_threshold = 1.8e-6;
double ransac_hi_threshold = 0.25;
double covbias = 2e-5;

double angbiasx =  -0.0020490;
double angbiasy =  0.0050130;
double angbiasz = 0.0019250;
double accbiasx = -0.007870;
double accbiasy = 0.0179085;
double accbiasz = -0.006310;

double xcorrsrc = 0.9;
double xcorrref = 0.4;

bool donew=true;
bool domono=false;

using std::cout;
using std::endl;
using std::cerr;

Matrix<double,4,3> attitude_jacobian(double roll, double pitch, double yaw)
{
    roll /= 2.;
    pitch /= 2.;
    yaw /= 2.;
    double sphi, cphi, sthe, cthe, spsi, cpsi;

    sphi = sin(roll);
    cphi = cos(roll);

    sthe = sin(pitch);
    cthe = cos(pitch);

    spsi = sin(yaw);
    cpsi = cos(yaw);
    
    double dqxdphi, dqxdthe, dqxdpsi;
    double dqydphi, dqydthe, dqydpsi;
    double dqzdphi, dqzdthe, dqzdpsi;
    double dqwdphi, dqwdthe, dqwdpsi;

    dqxdphi = 0.5*(cphi*cthe*cpsi+sphi*sthe*spsi);
    dqxdthe = 0.5*(-sphi*sthe*cpsi-cphi*cthe*spsi);
    dqxdpsi = 0.5*(-sphi*cthe*spsi-cphi*sthe*cpsi);

    dqydphi = 0.5*(-sphi*sthe*cpsi+cphi*cthe*spsi);
    dqydthe = 0.5*(cphi*cthe*cpsi-sphi*sthe*spsi);
    dqydpsi = 0.5*(-cphi*sthe*spsi+sphi*cthe*cpsi);

    dqzdphi = 0.5*(-sphi*cthe*spsi-cphi*sthe*cpsi);
    dqzdthe = 0.5*(-cphi*sthe*spsi-sphi*cthe*cpsi);
    dqzdpsi = 0.5*(cphi*cthe*cpsi-sphi*sthe*spsi);

    dqwdphi = 0.5*(-sphi*cthe*cpsi+cphi*sthe*spsi);
    dqwdthe = 0.5*(-cphi*sthe*cpsi+sphi*cthe*spsi);
    dqwdpsi = 0.5*(-cphi*cthe*spsi+sphi*sthe*cpsi);

    Matrix<double,4,3> J;
    J << dqxdphi, dqxdthe, dqxdpsi,
         dqydphi, dqydthe, dqydpsi,
         dqzdphi, dqzdthe, dqzdpsi,
         dqwdphi, dqwdthe, dqwdpsi;

    return J;
}

double 
aboveWater(const Quaterniond &q)
{
    Vector3d tip;
#ifdef  HEIGHT_FROM_ATTITUDE
    tip << canoecenter*3.43, 0, canoeheight;
    tip = q._transformVector(tip);
#else      /* -----  not HEIGHT_FROM_ATTITUDE  ----- */
    tip[2] = canoeheight;
#endif     /* -----  not HEIGHT_FROM_ATTITUDE  ----- */
    return tip[2];
}

/* 
 * ===  FUNCTION  ======================================================================
 *         Name:  covCallback
 *  Description:  Initializes position and velocity covariance in the proper
 *  coordinate frames. Rbw is the body to NED rotation. Renuned is the ENU->NED
 *  rotation. This is only set during the initialization phase and not during
 *  normal running.
 * =====================================================================================
 */
void
#if STATESIZE==13
covCallback(const message &msg, State &mu, const attitude_t &att)
#else
covCallback(const message &msg, State &mu, const Quaterniond &q)
#endif
{
    if (seencov && seenutm && seenpva) return;
    if (!seenpva) return;
    seencov=true;
    // Rotation from body to world
#if STATESIZE==13
    Quaterniond q = mu.qhat();
#endif
    Matrix3d Rbw(q.toRotationMatrix());
    // Rotation from ENU to NED
    Matrix3d Renuned;
    Renuned << 0.,1.,0.,1.,0.,0.,0.,0.,-1;
    Matrix3d pcov = Renuned*msg.covariance.position*Renuned.transpose();
    pcov(2,2) *= 10;
    //mu.position_covariance(pcov);
    mu.velocity_covariance(Rbw.transpose()*msg.covariance.velocity*Rbw);
#if STATESIZE==13
    Matrix3d Patt;
    Patt = msg.covariance.attitude;
    // Convert deg^2 to rad^2
    Patt *= M_PI*M_PI;
    Patt /= (180.*180.);
    // Swap values for roll, pitch, yaw ordering
    Patt(0,0) = msg.covariance.attitude(1,1);
    Patt(1,1) = msg.covariance.attitude(0,0);

    Patt(2,0) = msg.covariance.attitude(2,1);
    Patt(2,1) = msg.covariance.attitude(2,0);

    Patt(0,2) = msg.covariance.attitude(1,2);
    Patt(1,2) = msg.covariance.attitude(0,2);

    Matrix4d Pquat;
    Matrix<double,4,3> Jatt;
    Jatt = attitude_jacobian(att.roll, att.pitch, att.yaw);
    Pquat = Jatt*Patt*Jatt.transpose();
    mu.quaternion_covariance(Pquat);
#endif
}


/* 
 * ===  FUNCTION  ======================================================================
 *         Name:  imgCallback
 *  Description:  Handles image measurements and update the filter. Only runs
 *  after the initialization phase.
 * =====================================================================================
 */
void
#if STATESIZE==13
imgCallback(message &msg, State &mu, Camera &cam)
#else
imgCallback(message &msg, State &mu, Camera &cam, const Quaterniond &q)
#endif
{
    if (seenutm && seenpva && seencov) {
#if STATESIZE==13
        Quaterniond q = mu.qhat();
#endif
        std::vector<measurement_t> z;
#ifdef MEASURE_HEIGHT
        measurement_t height;
        height.z_type = HEIGHT;
        height.height = aboveWater(q);
        z.push_back(height);
#endif
        Vision viz;
        viz.open(msg.image_names[0],cam);

#if STATESIZE==13
        mu.doMeasurements(z,viz,cam);
#else
        mu.doMeasurements(z,viz,q,cam);
#endif
    }

    return;
}

/* 
 * ===  FUNCTION  ======================================================================
 *         Name:  imuCallback
 *  Description:  Only runs after the initialization is complete. Updates the
 *  motion model, P_k|k-1, and removes features that leave the FOV.
 * =====================================================================================
 */
void 
#if STATESIZE==13
imuCallback(const message &msg, State &mu, const timestamp dt)
#else
imuCallback(const message &msg, State &mu, const Quaterniond &q, const timestamp dt)
#endif
{
    if (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(angbiasx, angbiasy, angbiasz);
    ang -= ang_bias;
    double dtf = dt.secs+dt.nsecs*1e-9;

#if STATESIZE==13
    mu.motionModel(acc,ang,dtf);
#else
    mu.motionModel(acc,ang,q,dtf);
#endif
}

/* 
 * ===  FUNCTION  ======================================================================
 *         Name:  pvaCallback
 *  Description:  During the initialization phase, updates the velocity and
 *  quaternion. Afterwards, updates only the quaternion.
 * =====================================================================================
 */
void
pvaCallback(const message &msg, State &mu, Quaterniond &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 = Quaterniond(R);
    //cout <<q.x() <<" " << q.y() << " " << q.z() << " " << q.w() << endl;

    // Set body velocity
    if (!seenpva || !seencov || !seenutm) {
        Vector3d vw(msg.velocity.north,msg.velocity.east,-msg.velocity.up);
        mu.vel(R.transpose()*vw);
#if STATESIZE==13
        mu.qhat(q);
#endif
        seenpva=true;
    } 
#if STATESIZE==13
        //mu.qhat(q);
#endif
    return;
}

/* 
 * ===  FUNCTION  ======================================================================
 *         Name:  utmCallback
 *  Description:  During the initialization, updates the position. Does nothing
 *  during normal running.
 * =====================================================================================
 */
void
#if STATESIZE==13
utmCallback(const message &msg, State &mu)
#else
utmCallback(const message &msg, State &mu, const Quaterniond &q)
#endif
{
#if STATESIZE==13
    Quaterniond q = mu.qhat();
#endif
    static int i=0;
    if ((!seenutm || !seencov) && seenpva) {
        seenutm=true;
        UTM utm_water;
        utm_water.northing = msg.utm.northing;
        utm_water.easting = msg.utm.easting;
        utm_water.up = -aboveWater(q);
        utm_water.zone_i = msg.utm.zone_i;
        utm_water.zone_c = msg.utm.zone_c;
        mu.pos(utm_water);
    } else {
        i+=1;
        //if ( i==20) {
        //if (i%18==0) {
        if (false) {
            UTM utm_water;
            utm_water.northing = msg.utm.northing;
            utm_water.easting = msg.utm.easting;
            utm_water.up = -aboveWater(q);
            utm_water.zone_i = msg.utm.zone_i;
            utm_water.zone_c = msg.utm.zone_c;
            mu.pos(utm_water);
        }
    }
    return;
}

/* 
 * ===  FUNCTION  ======================================================================
 *         Name:  parseLine
 *  Description:  Parses a line from regular file input. Gets the time,
 *  determines the message type and fills in the corresponding data fields.
 * =====================================================================================
 */
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;
        lfn = strsep(&line, ",");
        sscanf(lfn, "%s", (char *) &data->image_names);
        sscanf(line, "%s", (char *) &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);
    }
    return 0;
}

/* 
 * ===  FUNCTION  ======================================================================
 *         Name:  update_dt
 *  Description:  Determines dt based on the current and previous timestamp.
 * =====================================================================================
 */
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)
{
    if (argc<3) {
        fprintf(stderr, "Usage: %s camera params [donew]\n", argv[0]);
        exit(1);
    }
    State mu;
    Quaterniond qbw;
    attitude_t att;
    // bias in FRD coordinates
    timestamp dt{0,0};
    timestamp t_old{0,0};

    // Open camera YAML
    Camera cam(argv[1]);

    // Print unicsv header
#ifdef FEATUREMAP
    printf("Name,");
#endif
#if STATESIZE==13
    printf("GPS-stamp,UTM-Zone,UTM-Ch,UTM-North,UTM-East,Altitude,Vel-X,Vel-Y,Vel-Z,Quat-X,Quat-Y,Quat-Z,Quat-W,Bias-X,Bias-Y,Bias-Z,Nfeats\n");
#else
    printf("UTM-Zone,UTM-Ch,UTM-North,UTM-East,Altitude,Vel-X,Vel-Y,Vel-Z,Bias-X,Bias-Y,Bias-Z,Nfeats\n");
#endif     /* -----  not ROS_PUBLISH  ----- */
    // read params file
    FILE *pfin;
    pfin = fopen(argv[2], "r");
    fscanf(pfin,"%lf",&feature_noise);
    fscanf(pfin,"%lf",&view_noise);
    fscanf(pfin,"%lf",&initial_view_noise);
    fscanf(pfin,"%lf",&reflection_view_noise);
    fscanf(pfin,"%lf",&r_height);
    fscanf(pfin,"%lf",&acc_std);
    fscanf(pfin,"%lf",&ang_std);
    fscanf(pfin,"%lf",&acc_bias_std);
    fscanf(pfin,"%lf",&canoecenter);
    fscanf(pfin,"%lf",&canoeheight);
    fscanf(pfin,"%lf",&ransac_li_threshold);
    fscanf(pfin,"%lf",&ransac_hi_threshold);
    fscanf(pfin,"%lf",&covbias);
    fscanf(pfin,"%lf",&xcorrsrc);
    fscanf(pfin,"%lf",&xcorrref);
    fscanf(pfin,"%lf",&angbiasx);
    fscanf(pfin,"%lf",&angbiasy);
    fscanf(pfin,"%lf",&angbiasz);
    fscanf(pfin,"%lf",&accbiasx);
    fscanf(pfin,"%lf",&accbiasy);
    fscanf(pfin,"%lf",&accbiasz);
    fclose(pfin);
    Vector3d acc_bias;
    acc_bias << accbiasx, accbiasy, accbiasz;
    mu.accelerometer_bias(acc_bias);

    // check for donew
    if (argc==4) {
        if (!strcmp(argv[3],"false")) {
            donew=false;
            cerr << "setting old process noise" << endl;
        } else if (!strcmp(argv[3],"mono")) {
            domono=true;
            cerr << "setting all measurements to monocular" << endl;
        }
    }
    // Read sensors from file
    int i=0;
    char line[MAXLINE];
    while (scanf("%s", line)!=EOF) {
        message msg;
        parseLine(line, &msg);
        
        switch ( msg.msg_type ) {
            case BESTUTM:	
#if STATESIZE==13
                utmCallback(msg, mu);
#else
                utmCallback(msg, mu,qbw);
#endif
                //printf("%f,", msg.stamp.secs+msg.stamp.nsecs*1e-9);
                if (seenutm && seenpva && seencov) {
                    mu.unicsv(msg.stamp.secs+msg.stamp.nsecs*1e-9);
                }
                break;

            case IMG:	
#if STATESIZE==13
                imgCallback(msg, mu, cam);
#else
                imgCallback(msg, mu, cam, qbw);
#endif
                break;

            case INSPVAS:	
                att = msg.attitude;
                pvaCallback(msg, mu, qbw);
                break;

            case RAWIMUS:	
                if (i++%DOWNSAMPLE==0) {
                    dt = update_dt(msg.stamp, &t_old);
#if STATESIZE==13
                    imuCallback(msg, mu, dt);
#else
                    imuCallback(msg, mu, qbw, dt);
#endif
                    if (seenutm && seencov && seenpva) {
#ifdef  ROS_PUBLISH
                        geometry_msgs::PoseStamped msg;
                        ros::spinOnce();
#else
#endif
                    }
                }
                break;

            case INSCOVS:
#if STATESIZE==13
                covCallback(msg, mu, att);
#else
                covCallback(msg, mu, qbw);
#endif
                break;

            default:	
                break;
        }				/* -----  end switch  ----- */
    }
	return 0;
}