///////////////////////////////////////////////////////////////////////////
//
// 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>

// 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;
}

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 {
        unsigned int serial_left  = 307994820;
        unsigned int serial_right = 305743419;
        unsigned int virt_sn = generateVirtualStereoSerialNumber(serial_left, serial_right);
        init_parameters.input.setVirtualStereoFromCameraIDs(1, 0, virt_sn);

    }

    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;
    }

    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;
        }
        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();
            }
        }
    }

    zed.close();
    return EXIT_SUCCESS;
}