#include <sl/Camera.hpp>
#include  <sl/Fusion.hpp>
#include <cmath>
#include <nlohmann/json.hpp>

using json = nlohmann::json;
using namespace std;
using namespace sl;

inline bool is_microseconds(uint64_t timestamp) {
    // Check if the timestamp is in microseconds
    return (1'000'000'000'000'000 <= timestamp && timestamp < 10'000'000'000'000'000ULL);
}

inline bool is_nanoseconds(uint64_t timestamp) {
    // Check if the timestamp is in microseconds
    return (1'000'000'000'000'000'000 <= timestamp && timestamp < 10'000'000'000'000'000'000ULL);
}

int main() {

    // Create a ZED camera object
    Camera zed;

    sl::FusedPointCloud finalObj;

    // Set configuration parameters
    InitParameters init_parameters;
    init_parameters.coordinate_system = COORDINATE_SYSTEM::IMAGE; // Use a right-handed Y-up coordinate system
    init_parameters.coordinate_units = UNIT::METER; // Set units in meters
    init_parameters.depth_mode = DEPTH_MODE::ULTRA; // Set units in meters
    init_parameters.input.setFromSVOFile("zed-sdk/global localization/recording/python/output/ZED_SN35169074_25-03-2025_17-08-22.svo2");
    init_parameters.sdk_verbose = 1;

    // Open the camera
    auto returned_state = zed.open(init_parameters);
    if (returned_state != ERROR_CODE::SUCCESS) {
        cout << "Error " << returned_state << ", exit program.\n";
        return EXIT_FAILURE;
    }

    // Enable positional tracking with default parameters. Positional tracking needs to be enabled before using spatial mapping
    sl::PositionalTrackingParameters tracking_parameters;
    tracking_parameters.mode = sl::POSITIONAL_TRACKING_MODE::GEN_2;
    returned_state = zed.enablePositionalTracking(tracking_parameters);
    if (returned_state != ERROR_CODE::SUCCESS) {
        cout << "Error " << returned_state << ", exit program.\n";
        return EXIT_FAILURE;
    }

    auto display_resolution = sl::Resolution(1280, 720);


    sl::Fusion fusion;
    sl::InitFusionParameters init_fusion_param;
    init_fusion_param.coordinate_units = UNIT::METER;
    init_fusion_param.coordinate_system = COORDINATE_SYSTEM::IMAGE;
    init_fusion_param.output_performance_metrics = true;
    init_fusion_param.verbose = true;
    init_fusion_param.timeout_period_number = 66;

    sl::FUSION_ERROR_CODE fusion_init_code = fusion.init(init_fusion_param);
    if (fusion_init_code != sl::FUSION_ERROR_CODE::SUCCESS) {
        std::cerr << "[Fusion][ERROR] Failed to initialize fusion, error: " << fusion_init_code << std::endl;
        return EXIT_FAILURE;
    }
    // Enable odometer publishing:
    sl::CommunicationParameters configuration;
    zed.startPublishing(configuration);
    /// Run a first grab for starting sending data:
    while (zed.grab() != sl::ERROR_CODE::SUCCESS) {
    }

    sl::CameraIdentifier uuid(zed.getCameraInformation().serial_number);
    fusion.subscribe(uuid, configuration, sl::Transform());
    // Enable positional tracking for Fusion object
    sl::PositionalTrackingFusionParameters tracking_fusion_param;
    tracking_fusion_param.enable_GNSS_fusion = true;
    sl::GNSSCalibrationParameters gnss_calibration_parameters;
    gnss_calibration_parameters.target_yaw_uncertainty = 1e-2;
    gnss_calibration_parameters.enable_translation_uncertainty_target = false;
    gnss_calibration_parameters.target_translation_uncertainty = 15e-2;
    gnss_calibration_parameters.enable_reinitialization = false;
    gnss_calibration_parameters.gnss_vio_reinit_threshold = 5;
    gnss_calibration_parameters.enable_rolling_calibration = true;
    gnss_calibration_parameters.gnss_antenna_position = sl::float3 (1.0, 0.0, 0.25);
    tracking_fusion_param.gnss_calibration_parameters = gnss_calibration_parameters;
    if(sl::FUSION_ERROR_CODE tracking_error_code = fusion.enablePositionalTracking(tracking_fusion_param); tracking_error_code != sl::FUSION_ERROR_CODE::SUCCESS){
        std::cout << "[Fusion][ERROR] Could not start tracking, error: " << tracking_error_code << std::endl;
        return EXIT_FAILURE;
    }

    SpatialMappingParameters spatial_mapping_parameters;
    spatial_mapping_parameters.set(SpatialMappingParameters::MAPPING_RANGE::AUTO);
    spatial_mapping_parameters.set(SpatialMappingParameters::MAPPING_RESOLUTION::HIGH);
    spatial_mapping_parameters.max_memory_usage = 2560;
    spatial_mapping_parameters.save_texture = false;
    spatial_mapping_parameters.use_chunk_only = true;
    spatial_mapping_parameters.reverse_vertex_order = false;
    spatial_mapping_parameters.map_type = SpatialMappingParameters::SPATIAL_MAP_TYPE::FUSED_POINT_CLOUD;
    spatial_mapping_parameters.stability_counter = 2;

    FusedPointCloud zed_mesh;
    FusedPointCloud fusion_mesh;

    RuntimeParameters runtime_parameters;
    // Use low depth confidence to avoid introducing noise in the constructed model
    runtime_parameters.measure3D_reference_frame =  REFERENCE_FRAME::WORLD;
    runtime_parameters.enable_depth = true;
    runtime_parameters.confidence_threshold = 50;
    Mat left_image(display_resolution, MAT_TYPE::U8_C4, sl::MEM::GPU);
    Mat point_cloud(display_resolution, MAT_TYPE::F32_C4, sl::MEM::GPU);
    POSITIONAL_TRACKING_STATE tracking_state = POSITIONAL_TRACKING_STATE::OFF;
    Pose pose;

    sl::Timestamp last_timestamp = 0;
    bool is_running = true;

    std::string gnss_key;
    try {
        gnss_key = zed.getSVODataKeys()[0];
    }catch (const std::runtime_error &e) {
        std::cerr << "Error while Locating GNSS Key in SVO File: " << e.what() << std::endl;
    }

    bool mapping_enable = false;
    bool spatial_mapping_fusion_status = false;
    while(is_running) {
        if (zed.grab(runtime_parameters) == ERROR_CODE::SUCCESS) {
            // Retrieve the left image
            zed.retrieveImage(left_image, VIEW::LEFT, MEM::GPU, display_resolution);
            zed.retrieveMeasure(point_cloud, MEASURE::XYZBGRA, MEM::GPU, display_resolution);
            // Retrieve the camera pose data
            tracking_state = zed.getPosition(pose, REFERENCE_FRAME::WORLD);
            if (tracking_state != POSITIONAL_TRACKING_STATE::OK) {
                std::cout << "Waiting for Positional Tracking to Begin" << std::endl;
            }

            auto current_ts = zed.getTimestamp(TIME_REFERENCE::IMAGE);

            // auto status  use status in real problem
            if (gnss_key.empty()) {
                std::cout << "GNSS Key is empty. Killing Program" << std::endl;
                break;
            }
            std::map<sl::Timestamp, sl::SVOData> data_map;
            zed.retrieveSVOData(gnss_key,
                data_map,
                last_timestamp,
                current_ts);

            sl::GNSSData input_gnss;
            for (auto &[fst, snd] : data_map) {
                SVOData& data = snd;  // Accessing the value (SVOData)
                std::string  s;
                bool data_string = data.getContent(s);

                auto tmp_array = json::array();
                try {
                    auto gnss_data_point = json::parse(s);
                    auto gnss_data_point_formatted = json::object();

                    if (!gnss_data_point["coordinates"].is_null()) {
                        gnss_data_point_formatted["coordinates"] = {
                            {"latitude", gnss_data_point["coordinates"]["latitude"].get<float>()},
                            {"longitude", gnss_data_point["coordinates"]["longitude"].get<float>()},
                            {"altitude", gnss_data_point["coordinates"]["altitude"].get<float>()},
                        };
                        gnss_data_point_formatted["ts"] = gnss_data_point["ts"];

                        float latitude_std = gnss_data_point["latitude_std"];
                        float longitude_std = gnss_data_point["longitude_std"];
                        float altitude_std = gnss_data_point["altitude_std"];

                        gnss_data_point_formatted["latitude_std"] = sqrt(latitude_std);
                        gnss_data_point_formatted["longitude_std"] = sqrt(longitude_std);
                        gnss_data_point_formatted["altitude_std"] = sqrt(altitude_std);

                        gnss_data_point_formatted["position_covariance"] = json::array({
                            longitude_std, 0, 0, 0, latitude_std, 0, 0, 0, altitude_std
                        });

                        gnss_data_point_formatted["original_gnss_data"] = gnss_data_point;
                    } else if (!gnss_data_point["coordinates"].is_null() && !gnss_data_point["latitude_std"].is_null() && !gnss_data_point["longitude_std"].is_null()) {
                        // no conversion
                        gnss_data_point_formatted = gnss_data_point;
                    }

                    tmp_array.push_back(gnss_data_point_formatted);
                } catch (const std::runtime_error &e) {
                    std::cerr << "Error while reading GNSS data: " << e.what() << std::endl;
                }

                input_gnss.gnss_mode = GNSS_MODE::FIX_2D;
                input_gnss.gnss_status = GNSS_STATUS::SINGLE;
                input_gnss.longitude_std = tmp_array[0]["longitude_std"];
                input_gnss.latitude_std = tmp_array[0]["latitude_std"];
                input_gnss.altitude_std = tmp_array[0]["altitude_std"];
                input_gnss.position_covariance = tmp_array[0]["position_covariance"];

                if (is_microseconds(tmp_array[0]["ts"].get<uint64_t>())){
                    input_gnss.ts.setMicroseconds(tmp_array[0]["ts"].get<uint64_t>());
                }
                else if (is_nanoseconds(tmp_array[0]["ts"].get<uint64_t>())){
                    input_gnss.ts.setNanoseconds(tmp_array[0]["ts"].get<uint64_t>());
                }
                input_gnss.setCoordinates(tmp_array[0]["coordinates"]["latitude"],
                    tmp_array[0]["coordinates"]["longitude"],
                    tmp_array[0]["coordinates"]["altitude"],
                    false
                    );
                sl::FUSION_ERROR_CODE ingest_status = fusion.ingestGNSSData(input_gnss);
                if (ingest_status != sl::FUSION_ERROR_CODE::SUCCESS and ingest_status != sl::FUSION_ERROR_CODE::NO_NEW_DATA_AVAILABLE) {
                    std::cout << "Ingest error occurred when ingesting GNSSData: " << ingest_status << std::endl;
                }else {
                    std::cout << "Ingest OK" << std::endl;
                    break;
                }
            }
            last_timestamp = current_ts;
            if (fusion.process() == sl::FUSION_ERROR_CODE::SUCCESS){
                if (!mapping_enable) {
                    sl::ERROR_CODE mapping_status = zed.enableSpatialMapping(spatial_mapping_parameters);
                    if (mapping_status == sl::ERROR_CODE::SUCCESS) {
                        mapping_enable = true;
                    }
                }
                if (!spatial_mapping_fusion_status) {
                    sl::SpatialMappingFusionParameters spatial_mapping_fusion_parameters;
                    spatial_mapping_fusion_parameters.set(SpatialMappingParameters::MAPPING_RANGE::AUTO);
                    spatial_mapping_fusion_parameters.set(SpatialMappingParameters::MAPPING_RESOLUTION::HIGH);
                    spatial_mapping_fusion_parameters.max_memory_usage = 2000;
                    spatial_mapping_fusion_parameters.use_chunk_only = true;
                    spatial_mapping_fusion_parameters.map_type = SpatialMappingParameters::SPATIAL_MAP_TYPE::FUSED_POINT_CLOUD;
                    spatial_mapping_fusion_parameters.stability_counter = 2;
                    sl::FUSION_ERROR_CODE fusion_mapping_status = fusion.enableSpatialMapping(spatial_mapping_fusion_parameters);
                    if (fusion_mapping_status == sl::FUSION_ERROR_CODE::SUCCESS) {
                        spatial_mapping_fusion_status = true;
                    }
                }

                sl::Pose fused_pose;
                sl::POSITIONAL_TRACKING_STATE state = fusion.getPosition(fused_pose);
                float yaw_std;
                sl::float3 position_std;
                fusion.getCurrentGNSSCalibrationSTD(yaw_std, position_std);
                if (yaw_std != -1.f) {
                    std::cout << ": calibration uncertainty yaw_std " << yaw_std << " rad position_std " << position_std
                            [0] << " m, " << position_std[1] << " m, " << position_std[2] << " m\t\t\t\r";
                }

                if (state == sl::POSITIONAL_TRACKING_STATE::OK) {
                    sl::GeoPose fused_geo_pose;
                    sl::GNSS_FUSION_STATUS gnss_fusion_status = fusion.getGeoPose(fused_geo_pose);
                    sl::GNSS_FUSION_STATUS geo_fusion_status = fusion.Camera2Geo(fused_pose, fused_geo_pose);
                    // zed.requestSpatialMapAsync();
                    fusion.requestSpatialMapAsync();
                    if (fusion.getSpatialMapRequestStatusAsync() == sl::FUSION_ERROR_CODE::SUCCESS) {
                        fusion.retrieveSpatialMapAsync(fusion_mesh);
                        fusion_mesh.save("test");
                    }
                    // zed.retrieveSpatialMapAsync(zed_mesh);
                    // fusion.getGeoPose();


                    // finalObj.updateFromChunkList(fused_geo_pose.pose_data)

                }
            }
        }
        else if (zed.grab() == ERROR_CODE::END_OF_SVOFILE_REACHED) {
            is_running = false;
            std::cout << "End of file";
            break;
        }
        else {
            is_running = false;
        }
    }
    // zed.extractWholeSpatialMap(zed_mesh);
    // zed_mesh.save("/home/ce00169625/Documents/test_cpp_zed.ply");
    fusion_mesh.save("/home/Documents/test_cpp_fused.ply");

    fusion.disablePositionalTracking();
    zed.disablePositionalTracking();
    zed.disableSpatialMapping();
    fusion.close();
    zed.close();
}