#include "endo_guide.h"
#include "core/imgui_utility.hpp"
#include "module/camera_augment_helper_v2.h"

app_endo_guide::app_endo_guide(const create_config &_conf) {
    main_conf = _conf;
    auto conf = main_conf.ext_conf;

    // initialize object manager
    OBJ_SAVE(img_color, image_u8c3());
    OBJ_SAVE(img_out, image_ptr());

    // initialize sophiar
    sophiar_thread = std::make_unique<std::thread>([conf_path = LOAD_STR("sophiar_config")] {
        sophiar::run_sophiar(conf_path);
    });
    sophiar_conn = std::make_unique<sophiar_conn_type>();
    sophiar_start_var = LOAD_STR("sophiar_start_var");

    // initialize modules
    auto uvc_cam_conf = uvc_camera_ui::create_config{
            .img_name = img_color,
            .ctx = main_conf.asio_ctx,
            .stream = default_cuda_stream,
    };
    uvc_cam = std::make_unique<uvc_camera_ui>(uvc_cam_conf);

    auto bg_viewer_conf = image_viewer::create_config{
            .mode = VIEW_COLOR_ONLY, .flip_y = true,
            .stream = default_cuda_stream,
    };
    auto &bg_extra_conf = bg_viewer_conf.extra.color;
    bg_extra_conf.fmt = COLOR_RGB;
    bg_extra_conf.name = img_out;
    bg_viewer = std::make_unique<image_viewer>(bg_viewer_conf);

    auto saver_conf = image_saver::create_config{.ctx = main_conf.asio_ctx};
    saver_conf.img_list.emplace_back("Image", img_color);
    out_saver = std::make_unique<image_saver>(saver_conf);

    auto calib_conf = camera_calibrator::create_config{
            .img_name = img_color,
            .board_type = camera_calibrator::CALIB_CHESS,
            .pattern_size = cv::Size(11, 8), .corner_distance = 5,
            .stream = default_cuda_stream, .ctx = main_conf.asio_ctx,
            .cb_func = [this](const auto &ret) {
                aug_helper->set_camera_info(ret.cam_in);
                aug_helper->get_camera_helper()
                        ->set_fixed_transform_latency(ret.time_offset);
            },
            .sophiar_conn = sophiar_conn.get(),
            .ref_transform_var = LOAD_STR("camera_ref_transform_var"),
            .result_transform_var = LOAD_STR("camera_calib_transform_var"),
    };
    cam_calib = std::make_unique<camera_calibrator>(calib_conf);

    auto aug_list_v1 = augment_manager::item_list_from_yaml(LOAD_LIST("augment_list"));
    auto aug_list = augment_manager_v2::item_list_from_v1(aug_list_v1, extra_name);
    auto aug_conf = augment_manager_v2::create_config{
            .sophiar_conn = sophiar_conn.get(),
            .stream = default_cuda_stream,
    };
    std::copy(aug_list.begin(), aug_list.end(), std::back_inserter(aug_conf.item_list));
    aug_manager = std::make_unique<augment_manager_v2>(aug_conf);

    auto helper_conf = image_augment_helper_v2::create_config{
            .img_name = img_color, .mask_name = invalid_obj_name,
            .out_name = img_out, .img_flip_y = false,
            .manager = aug_manager.get(), .stream = default_cuda_stream,
            .ctx = main_conf.asio_ctx,
            .sophiar_conn = sophiar_conn.get(),
            .transform_var = LOAD_STR("camera_transform_var"),
    };
    aug_helper = std::make_unique<image_augment_helper_v2>(helper_conf);
    aug_helper->post_render_sig.connect([this] { cam_calib->render(); });

    auto monitor_conf = sophiar_monitor::create_config{
            .item_list = sophiar_monitor::item_list_from_yaml(LOAD_LIST("monitor")),
            .sophiar_conn = sophiar_conn.get(),
    };
    monitor = std::make_unique<sophiar_monitor>(monitor_conf);

//    auto sim_info = camera_calibrator::simulate_info_type{
//            .data_path = "/home/tpx/project/DepthGuide/cmake-build-debug/calib_data_10.96.txt",
//            .img_size = cv::Size(1920, 1080),
//    };
//    cam_calib->simulate_process(sim_info);
}

void app_endo_guide::show_ui() {
    if (ImGui::Begin("Endo Guide Control")) {
        ImGui::PushItemWidth(200);

        if (ImGui::CollapsingHeader("Camera")) {
            auto id_guard = imgui_id_guard("camera");
            uvc_cam->show();
        }

        if (ImGui::CollapsingHeader("Tracker")) {
            if (ImGui::Button("Start")) {
                start_tracking();
            }
            monitor->show();
            {
                ImGui::SeparatorText("Scene");
                auto id_guard = imgui_id_guard("augment_scene");
                aug_manager->show();
            }
        }

        if (ImGui::CollapsingHeader("Calibration")) {
            auto id_guard = imgui_id_guard("calibration");
            cam_calib->show();
        }

        if (ImGui::CollapsingHeader("Debug")) {
            if (ImGui::TreeNode("Image Saver")) {
                out_saver->show();
                ImGui::TreePop();
            }

        }
    }
    ImGui::End();
}

void app_endo_guide::render_background() {
    bg_viewer->render();
}

void app_endo_guide::start_tracking() {
    // sophiar_conn->start_object(sophiar_start_var);
    // work in another thread to prevent blocking
    auto t = std::thread([this] {
        auto conn = sophiar::local_connection();
        conn.start_object(sophiar_start_var);
    });
    t.detach();
}

app_endo_guide::~app_endo_guide() {
    // sophiar
    sophiar::stop_sophiar();
    sophiar_thread->join();
}