//
// Created by admin1 on 25.08.2022.
//

/*******************************************************
 * Copyright (C) 2019, Aerial Robotics Group, Hong Kong University of Science and Technology
 *
 * This file is part of VINS.
 *
 * Licensed under the GNU General Public License v3.0;
 * you may not use this file except in compliance with the License.
 *
 * Author: Qin Tong (qintonguav@gmail.com)
 *******************************************************/

#include "ros/ros.h"
#include "globalOpt.h"
#include <sensor_msgs/NavSatFix.h>
#include <sensor_msgs/Image.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_broadcaster.h>
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Geometry>
#include <iostream>
#include <stdio.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <fstream>
#include <queue>
#include <mutex>
#include <math.h>

GlobalOptimization globalEstimator;
ros::Publisher pub_global_odometry, pub_global_path, pub_car, pub_imu_tf_global;
nav_msgs::Path *global_path;
// we need to use pointer becuase otherwise TransformBroadcaster will be initializing here, before
// ros::init() called
std::shared_ptr<tf::TransformBroadcaster> TfBr;
double last_vio_t = -1;
std::queue<sensor_msgs::NavSatFixConstPtr> gpsQueue;
std::mutex m_buf;

void publish_car_model(double t, Eigen::Vector3d t_w_car, Eigen::Quaterniond q_w_car)
{
    visualization_msgs::MarkerArray markerArray_msg;
    visualization_msgs::Marker car_mesh;
    car_mesh.header.stamp = ros::Time(t);
    car_mesh.header.frame_id = "gps_global";
    car_mesh.type = visualization_msgs::Marker::MESH_RESOURCE;
    car_mesh.action = visualization_msgs::Marker::ADD;
    car_mesh.id = 0;

    car_mesh.mesh_resource = "package://global_fusion/models/car.dae";

    Eigen::Matrix3d rot;
    rot << 0, 0, -1, 0, -1, 0, -1, 0, 0;

    Eigen::Quaterniond Q;
    Q = q_w_car * rot;
    car_mesh.pose.position.x    = t_w_car.x();
    car_mesh.pose.position.y    = t_w_car.y();
    car_mesh.pose.position.z    = t_w_car.z();
    car_mesh.pose.orientation.w = Q.w();
    car_mesh.pose.orientation.x = Q.x();
    car_mesh.pose.orientation.y = Q.y();
    car_mesh.pose.orientation.z = Q.z();

    car_mesh.color.a = 1.0;
    car_mesh.color.r = 1.0;
    car_mesh.color.g = 0.0;
    car_mesh.color.b = 0.0;

    float major_scale = 2.0;

    car_mesh.scale.x = major_scale;
    car_mesh.scale.y = major_scale;
    car_mesh.scale.z = major_scale;
    markerArray_msg.markers.push_back(car_mesh);
    pub_car.publish(markerArray_msg);
}

void GPS_callback(const sensor_msgs::NavSatFixConstPtr &GPS_msg)
{
    //printf("gps_callback! \n");
    m_buf.lock();
    gpsQueue.push(GPS_msg);
    m_buf.unlock();
}

