#include <dora-node-api.h>
#include <sl/Camera.hpp>
#include <opencv2/opencv.hpp>

#include <array>
#include <cctype>
#include <chrono>
#include <cstdlib>
#include <initializer_list>
#include <iostream>
#include <iterator>
#include <string>
#include <thread>
#include <tuple>
#include <vector>

namespace {

int get_env_int(const char* name, int default_value) {
    const char* value = std::getenv(name);
    if (!value || std::string(value).empty()) {
        return default_value;
    }
    return std::stoi(value);
}

bool get_env_bool(const char* name, bool default_value) {
    const char* value = std::getenv(name);
    if (!value || std::string(value).empty()) {
        return default_value;
    }
    std::string v(value);
    for (auto& c : v) {
        c = static_cast<char>(std::tolower(c));
    }
    return v == "1" || v == "true" || v == "yes" || v == "on";
}

std::string get_env_str(const char* name, const std::string& default_value) {
    const char* value = std::getenv(name);
    if (!value || std::string(value).empty()) {
        return default_value;
    }
    return std::string(value);
}

sl::RESOLUTION parse_resolution(int resolution) {
    switch (resolution) {
        case 1080:
            return sl::RESOLUTION::HD1080;
        case 2000:
            return sl::RESOLUTION::HD2K;
        case 720:
        default:
            return sl::RESOLUTION::HD720;
    }
}

sl::DEPTH_MODE parse_depth_mode(const std::string& mode) {
    if (mode == "PERFORMANCE") {
        return sl::DEPTH_MODE::PERFORMANCE;
    }
    if (mode == "QUALITY") {
        return sl::DEPTH_MODE::QUALITY;
    }
    if (mode == "NONE") {
        return sl::DEPTH_MODE::NONE;
    }
    return sl::DEPTH_MODE::NEURAL;
}

sl::FLIP_MODE parse_flip(const std::string& mode) {
    if (mode == "OFF") {
        return sl::FLIP_MODE::OFF;
    }
    return sl::FLIP_MODE::ON;
}

rust::Vec<int64_t> make_shape(int height, int width, int channels) {
    rust::Vec<int64_t> shape;
    shape.push_back(height);
    shape.push_back(width);
    shape.push_back(channels);
    return shape;
}

rust::Vec<double> make_distortion(const std::vector<double>& distortion) {
    rust::Vec<double> values;
    for (double v : distortion) {
        values.push_back(v);
    }
    return values;
}

rust::Vec<int64_t> make_vector_int(std::initializer_list<int64_t> values) {
    rust::Vec<int64_t> out;
    for (int64_t v : values) {
        out.push_back(v);
    }
    return out;
}

void list_available_cameras() {
    auto devices = sl::Camera::getDeviceList();
    std::cout << "Detected ZED cameras: " << devices.size() << std::endl;
    for (std::size_t i = 0; i < devices.size(); i++) {
        const auto& dev = devices[i];
        std::cout << " - [" << i << "] model=" << sl::toString(dev.camera_model)
                  << ", serial=" << dev.serial_number << std::endl;
    }
}

bool open_camera(sl::Camera& zed, const sl::InitParameters& init_params) {
    sl::ERROR_CODE err = zed.open(init_params);
    if (err != sl::ERROR_CODE::SUCCESS) {
        std::cerr << "Failed to open ZED camera: " << sl::toString(err) << std::endl;
        return false;
    }
    return true;
}

bool ensure_camera_ready(
    sl::Camera& zed,
    const sl::InitParameters& init_params,
    int sleep_ms
) {
    while (true) {
        if (open_camera(zed, init_params)) {
            return true;
        }
        std::this_thread::sleep_for(std::chrono::milliseconds(sleep_ms));
    }
}

} // namespace

