//
// Created by Podmogilnyi Ivan on 17.08.2022.
//

// C++ std library
#include <fstream>
#include <filesystem>

// ROS library
#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/Image.h>
#include <visualization_msgs/Marker.h>
#include <sensor_msgs/CameraInfo.h>
#include <geometry_msgs/TransformStamped.h>
#include <nav_msgs/Path.h>


// open_vis library
#include <utils/quat_ops.h>

// thirdparty libraries
#include <Eigen/Core>


float yCubesVisPos = -2.0;
float skew = 5.0;
std::vector<std::vector<double>> cubeVisPoints = {
        {71.215, 32.694, yCubesVisPos+2.0},
//            {81.668, 26.227, yCubesVisPos},
        {104.061, 36.691, yCubesVisPos+1.5},
//            {90.726, 32.825, yCubesVisPos},
        {109.541, 60.415, yCubesVisPos+1.0},
//            {149.264, 69.490, yCubesVisPos},
        {183.605, 91.474, yCubesVisPos+0.5},
//            {198.294, 97.113, yCubesVisPos},
        {251.391, 154.108, yCubesVisPos-1.0},
//            {310.030, 190.208, yCubesVisPos},
        {330.554, 183.244, yCubesVisPos-2.0},
        {746.086, 450.563, yCubesVisPos-2.0},
//            {792.608, 527.140, yCubesVisPos},
        {883.669, 517.444, yCubesVisPos-2.0},
        {957.811, 652.638, yCubesVisPos-2.0},
        {1086.045, 669.317, yCubesVisPos-2.0},
        {1125.310, 728.214, yCubesVisPos-2.0},
        {731.996, 425.021, yCubesVisPos-2.0},

        {1186.527, 700.189, yCubesVisPos},
//            {1205.432, 681.457, yCubesVisPos},
        {1184.375, 735.604, yCubesVisPos},
//            {1232.697, 715.647, yCubesVisPos},
        {1244.767, 784.257, yCubesVisPos},

        // CUBES FOR THE GPS ALIGNED TRAJECTORY
        {-5.473, 15.819, yCubesVisPos},

        {-15.586, 25.062, yCubesVisPos},

        {-22.395, 43.562, yCubesVisPos},

        {-36.035, 50.009, yCubesVisPos},

        {-41.113, 62.946, yCubesVisPos},

        {-47.675, 62.477, yCubesVisPos},

        {-56.081, 76.612, yCubesVisPos},

        {-68.155, 82.382, yCubesVisPos},

        {-76.903, 97.032, yCubesVisPos},

        {-93.218, 103.532, yCubesVisPos},

        {-121.903, 137.353, yCubesVisPos},

        {-134.567, 140.840, yCubesVisPos},

        {-158.030, 169.709, yCubesVisPos},

        {-170.521, 177.370, yCubesVisPos},

        {-225.440, 236.153, yCubesVisPos},

        {-259.810, 259.846, yCubesVisPos},

        {-287.220, 297.381, yCubesVisPos},

        {-313.876, 312.375, yCubesVisPos},

        {-446.905, 450.175, yCubesVisPos},

        {-489.056, 477.187, yCubesVisPos},

        {-544.828, 555.099, yCubesVisPos},

        {-568.349, 556.457, yCubesVisPos},

        {-638.075, 643.542, yCubesVisPos},

        {-702.377, 676.867, yCubesVisPos},

        {-753.507, 750.003, yCubesVisPos},

        {-807.197, 768.580, yCubesVisPos},

        {-863.780, 829.878, yCubesVisPos},

        {-877.002, 860.185, yCubesVisPos},

        {-905.568, 864.266, yCubesVisPos},

        {-915.407, 887.186, yCubesVisPos},

        {-931.954, 899.596, yCubesVisPos},

        {-936.091, 907.870, yCubesVisPos},

        {-971.656, 952.685, yCubesVisPos},

        {-1012.476, 947.723, yCubesVisPos},


        {-1016.629, 985.880, yCubesVisPos},

        {-1065.223, 987.986, yCubesVisPos},

        {-1078.723, 1012.979, yCubesVisPos},

        {-1145.907, 1035.300, yCubesVisPos},

        {-1234.198, 1116.285, yCubesVisPos},

        {-1338.651, 1152.282, yCubesVisPos},

        {-1422.556, 1202.064, yCubesVisPos},

        {-1488.881, 1208.139, yCubesVisPos},

        {-1613.822, 1281.229, yCubesVisPos},

        {-1682.561, 1274.593, yCubesVisPos},

        {-1927.827, 1384.315, yCubesVisPos},

        {-2035.692, 1400.027, yCubesVisPos},

        {-2224.536, 1462.204, yCubesVisPos},

        {-2313.168, 1458.187, yCubesVisPos},

        {-2436.409, 1496.333, yCubesVisPos},

        {-2503.565, 1473.606, yCubesVisPos},

        {-2605.994, 1481.901, yCubesVisPos},

        {-2563.683, 1431.523, yCubesVisPos},

        {-2599.753, 1395.453, yCubesVisPos},

        {-2679.961, 1407.933, yCubesVisPos},

        {-2630.649, 1438.753, yCubesVisPos},

        {-2581.338, 1506.557, yCubesVisPos},

        {-2528.483, 1197.294, yCubesVisPos},

        {-2553.878, 1178.248, yCubesVisPos},

        {-2553.878, 1108.412, yCubesVisPos},

        {-2450.511, 1023.108, yCubesVisPos},

        {-2450.511, 967.579, yCubesVisPos},

        {-2437.486, 890.971, yCubesVisPos},

        {-2450.511, 819.504, yCubesVisPos},

        {-2365.134, 690.688, yCubesVisPos},

        {-2296.590, 541.293, yCubesVisPos},

        {-2283.679, 620.835, yCubesVisPos},

        {-2151.879, 318.478, yCubesVisPos},

        {-2176.409, 287.816, yCubesVisPos},

        {-2102.820, 251.022, yCubesVisPos},

        {-2127.350, 189.698, yCubesVisPos},

        {-2028.905, 122.896, yCubesVisPos},

        {-2046.821, 66.008, yCubesVisPos},

        {-1958.025, 1.247, yCubesVisPos},

        {-1886.644, -52.849, yCubesVisPos},

        {-1863.596, -64.373, yCubesVisPos},

        {-1783.239, -134.282, yCubesVisPos},

        {-1705.906, -151.254, yCubesVisPos},

        {-1617.356, -206.598, yCubesVisPos},

        {-1565.032, -222.410, yCubesVisPos},

        {-1465.740, -282.723, yCubesVisPos},

        {-1361.237, -309.086, yCubesVisPos},

        {-1276.829, -355.967, yCubesVisPos},

        {-1236.832, -350.967, yCubesVisPos},

        {-1170.082, -368.522, yCubesVisPos},

        {-1145.877, -386.675, yCubesVisPos},

        {-1133.776, -374.573, yCubesVisPos},

        {-1064.985, -381.088, yCubesVisPos},

        {-1060.602, -409.499, yCubesVisPos},

        {-943.535, -368.153, yCubesVisPos},

        {-852.780, -365.980, yCubesVisPos},

        {-747.917, -375.279, yCubesVisPos},

        {-694.244, -346.169, yCubesVisPos},

        {-724.264, -360.724, yCubesVisPos},

        {-601.465, -334.072, yCubesVisPos},

        {-557.205, -336.465, yCubesVisPos},

        {-564.189, -332.895, yCubesVisPos},

        {-520.468, -340.201, yCubesVisPos},

        {-476.908, -317.140, yCubesVisPos},

        {-417.040, -318.781, yCubesVisPos},

        {-377.842, -296.022, yCubesVisPos},

        {-309.273, -302.223, yCubesVisPos},

        {-267.572, -281.072, yCubesVisPos},

        {-233.495, -287.929, yCubesVisPos},

        {-211.795, -257.754, yCubesVisPos},

        {-209.412, -234.544, yCubesVisPos},

        {-144.978, -252.842, yCubesVisPos},

        {-86.550, -255.100, yCubesVisPos},

        {-57.889, -200.404, yCubesVisPos},

        {-12.395, -168.373, yCubesVisPos},

        {-8.988, -117.709, yCubesVisPos},

        {-5.494, -73.048, yCubesVisPos},

        {12.541, -24.783, yCubesVisPos},

        {-8.001, 22.391, yCubesVisPos},

        {-7.999, -85.193, yCubesVisPos},

        {-61.402, 98.350, yCubesVisPos}

};
std::vector<std::vector<double>> cashedCubeVisPoints;

