///////////////////////////////////////////////////////////////////////////
//
// Copyright (c) 2025, STEREOLABS.
//
// All rights reserved.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
///////////////////////////////////////////////////////////////////////////

// Standard includes
#include <atomic>
#include <chrono>
#include <csignal>
#include <iomanip>
#include <iostream>
#include <limits>
#include <thread>

// ZED include
#include <sl/Camera.hpp>
#include <sl/CameraOne.hpp>

using namespace std;
using namespace sl;

static std::atomic<bool> g_running{true};

static void signalHandler(int) {
    g_running = false;
}

void printDevices() {
    auto devices = sl::CameraOne::getDeviceList();
    if (devices.empty()) {
        std::cout << "No ZED cameras found." << std::endl;
        return;
    }
    for (auto& it : devices) {
        if (it.camera_state == sl::CAMERA_STATE::AVAILABLE && isCameraOne(it.camera_model)) {
            std::cout << "- ZED One camera: " << it.camera_model
                      << " (ID : " << it.id << ", Serial: " << it.serial_number << ")" << std::endl;
        }
    }
}

int main(int argc, char** argv) {

    signal(SIGINT, signalHandler);

    Camera zed;

    sl::InitParameters init_parameters;
    init_parameters.sdk_verbose = true;
    init_parameters.camera_resolution = sl::RESOLUTION::AUTO;
    init_parameters.depth_mode = sl::DEPTH_MODE::NONE;

    // Set to a path to play back an SVO file, or leave empty for live cameras
    const std::string svo_path = "";

    if (!svo_path.empty()) {
        init_parameters.input.setFromSVOFile(svo_path.c_str());
        init_parameters.svo_real_time_mode = true;
        std::cout << "[Sample] SVO mode: " << svo_path << std::endl;
    } else {
        printDevices();
        // Serial numbers from OKVIS2-X Zed.cpp
        unsigned int serial_left  = 307994820;
        unsigned int serial_right = 305743419;
        unsigned int virt_sn = generateVirtualStereoSerialNumber(serial_left, serial_right);
        init_parameters.input.setVirtualStereoFromCameraIDs(0, 1, virt_sn);
        std::cout << "[Sample] Virtual Stereo: left SN=" << serial_left
                  << " right SN=" << serial_right
                  << " virt_sn=" << virt_sn << std::endl;
    }

    auto returned_state = zed.open(init_parameters);
    if (returned_state == ERROR_CODE::INVALID_CALIBRATION_FILE) {
        std::cout << "WARNING: Virtual Stereo rig not calibrated, continuing without calibration." << std::endl;
    } else if (returned_state > ERROR_CODE::SUCCESS) {
        std::cout << "[Error] Camera Open: " << toString(returned_state) << std::endl;
        return EXIT_FAILURE;
    }

    auto camera_info = zed.getCameraInformation();
    auto camera_conf = camera_info.camera_configuration;
    cout << "ZED Model             : " << camera_info.camera_model << endl;
    cout << "ZED Serial Number     : " << camera_info.serial_number << endl;
    cout << "ZED Firmware          : " << camera_conf.firmware_version
         << "/" << camera_info.sensors_configuration.firmware_version << endl;
    cout << "ZED Resolution        : " << camera_conf.resolution.width
         << "x" << camera_conf.resolution.height << endl;
    cout << "ZED FPS               : " << zed.getInitParameters().camera_fps << endl;
    cout << "Press Ctrl+C to exit." << endl;

    // IMU thread: poll sensor data and print frequency every 5 seconds
    std::thread imu_thread([&]() {
        sl::SensorsData sensor_data;
        uint64_t last_ts = 0;
        int imu_count = 0;
        auto timer_start = std::chrono::steady_clock::now();
        double min_freq = std::numeric_limits<double>::max();
        double max_freq = std::numeric_limits<double>::lowest();

        while (g_running) {
            auto status = zed.getSensorsData(sensor_data, sl::TIME_REFERENCE::CURRENT);
            if (status != sl::ERROR_CODE::SUCCESS) {
                continue;
            }
            else{
                uint64_t ts_ns = sensor_data.imu.timestamp.getNanoseconds();
                if (ts_ns > 0 && ts_ns != last_ts) {
                    if (last_ts > 0) {
                        double dt_s = (ts_ns - last_ts) * 1e-9;
                        if (dt_s > 0.0) {
                            double inst_freq = 1.0 / dt_s;
                            if (inst_freq < min_freq) min_freq = inst_freq;
                            if (inst_freq > max_freq) max_freq = inst_freq;
                        }
                    }
                    last_ts = ts_ns;
                    imu_count++;

                    auto now = std::chrono::steady_clock::now();
                    double elapsed = std::chrono::duration<double>(now - timer_start).count();
                    if (elapsed >= 1.0) {
                        double avg_freq = imu_count / elapsed;
                        std::cout << "[IMU] avg=" << std::fixed << std::setprecision(1) << avg_freq
                                  << " Hz  min=" << min_freq << " Hz  max=" << max_freq << " Hz" << std::endl;
                        imu_count = 0;
                        timer_start = now;
                        min_freq = std::numeric_limits<double>::max();
                        max_freq = std::numeric_limits<double>::lowest();
                    }
                    std::this_thread::sleep_for(std::chrono::milliseconds(1));
                }
                
            }
            
        }
    });

    // Main loop: keep grabbing to maintain camera session
    while (g_running) {
        returned_state = zed.grab();
        if (returned_state == ERROR_CODE::END_OF_SVOFILE_REACHED) {
            std::cout << "End of SVO file reached." << std::endl;
            break;
        } else if (returned_state != ERROR_CODE::SUCCESS && returned_state != ERROR_CODE::CORRUPTED_FRAME) {
            std::cout << "Grab error: " << toString(returned_state) << std::endl;
            if (returned_state != sl::ERROR_CODE::CAMERA_REBOOTING)
                break;
        }
    }

    g_running = false;
    imu_thread.join();
    zed.close();
    return EXIT_SUCCESS;
}