#include "debug.hpp"
#include <fcntl.h>
#include <fmt/format.h>
#include <fstream>
#include <nlohmann/json.hpp>
#include <sys/mman.h>
#include <sys/stat.h>
namespace wust_vision {
namespace auto_guidance {
    void drawAutoGuidanceDebugContent(cv::Mat& debug_img, const AutoGuidanceDebug& dbg) {
        auto target = dbg.target;
        auto lights = dbg.lights;

        lights.drawFront(debug_img);

        if (target.is_tracking_) {
            auto now = std::chrono::steady_clock::now();
            target.predict(now);
            target.draw(debug_img);
        }

        std::string latency_str = fmt::format("Latency: {:.2f}ms", dbg.latency_ms);
        cv::putText(
            debug_img,
            latency_str,
            cv::Point(10, 30),
            cv::FONT_HERSHEY_SIMPLEX,
            0.8,
            cv::Scalar(255, 255, 255),
            2
        );

        cv::circle(
            debug_img,
            cv::Point2i(debug_img.cols / 2, debug_img.rows / 2),
            5,
            cv::Scalar(255, 255, 255),
            2
        );
        double cx_norm = target.center().x / target.image_size_.width * 2.0 - 1.0;
        double diff_center_norm = (target.is_tracking_) ? cx_norm : 0;
        {
            std::string diff_str = fmt::format("diff_cx_norm: {:.3f}", diff_center_norm);

            int margin = 10;
            int font_face = cv::FONT_HERSHEY_SIMPLEX;
            double font_scale = 0.7;
            int thickness = 2;

            int baseline = 0;
            cv::Size text_size =
                cv::getTextSize(diff_str, font_face, font_scale, thickness, &baseline);

            // 右上角文本左下角坐标
            int x = debug_img.cols - margin - text_size.width;
            int y = margin + text_size.height;

            // 背景框，可选
            cv::rectangle(
                debug_img,
                cv::Rect(
                    x - 5,
                    y - text_size.height - 5,
                    text_size.width + 10,
                    text_size.height + 10
                ),
                cv::Scalar(0, 0, 0),
                cv::FILLED,
                cv::LINE_AA
            );

            cv::putText(
                debug_img,
                diff_str,
                cv::Point(x, y),
                font_face,
                font_scale,
                cv::Scalar(0, 255, 255),
                thickness,
                cv::LINE_AA
            );
        }

        if (target.is_tracking_) {
            const auto& s = target.target_state_;

            std::string line1 =
                fmt::format("pos: {:.1f} {:.1f} {:.1f} {:.1f}", s(0), s(2), s(4), s(6));
            std::string line2 =
                fmt::format("vel: {:.2f} {:.2f} {:.2f} {:.2f}", s(1), s(3), s(5), s(7));

            int x = 10;
            int y = debug_img.rows - 30; // 左下角位置
            int dy = 28;

            cv::putText(
                debug_img,
                line1,
                cv::Point(x, y),
                cv::FONT_HERSHEY_SIMPLEX,
                0.75,
                cv::Scalar(0, 255, 0),
                2
            );

            cv::putText(
                debug_img,
                line2,
                cv::Point(x, y + dy),
                cv::FONT_HERSHEY_SIMPLEX,
                0.75,
                cv::Scalar(0, 255, 0),
                2
            );

            double vx = s(1);
            double vy = s(3);

            cv::Point2f p0 = target.center();

            double scale = 0.5;

            cv::Point2f p1(p0.x + vx * scale, p0.y + vy * scale);

            cv::arrowedLine(
                debug_img,
                p0,
                p1,
                cv::Scalar(0, 255, 0),
                2,
                cv::LINE_AA,
                0,
                0.25 // 箭头比例
            );
        }
    }

