#include "common/utils.hpp"
#include "nodes/base/base.hpp"
#include "nodes/httpPush/httpPush.hpp"

namespace Node
{

static cv::Mat get_color_depth(cv::Mat inverse_depth)
{
    double min_val, max_val;
    cv::minMaxLoc(inverse_depth, &min_val, &max_val);

    float max_invdepth_vizu = std::min(static_cast<float>(max_val), 1.0f / 0.1f);
    float min_invdepth_vizu = std::max(static_cast<float>(1.0 / 250.0), static_cast<float>(min_val));

    cv::Mat inverse_depth_normalized = (inverse_depth - min_invdepth_vizu) / (max_invdepth_vizu - min_invdepth_vizu);
    
    cv::Mat inverse_depth_normalized_8u;
    inverse_depth_normalized.convertTo(inverse_depth_normalized_8u, CV_8UC1, 255);

    cv::Mat color_depth;
    cv::applyColorMap(inverse_depth_normalized_8u, color_depth, cv::COLORMAP_JET);

    cv::Mat color_depth_bgr;
    cv::cvtColor(color_depth, color_depth_bgr, cv::COLOR_BGR2RGB);
    return color_depth_bgr;
}

void HttpPushNode::work()
{
    printf("HttpPush %s\n", name_.c_str());
    while (running_)
    {
        bool has_data = false;
        for (auto& input_buffer : input_buffers_)
        {
            std::shared_ptr<meta::MetaData> metaData;
            if (!input_buffer.second->try_pop(metaData))
            {
                continue;
            }
            has_data = true;
            // printf("Node %s get data from %s\n", name_.c_str(), input_buffer.first.c_str());
            // do something
            cv::Mat image = metaData->draw_image;
            std::string image_name = "result/" + metaData->from + "_" + getTimeString() + ".jpg";
            cv::imwrite(image_name, image);

            if (!metaData->depth.empty())
            {
                std::string depth_name = "result/" + metaData->from + "_depth_" + getTimeString() + ".jpg";
                cv::imwrite(depth_name, get_color_depth(metaData->depth));
            }
        }
        if (!has_data) {
            std::unique_lock<std::mutex> lock(mutex_);
            cond_var_->wait_for(lock, std::chrono::milliseconds(100), [this] {
                return !running_;  // 等待时检查退出条件
            });
        }
    }
};

}   // namespace Node