int main() {
    auto dora_node = init_dora_node();

    const int resolution = get_env_int("ZED_RESOLUTION", 720);
    const int fps = get_env_int("ZED_FPS", 15);
    const std::string depth_mode = get_env_str("ZED_DEPTH_MODE", "NEURAL");
    const int depth_min_mm = get_env_int("ZED_DEPTH_MIN_MM", 10);
    const int depth_max_mm = get_env_int("ZED_DEPTH_MAX_MM", 500);
    const bool depth_fill = get_env_bool("ZED_DEPTH_FILL", false);
    const std::string flip = get_env_str("ZED_FLIP", "ON");
    const int warmup_frames = get_env_int("ZED_WARMUP_FRAMES", 30);
    const int retry_interval_seconds = get_env_int("ZED_RETRY_INTERVAL_SECONDS", 5);
    const int retry_interval_ms = retry_interval_seconds * 1000;

    sl::InitParameters init_params;
    init_params.camera_resolution = parse_resolution(resolution);
    init_params.camera_fps = fps;
    init_params.depth_mode = parse_depth_mode(depth_mode);
    init_params.coordinate_units = sl::UNIT::MILLIMETER;
    init_params.camera_image_flip = parse_flip(flip);
    init_params.depth_minimum_distance = depth_min_mm;
    init_params.depth_maximum_distance = depth_max_mm;

    list_available_cameras();

    sl::Camera zed;
    ensure_camera_ready(zed, init_params, retry_interval_ms);

    sl::RuntimeParameters runtime_params;
    runtime_params.enable_depth = true;
    runtime_params.enable_fill_mode = depth_fill;

    sl::Mat image_mat;
    sl::Mat point_cloud_mat;

    for (int i = 0; i < warmup_frames; i++) {
        zed.grab(runtime_params);
    }

    auto reload_calibration = [&]() {
        sl::CameraInformation info = zed.getCameraInformation();
        auto calib = info.camera_configuration.calibration_parameters;
        auto left = calib.left_cam;

        std::vector<double> dist;
        dist.reserve(std::size(left.disto));
        for (double v : left.disto) {
            dist.push_back(v);
        }
        return std::tuple<float, float, float, float, std::vector<double>>(
            left.fx, left.fy, left.cx, left.cy, std::move(dist));
    };

    auto [fx, fy, cx, cy, distortion] = reload_calibration();

    while (true) {
        auto event = dora_node.events->next();
        auto type = event_type(event);

        if (type == DoraEventType::Stop || type == DoraEventType::AllInputsClosed) {
            break;
        }

        if (type != DoraEventType::Input) {
            continue;
        }

        auto input = event_as_input(std::move(event));
        (void)input;

        if (zed.grab(runtime_params) != sl::ERROR_CODE::SUCCESS) {
            std::cerr << "Grab failed; attempting to reconnect..." << std::endl;
            zed.close();
            list_available_cameras();
            ensure_camera_ready(zed, init_params, retry_interval_ms);
            std::tie(fx, fy, cx, cy, distortion) = reload_calibration();
            continue;
        }

        zed.retrieveImage(image_mat, sl::VIEW::LEFT);
        zed.retrieveMeasure(point_cloud_mat, sl::MEASURE::XYZRGBA);

        int width = image_mat.getWidth();
        int height = image_mat.getHeight();

        cv::Mat image_rgba(height, width, CV_8UC4, image_mat.getPtr<sl::uchar1>());
        cv::Mat image_bgr;
        cv::cvtColor(image_rgba, image_bgr, cv::COLOR_BGRA2BGR);

        const auto image_size = static_cast<size_t>(image_bgr.total() * image_bgr.elemSize());
        rust::Slice<const uint8_t> image_slice{
            reinterpret_cast<const uint8_t*>(image_bgr.data), image_size};

        auto image_metadata = new_metadata();
        image_metadata->set_string("dtype", "uint8");
        image_metadata->set_string("encoding", "bgr8");
        image_metadata->set_string("layout", "HWC");
        image_metadata->set_list_int("shape", make_shape(height, width, 3));
        image_metadata->set_float("fx", fx);
        image_metadata->set_float("fy", fy);
        image_metadata->set_float("cx", cx);
        image_metadata->set_float("cy", cy);
        image_metadata->set_list_float("distortion", make_distortion(distortion));

        auto image_result = send_output_with_metadata(
            dora_node.send_output, "image_bgr", image_slice, std::move(image_metadata));
        if (!std::string(image_result.error).empty()) {
            std::cerr << "Error sending image_bgr: " << std::string(image_result.error) << std::endl;
        }

        int pc_width = point_cloud_mat.getWidth();
        int pc_height = point_cloud_mat.getHeight();
        const size_t pc_size = static_cast<size_t>(pc_width * pc_height * sizeof(sl::float4));
        const auto* pc_ptr = reinterpret_cast<const uint8_t*>(point_cloud_mat.getPtr<sl::float4>());
        rust::Slice<const uint8_t> pc_slice{pc_ptr, pc_size};

        auto pc_metadata = new_metadata();
        pc_metadata->set_string("dtype", "float32");
        pc_metadata->set_string("layout", "HWC");
        pc_metadata->set_string("channels", "XYZRGBA");
        pc_metadata->set_string("units", "mm");
        pc_metadata->set_list_int("shape", make_shape(pc_height, pc_width, 4));
        pc_metadata->set_float("fx", fx);
        pc_metadata->set_float("fy", fy);
        pc_metadata->set_float("cx", cx);
        pc_metadata->set_float("cy", cy);
        pc_metadata->set_list_float("distortion", make_distortion(distortion));

        auto pc_result = send_output_with_metadata(
            dora_node.send_output, "point_cloud", pc_slice, std::move(pc_metadata));
        if (!std::string(pc_result.error).empty()) {
            std::cerr << "Error sending point_cloud: " << std::string(pc_result.error) << std::endl;
        }

        float camera_data[4] = {fx, fy, cx, cy};
        rust::Slice<const uint8_t> cam_slice{
            reinterpret_cast<const uint8_t*>(camera_data), sizeof(camera_data)};

        auto cam_metadata = new_metadata();
        cam_metadata->set_string("dtype", "float32");
        cam_metadata->set_string("layout", "C");
        cam_metadata->set_list_int("shape", make_vector_int({4}));
        cam_metadata->set_list_float("distortion", make_distortion(distortion));

        auto cam_result = send_output_with_metadata(
            dora_node.send_output, "camera_info", cam_slice, std::move(cam_metadata));
        if (!std::string(cam_result.error).empty()) {
            std::cerr << "Error sending camera_info: " << std::string(cam_result.error) << std::endl;
        }
    }

    zed.close();
    return 0;
}