    void drawDebugOverlayWrite(const AutoGuidanceDebug& dbg, bool auto_fps) {
        static auto last_show_time = std::chrono::steady_clock::now();

        if (dbg.img_frame.src_img.empty())
            return;
        cv::Mat src_img = dbg.img_frame.src_img;
        auto now = std::chrono::steady_clock::now();
        const double min_interval_ms = 1000.0 / 30.0;
        if (std::chrono::duration<double, std::milli>(now - last_show_time).count()
                < min_interval_ms
            && auto_fps)
            return;
        last_show_time = now;

        // 图像构造
        cv::Mat debug_img;
        src_img.convertTo(debug_img, -1, 1, 0);
        cv::cvtColor(debug_img, debug_img, cv::COLOR_BGR2RGB);
        if (debug_img.empty())
            return;

        // 封装后的绘图函数
        drawAutoGuidanceDebugContent(debug_img, dbg);
        cv::cvtColor(debug_img, debug_img, cv::COLOR_RGB2BGR);
        // 编码写入共享内存路径
        std::vector<uchar> buf;
        cv::imencode(".jpg", debug_img, buf);
        std::ofstream ofs("/dev/shm/debug_frame.jpg.tmp", std::ios::binary);
        ofs.write(reinterpret_cast<const char*>(buf.data()), buf.size());
        ofs.close();
        std::rename("/dev/shm/debug_frame.jpg.tmp", "/dev/shm/debug_frame.jpg");
    }
    void drawDebugOverlayShm(const AutoGuidanceDebug& dbg, bool auto_fps) {
        static auto last_show_time = std::chrono::steady_clock::now();
        const char* shm_name = "/debug_frame";
        const size_t shm_max_size = 2 * 1024 * 1024; // 2MB 最大图像编码缓存

        if (dbg.img_frame.src_img.empty())
            return;
        cv::Mat src_img = dbg.img_frame.src_img;

        auto now = std::chrono::steady_clock::now();
        const double min_interval_ms = 1000.0 / 30.0;
        if (std::chrono::duration<double, std::milli>(now - last_show_time).count()
                < min_interval_ms
            && auto_fps)
            return;
        last_show_time = now;

        // 复制并转RGB
        cv::Mat debug_img;
        src_img.convertTo(debug_img, -1, 1, 0);
        cv::cvtColor(debug_img, debug_img, cv::COLOR_BGR2RGB);
        if (debug_img.empty())
            return;

        // 绘制内容
        drawAutoGuidanceDebugContent(debug_img, dbg);
        // 编码为 JPG
        std::vector<uchar> buf;
        cv::imencode(".jpg", debug_img, buf);
        size_t img_size = buf.size();

        if (img_size > shm_max_size) {
            std::cerr << "[drawDebugOverlayWrite] 图像过大: " << img_size << " bytes\n";
            return;
        }

        // 创建/打开共享内存
        int fd = shm_open(shm_name, O_CREAT | O_RDWR, 0666);
        if (fd == -1) {
            perror("shm_open failed");
            return;
        }

        // 设置共享内存大小
        if (ftruncate(fd, shm_max_size) == -1) {
            perror("ftruncate failed");
            close(fd);
            return;
        }

        // 映射共享内存
        void* ptr = mmap(nullptr, shm_max_size, PROT_WRITE, MAP_SHARED, fd, 0);
        if (ptr == MAP_FAILED) {
            perror("mmap failed");
            close(fd);
            return;
        }

        // 写入图像数据
        uint32_t size = static_cast<uint32_t>(img_size);
        std::memcpy(ptr, &size, 4); // 前4字节写入长度
        std::memcpy(static_cast<char*>(ptr) + 4, buf.data(), img_size);

        // 关闭映射和文件描述符
        munmap(ptr, shm_max_size);
        close(fd);
    }
    void drawDebugOverlayShow(const AutoGuidanceDebug& dbg, bool auto_fps) {
        static auto last_show_time = std::chrono::steady_clock::now();

        if (dbg.img_frame.src_img.empty())
            return;
        cv::Mat src_img = dbg.img_frame.src_img;
        auto now = std::chrono::steady_clock::now();
        const double min_interval_ms = 1000.0 / 30.0;
        if (std::chrono::duration<double, std::milli>(now - last_show_time).count()
                < min_interval_ms
            && auto_fps)
            return;
        last_show_time = now;

        // 图像构造
        cv::Mat debug_img;
        src_img.convertTo(debug_img, -1, 1, 0);
        cv::cvtColor(debug_img, debug_img, cv::COLOR_BGR2RGB);
        if (debug_img.empty())
            return;

        // 封装后的绘图函数
        drawAutoGuidanceDebugContent(debug_img, dbg);

        cv::imshow("debug_armor", debug_img);
        cv::waitKey(1);
    }
    void debuglog(const GuidanceTarget& target) {
        static bool first_log = true;
        static std::chrono::steady_clock::time_point start_time;
        static DebugLogs log;
        if (first_log) {
            start_time = std::chrono::steady_clock::now();
            first_log = false;
        }
        auto now = std::chrono::steady_clock::now();
        double t = std::chrono::duration<double>(now - start_time).count();
        log.time_log.push_back(t);
        double cx_norm = target.center().x / target.image_size_.width * 2.0 - 1.0;
        log.cx_log.push_back(cx_norm);
        auto trim = [](std::vector<double>& v) {
            if (v.size() > 1000)
                v.erase(v.begin());
        };

        trim(log.time_log);
        trim(log.cx_log);
        nlohmann::json j;
        {
            j["time"] = log.time_log;
            j["yaw"] = log.cx_log;
        }
        std::ofstream file("/dev/shm/cmd_log.json");
        if (file.is_open()) {
            file << j.dump();
        }
    }
} // namespace auto_guidance
} // namespace wust_vision