ros::Publisher pub_cubes, pub_cubes_gps, pub_aligned_traj, pub_sensor_left_gps;
ros::Subscriber sub_imu, subs_T_align, sub_stereo_left;
visualization_msgs::Marker Marker, Marker_gps;
std::ofstream fT_aligned;
nav_msgs::Path curr_path;

void callback_imu(const sensor_msgs::Imu::ConstPtr &msg){
    ROS_INFO_STREAM("received imu");
    if (pub_cubes.getNumSubscribers() != 0 || pub_cubes_gps.getNumSubscribers() != 0){
        pub_cubes_gps.publish(Marker_gps);
        pub_cubes.publish(Marker);
    }
}

void T_align_callback(const geometry_msgs::TransformStamped &msg){
    // the main thing
    const uint pointsLen = cubeVisPoints.size();
    std::cout << "1" << std::endl;
    Eigen::MatrixXd points(pointsLen, 4);
    Eigen::Matrix4d T_align;

    Eigen::Vector4d qRot;
    qRot << msg.transform.rotation.x, msg.transform.rotation.y, msg.transform.rotation.z, msg.transform.rotation.w;
    Eigen::Matrix3d mRot = ov_core::quat_2_Rot(qRot);

    for (int i=0;i<3;i++){
        for (int j=0;j<3;j++){
            T_align(i,j) = mRot(i,j);
        }
    }
    std::cout << "2" << std::endl;
    T_align(0,3) = msg.transform.translation.x;
    T_align(1,3) = msg.transform.translation.y;
    T_align(2,3) = msg.transform.translation.z;
    T_align(3,3) = 1;
    std::cout << "3" << std::endl;

    for (int i=0;i<cubeVisPoints.size();i++){
        points(i, 0) = cashedCubeVisPoints[0][0];
        points(i, 1) = cashedCubeVisPoints[0][1];
        points(i, 2) = cashedCubeVisPoints[0][2];
        points(i, 3) = 1;
    }
    std::cout << "4" << std::endl;
    std::cout << "Extracted T_align: " << T_align << std::endl;
    std::stringstream T_align_output;
    T_align_output << T_align << std::endl;
    ROS_INFO(T_align_output.str().c_str());
    fT_aligned << msg.header.stamp.toSec() << std::endl << T_align_output.str().c_str() << std::endl;

    points = (T_align * points.transpose()).transpose();
    for (int i=0;i<cubeVisPoints.size();i++){
        cubeVisPoints[0][0] = points(i, 0);
        cubeVisPoints[0][1] = points(i, 1);
        cubeVisPoints[0][2] = points(i, 2);
    }
    std::cout << "5" << std::endl;

    // Align the trajectory and publish to see the visualization of obtained R and t.
    Eigen::MatrixXd poses(curr_path.poses.size(), 4);
    // extract
    for (int i=0;i<curr_path.poses.size();i++){
        poses(i, 0) = curr_path.poses[i].pose.position.x;
        poses(i, 1) = curr_path.poses[i].pose.position.y;
        poses(i, 2) = curr_path.poses[i].pose.position.z;
    }
    // align
    auto aligned_poses = (T_align * poses.transpose()).transpose();

    // fill again
    for (int i=0;i<curr_path.poses.size();i++){
        curr_path.poses[i].pose.position.x = poses(i, 0);
        curr_path.poses[i].pose.position.y = poses(i, 1);
        curr_path.poses[i].pose.position.z = poses(i, 2);
    }

    // publish
    pub_aligned_traj.publish(curr_path);

}

