#ifndef REMOTEAR2_STEREO_CAMERA_HPP
#define REMOTEAR2_STEREO_CAMERA_HPP

#include "config.h"
#include "mvs_camera.h"

#include <opencv2/cudaimgproc.hpp>

#include <atomic>
#include <thread>

struct stereo_camera {

    mvs_camera left_camera, right_camera;
    cv::cuda::GpuMat *left_raw_image = nullptr, *right_raw_image = nullptr;
    cv::cuda::GpuMat left_rgb_image, right_rgb_image;
    std::atomic_flag *left_save_raw_flag, *right_save_raw_flag;

    stereo_camera() {
        left_rgb_image = cv::cuda::GpuMat{image_height, image_width, CV_8UC3};
        right_rgb_image = cv::cuda::GpuMat{image_height, image_width, CV_8UC3};
        left_save_raw_flag = left_camera.get_save_raw_flag();
        right_save_raw_flag = right_camera.get_save_raw_flag();
    }

    ~stereo_camera() {
        close();
    }

    bool open() {
        if (!open_impl()) { // keep consistency
            close();
            return false;
        }
        return true;
    }

    void close() {
        stop_capture();
        left_camera.close();
        right_camera.close();
    }

    bool start_capture(float exposure_time_us, float analog_gain, int trigger_interval_ms) {
        if (!start_capture_impl(exposure_time_us, analog_gain)) {
            stop_capture();
            return false;
        }
        start_trigger_thread(trigger_interval_ms);
        return true;
    }

    bool set_capture_config(float exposure_time_us, float analog_gain) {
        auto config = mvs_camera::capture_config{};
        config.exposure_time = exposure_time_us;
        config.analog_gain = analog_gain;
        if (memcmp(&config, &last_config, sizeof(config)) == 0) return true;
        CALL_CHECK(left_camera.modify_config(config));
        CALL_CHECK(right_camera.modify_config(config));
        return true;
    }

    void stop_capture() {
        // stop trigger thread
        if (trigger_thread != nullptr) {

            // let thread exit by itself
            should_stop.test_and_set();
            trigger_thread->join();
            delete trigger_thread;

            // cleanup
            should_stop.clear();
            trigger_thread = nullptr;
        }

        left_camera.stop_capture();
        right_camera.stop_capture();
    }

    bool software_trigger() {
        return left_camera.software_trigger() &&
               right_camera.software_trigger();
    }

    void retrieve_raw_images() {
        assert(is_capturing());

        // clean old images
        delete left_raw_image;
        delete right_raw_image;
        left_raw_image = nullptr;
        right_raw_image = nullptr;

        // retrieve new images
        left_camera.retrieve_image(&left_raw_image);
        right_camera.retrieve_image(&right_raw_image);
        assert(left_raw_image != nullptr);
        assert(right_raw_image != nullptr);
    }

    bool is_opened() const {
        assert(left_camera.is_opened() == right_camera.is_opened());
        return left_camera.is_opened();
    }

    bool is_capturing() const {
        assert(left_camera.is_capturing() == right_camera.is_capturing());
        return left_camera.is_capturing();
    }

    void debayer_images() {
        cv::cuda::cvtColor(*left_raw_image, left_rgb_image, cv::COLOR_BayerRG2RGB);
        cv::cuda::cvtColor(*right_raw_image, right_rgb_image, cv::COLOR_BayerRG2RGB);
    }

    void set_raw_saver(raw_file_saver *saver) {
        left_camera.set_raw_saver(saver);
        right_camera.set_raw_saver(saver);
    }

    void request_save_raw() {
        left_save_raw_flag->test_and_set();
        right_save_raw_flag->test_and_set();
    }

private:

    std::thread *trigger_thread = nullptr;
    std::atomic_flag should_stop;
    mvs_camera::capture_config last_config;

    bool open_impl() {
        CALL_CHECK(left_camera.open(left_camera_name));
        CALL_CHECK(right_camera.open(right_camera_name));
        return true;
    }

    bool start_capture_impl(float exposure_time, float analog_gain) {
        last_config.exposure_time = exposure_time;
        last_config.analog_gain = analog_gain;
        CALL_CHECK(left_camera.start_capture(last_config));
        CALL_CHECK(right_camera.start_capture(last_config));
        return true;
    }

    void start_trigger_thread(int fps) {
        assert(trigger_thread == nullptr);
        trigger_thread = new std::thread{[=, this]() {
            auto trigger_interval = std::chrono::microseconds{(int) 1e6 / fps};
            auto next_trigger_time = std::chrono::high_resolution_clock::now();
            while (true) {
                if (should_stop.test()) break;
                software_trigger();

                // resume at (almost) exact time // TODO: test performance
                next_trigger_time += trigger_interval;
                std::this_thread::sleep_until(next_trigger_time - default_spin_time);
                while (std::chrono::high_resolution_clock::now() < next_trigger_time)
                    std::this_thread::yield();
            }
        }};
    }

};

#endif //REMOTEAR2_STEREO_CAMERA_HPP