void vio_callback(const nav_msgs::Odometry::ConstPtr &pose_msg)
{
    //printf("vio_callback! \n");
    double t = pose_msg->header.stamp.toSec();
    last_vio_t = t;
    Eigen::Vector3d vio_t(pose_msg->pose.pose.position.x, pose_msg->pose.pose.position.y, pose_msg->pose.pose.position.z);
    Eigen::Quaterniond vio_q;
    vio_q.w() = pose_msg->pose.pose.orientation.w;
    vio_q.x() = pose_msg->pose.pose.orientation.x;
    vio_q.y() = pose_msg->pose.pose.orientation.y;
    vio_q.z() = pose_msg->pose.pose.orientation.z;

//    auto cov = pose_msg->pose.covariance;

//    auto rCovArr = pose_msg->twist.covariance;
//    auto tCovArr = pose_msg->pose.covariance;
    // convert boost::array to std::array
//    std::vector<double> transCov(9);
//    std::vector<double> rotCov(9);
//    std::vector<double> tCov(tCovArr.size());

    // Presuming that the first 3 block shows the translation cov, and the rest shows rotation cov
//    for (int r = 0; r < 3; r++){
//        for (int c = 0; c < 3; c++){
//            transCov[r*3 + c] = cov[r*6 + c];
//        }
//    }
//
//    for (int r = 3; r < 6; r++){
//        for (int c = 3; c < 6; c++){
//            rotCov[(r-3)*3 + (c-3)] = cov[r*6 + c];
//        }
//    }

//    std::cout << "The obtained below matrix from cov" << std::endl;
//    // debugging. Output the below matrix
//    for (int r = 0; r < 3; r++){
//        for (int c = 0; c < 3; c++){
//            std::cout << rotCov[r*3 + c] << " ";
//        }
//        std::cout << std::endl;
//    }

//    ROS_INFO_STREAM("the filled vector size" << rCov.size() << " the filled vector first element: " << rCov[0]);
    globalEstimator.inputOdom(t, vio_t, vio_q);


    m_buf.lock();
    while(!gpsQueue.empty())
    {
        sensor_msgs::NavSatFixConstPtr GPS_msg = gpsQueue.front();
        double gps_t = GPS_msg->header.stamp.toSec();
        printf("vio t: %f, gps t: %f \n", t, gps_t);
        // 10ms sync tolerance
        // PI: Let's change the sync tolerance from 10 ms to 100 ms
//        if(gps_t >= t - 0.01 && gps_t <= t + 0.01)
        if(gps_t >= t - 0.1 && gps_t <= t + 0.1)
        {
            //printf("receive GPS with timestamp %f\n", GPS_msg->header.stamp.toSec());
            double latitude = GPS_msg->latitude;
            double longitude = GPS_msg->longitude;
            double altitude = GPS_msg->altitude;
            //int numSats = GPS_msg->status.service;
            // it was made for KITTI dataset before, which only gives the one parameter. But here we have 3 variances
            // (the other values in the covariance matrix are 0).
            double pos_accuracy;
            pos_accuracy = std::sqrt( pow(GPS_msg->position_covariance[0], 2.0) + pow(GPS_msg->position_covariance[4], 2) + pow(GPS_msg->position_covariance[8], 2) );
            if(pos_accuracy <= 0)
                pos_accuracy = 1;
            //printf("receive covariance %lf \n", pos_accuracy);
            //if(GPS_msg->status.status > 8)
            globalEstimator.inputGPS(t, latitude, longitude, altitude, pos_accuracy);
            gpsQueue.pop();
            break;
        }
        else if(gps_t < t - 0.01)
            gpsQueue.pop();
        else if(gps_t > t + 0.01)
            break;
    }
    m_buf.unlock();

    Eigen::Vector3d global_t;
    Eigen:: Quaterniond global_q;
    globalEstimator.getGlobalOdom(global_t, global_q);

    nav_msgs::Odometry odometry;
    odometry.header = pose_msg->header;
    odometry.header.frame_id = "gps_global";
    odometry.pose.pose.position.x = global_t.x();
    odometry.pose.pose.position.y = global_t.y();
    odometry.pose.pose.position.z = global_t.z();
    odometry.pose.pose.orientation.x = global_q.x();
    odometry.pose.pose.orientation.y = global_q.y();
    odometry.pose.pose.orientation.z = global_q.z();
    odometry.pose.pose.orientation.w = global_q.w();
    pub_global_odometry.publish(odometry);
    pub_global_path.publish(*global_path);
    publish_car_model(t, global_t, global_q);

    // write result to file
    std::ofstream foutC("/home/admin1/workspace/catkin_ws_ov/src/open_vins/global_fusion/data/vio_global.csv", ios::app);
    foutC.setf(ios::fixed, ios::floatfield);
    foutC.precision(0);
    foutC << pose_msg->header.stamp.toSec() * 1e9 << ",";
    foutC.precision(5);
    foutC << global_t.x() << ","
          << global_t.y() << ","
          << global_t.z() << ","
          << global_q.w() << ","
          << global_q.x() << ","
          << global_q.y() << ","
          << global_q.z() << endl;
    foutC.close();
}

void gps_img_callback(sensor_msgs::ImageConstPtr msg){
    Eigen::Vector3d global_t;
    Eigen:: Quaterniond global_q;
    globalEstimator.getGlobalOdom(global_t, global_q);

    // Publish the tf so that Camera can bind to it.
    tf::StampedTransform trans;
    trans.frame_id_ = "gps_global";
    trans.child_frame_id_ = "imu_gps";
    trans.stamp_ = msg->header.stamp;
    tf::Quaternion quat(global_q.x(), global_q.y(), global_q.z(), global_q.w());
    trans.setRotation(quat);
    tf::Vector3 orig(global_t.x(), global_t.y(), global_t.z());
    trans.setOrigin(orig);
    TfBr->sendTransform(trans);
//    pub_imu_tf_global.publish(trans);
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "globalEstimator");
    ros::NodeHandle n("~");

    global_path = &globalEstimator.global_path;
    TfBr = std::make_shared<tf::TransformBroadcaster>();

    ros::Subscriber sub_GPS = n.subscribe("/gps", 100, GPS_callback);
    ros::Subscriber sub_vio = n.subscribe("/vins_estimator/odometry", 100, vio_callback);
    ros::Subscriber sub_img = n.subscribe("/global_fusion_node/sensor_left", 100, gps_img_callback);
    pub_global_path = n.advertise<nav_msgs::Path>("global_path", 100);
    pub_global_odometry = n.advertise<nav_msgs::Odometry>("global_odometry", 100);
//    pub_imu_tf_global = n.advertise<tf::StampedTransform>("tf_imu_global", 100);
    pub_car = n.advertise<visualization_msgs::MarkerArray>("car_model", 1000);
    ros::spin();
    return 0;
}