//void callback_stereo_left_gps(sensor_msgs::ImagePtr msg){
//    auto new_msg = sensor_msgs::ImagePtr();
//    new_msg = msg;
////    new_msg->header.stamp = ros::Time(0.0);
//    new_msg->header.frame_id = "cam0_gps";
//    pub_sensor_left_gps.publish(new_msg);
//}

int main(int argc, char* argv[]){
    ros::init(argc, argv, "render_cubes");

    ros::NodeHandle nh;

    fT_aligned.open("T_aligned_history.txt");
    if (!fT_aligned.is_open()){
        ROS_INFO("error opening file!");
    }
    else{
        ROS_INFO("file open success!");
        std::string curr_path = std::filesystem::current_path();
        std::string output_str = "current path is: " + curr_path;
        ROS_INFO(output_str.c_str());

    }

    // the idea: set the subscriber for IMU, and every time the IMU measurement comes, publish
    // the cubes because the images are also published when the IMU measurement arrives.
    sub_imu = nh.subscribe("/imu/data_raw", 1000, &callback_imu);
    pub_cubes = nh.advertise<visualization_msgs::Marker>("/ov_msckf/vio_vis_markers", 0);
    pub_cubes_gps = nh.advertise<visualization_msgs::Marker>("/ov_msckf/vis_markers", 0);
    pub_aligned_traj = nh.advertise<nav_msgs::Path>("/ov_msckf/pathaligned", 2);

    // a lame fix: create the subscirber here which sends the message from /stereo/left/image_raw to /global_fusion_node/sensor_left
//    sub_stereo_left = nh.subscribe("/stereo/left/image_raw", 1000, &callback_stereo_left_gps);
//    pub_sensor_left_gps = nh.advertise<sensor_msgs::Image>("/global_fusion_node/sensor_left", 2);

    //// UNCOMMENT THIS TO RESTORE THE T_align_callback
//    subs_T_align = nh.subscribe("/ov_msckf/T_align", 1000, T_align_callback);

    Marker.header.frame_id = "gps_global";
    Marker.header.stamp = ros::Time();
    Marker.ns = "my_namespace";
    Marker.id = 0;
    Marker.type = visualization_msgs::Marker::CUBE_LIST;
    Marker.action = visualization_msgs::Marker::ADD;
    Marker.pose.orientation.x = 0.0;
    Marker.pose.orientation.y = 0.0;
    Marker.pose.orientation.z = 0.0;
    Marker.pose.orientation.w = 1.0;
    Marker.scale.x = 6.8f;
    Marker.scale.y = 6.8f;
    Marker.scale.z = 6.8f;
    Marker.color.a = 1.0; // Don't forget to set the alpha!
    Marker.color.r = 0.0;
    Marker.color.g = 1.0;
    Marker.color.b = 0.0;
//only if using a MESH_RESOURCE marker type:
    Marker.mesh_resource = "package://pr2_description/meshes/base_v0/base.dae";

    for (auto & cubeVisPoint : cubeVisPoints){
        geometry_msgs::Point tempPoint;
        tempPoint.x = cubeVisPoint[0];
        tempPoint.y = cubeVisPoint[1];
        tempPoint.z = cubeVisPoint[2];
        Marker.points.push_back(tempPoint);
    }

    Marker_gps.header.frame_id = "global";
    Marker_gps.header.stamp = ros::Time();
    Marker_gps.ns = "gps_namespace";
    Marker_gps.id = 1;
    Marker_gps.type = visualization_msgs::Marker::CUBE_LIST;
    Marker_gps.action = visualization_msgs::Marker::ADD;
    Marker_gps.pose.orientation.x = 0.0;
    Marker_gps.pose.orientation.y = 0.0;
    Marker_gps.pose.orientation.z = 0.0;
    Marker_gps.pose.orientation.w = 1.0;
    Marker_gps.scale.x = 6.8f;
    Marker_gps.scale.y = 6.8f;
    Marker_gps.scale.z = 6.8f;
    Marker_gps.color.a = 1.0; // Don't forget to set the alpha!
    Marker_gps.color.r = 0.0;
    Marker_gps.color.g = 1.0;
    Marker_gps.color.b = 0.0;
//only if using a MESH_RESOURCE marker type:
    Marker.mesh_resource = "package://pr2_description/meshes/base_v0/base.dae";

    for (auto & cubeVisPoint : cubeVisPoints){
        geometry_msgs::Point tempPoint;
        tempPoint.x = cubeVisPoint[0];
        tempPoint.y = cubeVisPoint[1];
        tempPoint.z = cubeVisPoint[2];
        Marker_gps.points.push_back(tempPoint);
    }

    ros::spin();
}