// vision-anti-pinch - V4L2 MMAP 取图 + USBCANFD CAN 收发 (OpenCV 版本)
//
// 使用 OpenCV 进行 UYVY/YUYV -> BGR 转换，避免手动转换公式问题
// CAN 收发默认开启, --no-can 关闭
//
// 用法:
//   ./vision-anti-pinch -d /dev/video5 -n 10 -s 2 -t jpg -o ./out

#include "v4l2_capture.h"
#include "can_comm.h"
#include "send.hpp"

#include <opencv2/core.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/imgcodecs.hpp>

#include <cstdio>
#include <cstdlib>
#include <cstring>
#include <string>
#include <vector>
#include <bitset>
#include <atomic>
#include <chrono>
#include <csignal>
#include <linux/videodev2.h>

std::atomic<uint64_t> g_Feedback_Of_The_Actual_Position_Of_The_Left_Front_Door_391_S;
std::atomic<uint64_t> g_Feedback_Of_The_Actual_Position_Of_The_Right_Front_Door_391_S;
std::atomic<uint64_t> g_Feedback_Of_The_Actual_Position_Of_The_Left_Rear_Door_391_S;
std::atomic<uint64_t> g_Feedback_Of_The_Actual_Position_Of_The_Right_Rear_Door_391_S;

// Sensing GMSL2 驱动自定义控制 ID
#define CID_SENSOR_MODE  0x009a2008
#define CID_TRIG_PIN     0x009a2084
#define CID_TRIG_MODE    0x009a2085
#define CID_GAIN         0x009a2009
#define CID_EXPOSURE     0x009a200a

static volatile sig_atomic_t g_stop = 0;
static void onSignal(int) { g_stop = 1; }

static void printUsage(const char *prog) {
    printf(
        "Usage: %s [options]\n"
        "V4L2 MMAP capture demo for Sensing GMSL2 camera on Jetson Orin.\n"
        "\n"
        "Capture options:\n"
        "  -d <device>      V4L2 device node (default: /dev/video5)\n"
        "  -W <width>       frame width  (default: 1920)\n"
        "  -H <height>      frame height (default: 1536)\n"
        "  -f <fps>         target frame rate (default: 30)\n"
        "  -F <format>      pixel format: UYVY | YUYV (default: UYVY)\n"
        "  -m <mode>        sensor_mode control value\n"
        "  -p <trig_pin>    trig_pin control value (hex/dec)\n"
        "  --gain <val>     sensor gain\n"
        "  --exposure <val> sensor exposure\n"
        "\n"
        "Output options:\n"
        "  -n <count>       frames to capture, 0 = run forever\n"
        "  -s <count>       frames to save\n"
        "  -o <dir>         output directory (default: ./frames)\n"
        "  -t <type>        file type: jpg | png | bmp | raw (default: jpg)\n"
        "  --quality <1..100>  JPEG quality (default: 95)\n"
        "  -v               verbose output\n"
        "  -h               print this help\n"
        "\n"
        "CAN options:\n"
        "  --no-can         disable USBCANFD CAN/CANFD receive (enabled by default)\n"
        "\n"
        "Example:\n"
        "  %s -d /dev/video5 -n 100 -s 5 -t jpg -o ./out\n"
        "  %s -d /dev/video5 -n 0 -v\n",
        prog, prog, prog);
}

struct Args {
    std::string device = "/dev/video5";
    uint32_t width = 1920;
    uint32_t height = 1536;  // SG3S-ISX031C-GMSL2F 默认分辨率
    uint32_t fps = 30;
    std::string format = "UYVY";
    long sensor_mode = -1;
    long trig_pin = -1;
    long trig_mode = -1;
    long gain = -1;
    long exposure = -1;
    long count = 0;
    long save = 0;
    std::string out_dir = "./frames";
    std::string save_type = "jpg";
    int quality = 95;
    bool verbose = false;
    bool can_enabled = true;   // 默认启用 CAN 收发, --no-can 关闭
};

// 手动解析参数
static bool parseArgs(int argc, char **argv, Args &a) {
    auto need_val = [&](int &i) -> std::string {
        if (i + 1 >= argc) {
            printf("missing value for %s\n", argv[i]);
            std::exit(1);
        }
        return argv[++i];
    };

    for (int i = 1; i < argc; ++i) {
        std::string arg = argv[i];
        if      (arg == "-d")  a.device = need_val(i);
        else if (arg == "-W")  a.width = (uint32_t)std::atoi(need_val(i).c_str());
        else if (arg == "-H")  a.height = (uint32_t)std::atoi(need_val(i).c_str());
        else if (arg == "-f")  a.fps = (uint32_t)std::atoi(need_val(i).c_str());
        else if (arg == "-F")  a.format = need_val(i);
        else if (arg == "-m")  a.sensor_mode = std::strtol(need_val(i).c_str(), nullptr, 0);
        else if (arg == "-p")  a.trig_pin = std::strtol(need_val(i).c_str(), nullptr, 0);
        else if (arg == "-M")  a.trig_mode = std::strtol(need_val(i).c_str(), nullptr, 0);
        else if (arg == "-n")  a.count = std::strtol(need_val(i).c_str(), nullptr, 0);
        else if (arg == "-s")  a.save = std::strtol(need_val(i).c_str(), nullptr, 0);
        else if (arg == "-o")  a.out_dir = need_val(i);
        else if (arg == "-t")  a.save_type = need_val(i);
        else if (arg == "-v")  a.verbose = true;
        else if (arg == "-h")  { printUsage(argv[0]); std::exit(0); }
        else if (arg == "--gain")     a.gain = std::strtol(need_val(i).c_str(), nullptr, 0);
        else if (arg == "--exposure")  a.exposure = std::strtol(need_val(i).c_str(), nullptr, 0);
        else if (arg == "--quality")   a.quality = std::atoi(need_val(i).c_str());
        else if (arg == "--no-can")     a.can_enabled = false;
        else {
            printf("unknown option: %s\n", arg.c_str());
            printUsage(argv[0]);
            return false;
        }
    }
    return true;
}

// 使用 OpenCV 将 V4L2 帧 (UYVY/YUYV) 转为 BGR Mat
static cv::Mat frameToBgr(const V4l2Capture::Frame &frame) {
    // OpenCV 原生支持 UYVY/YUYV -> BGR 转换
    // UYVY = COLOR_YUV2BGR_UYVY, YUYV = COLOR_YUV2BGR_YUYV
    int code = (frame.pixelformat == V4L2_PIX_FMT_UYVY)
               ? cv::COLOR_YUV2BGR_UYVY
               : cv::COLOR_YUV2BGR_YUYV;
    cv::Mat yuv((int)frame.height, (int)frame.width, CV_8UC2, (void *)frame.data);
    cv::Mat bgr;
    cv::cvtColor(yuv, bgr, code);
    return bgr;
}

/* =====================================================================
 * CAN 报文接收解析 (由 can_comm 接收线程回调)
 * ===================================================================== */

// 512 位报文位域, deal_msg 中按 msg_id 拷贝后可按位解析信号
static std::bitset<512> g_bitMsg;

static uint64_t get_bit_value(int star_bit, int bit_len)
{
    uint64_t signal_value = 0;
    for (int i = 0; i < bit_len; ++i)
    {
        signal_value |= g_bitMsg[star_bit + i] << i;
    }
    return signal_value;
}

// 收到报文的回调点: 目前仅解析 0x391, 后续信号解析在此扩展
static void deal_msg(int64_t time, uint32_t msg_id, const uint8_t *data, uint32_t len)
{
    if (msg_id == 0x391 && len >= 4 && len <= 64)
    {
        memcpy(&g_bitMsg, data, len);
        g_Feedback_Of_The_Actual_Position_Of_The_Left_Front_Door_391_S = data[0];
        g_Feedback_Of_The_Actual_Position_Of_The_Right_Front_Door_391_S = data[1];
        g_Feedback_Of_The_Actual_Position_Of_The_Left_Rear_Door_391_S = data[2];
        g_Feedback_Of_The_Actual_Position_Of_The_Right_Rear_Door_391_S = data[3];
        // Rear_BCM_0x391,0x391,64,1.0-1.7,8,Feedback_Of_The_Actual_Position_Of_The_Left_Front_Door_391_S
        // Rear_BCM_0x391,0x391,64,2.0-2.7,8,Feedback_Of_The_Actual_Position_Of_The_Right_Front_Door_391_S
        // Rear_BCM_0x391,0x391,64,3.0-3.7,8,Feedback_Of_The_Actual_Position_Of_The_Left_Rear_Door_391_S
        // Rear_BCM_0x391,0x391,64,4.0-4.7,8,Feedback_Of_The_Actual_Position_Of_The_Right_Rear_Door_391_S
    }
}

int main(int argc, char **argv) {
    Args args;
    if (!parseArgs(argc, argv, args)) {
        return 1;
    }

    std::signal(SIGINT, onSignal);
    std::signal(SIGTERM, onSignal);

    uint32_t pixfmt = (args.format == "UYVY") ? V4L2_PIX_FMT_UYVY : V4L2_PIX_FMT_YUYV;

    // 启动 CAN 收发(失败不致命, 可继续仅取图)
    if (args.can_enabled) {
        CanComm::set_msg_callback(deal_msg);  // 注册报文解析回调
        if (!CanComm::can_start()) {
            printf("CAN start failed, continue with camera only.\n");
            args.can_enabled = false;
        }
    }

    V4l2Capture cap;

    if (!cap.open(args.device)) {
        return 1;
    }

    // 设置相机控制
    if (args.sensor_mode >= 0) cap.setControl(CID_SENSOR_MODE, (int32_t)args.sensor_mode);
    if (args.trig_pin >= 0)    cap.setControl(CID_TRIG_PIN, (int32_t)args.trig_pin);
    if (args.trig_mode >= 0)   cap.setControl(CID_TRIG_MODE, (int32_t)args.trig_mode);
    if (args.gain >= 0)        cap.setControl(CID_GAIN, (int32_t)args.gain);
    if (args.exposure >= 0)    cap.setControl(CID_EXPOSURE, (int32_t)args.exposure);

    if (!cap.setFormat(args.width, args.height, pixfmt)) {
        return 1;
    }
    cap.setFps(args.fps);

    if (!cap.initMmap(4)) {
        return 1;
    }
    if (!cap.startStreaming()) {
        return 1;
    }

    printf("=========================================================\n");
    printf("Capturing %s %ux%u @ %u fps from %s\n",
           args.format.c_str(), cap.width(), cap.height(), args.fps, args.device.c_str());
    printf("Press Ctrl+C to stop.\n");
    printf("=========================================================\n");

    // 创建输出目录
    if (args.save > 0) {
        std::string mkcmd = "mkdir -p " + args.out_dir;
        std::system(mkcmd.c_str());
    }

    using clock = std::chrono::steady_clock;
    auto t_start = clock::now();
    auto t_last_report = t_start;
    uint64_t frame_count = 0;
    uint64_t save_count = 0;
    uint32_t timeout_consecutive = 0;

    while (!g_stop) {
        if (args.count > 0 && frame_count >= (uint64_t)args.count) {
            break;
        }

        V4l2Capture::Frame frame;
        if (!cap.getFrame(frame, 2000)) {
            timeout_consecutive++;
            if (timeout_consecutive > 5) {
                printf("getFrame: timeout/empty for %u consecutive tries, aborting\n",
                       timeout_consecutive);
                break;
            }
            continue;
        }
        timeout_consecutive = 0;
        frame_count++;

        // 保存帧
        if (save_count < (uint64_t)args.save) {
            bool ok = false;
            char fname[512];

            // 使用 OpenCV 转换并保存
            cv::Mat bgr = frameToBgr(frame);
            if (!bgr.empty())
            {
                std::snprintf(fname, sizeof(fname), "%s/frame_%06lu.%s",
                              args.out_dir.c_str(), save_count, args.save_type.c_str());

                std::vector<int> params;
                if (args.save_type == "jpg" || args.save_type == "jpeg")
                {
                    params = {cv::IMWRITE_JPEG_QUALITY, args.quality};
                }
                else if (args.save_type == "png")
                {
                    params = {cv::IMWRITE_PNG_COMPRESSION, 3};
                }
                ok = cv::imwrite(fname, bgr, params);
            }

            if (ok) {
                printf("[saved] %s\n", fname);
                save_count++;
            } else {
                printf("[save failed] frame %lu\n", save_count);
            }
        }

        DebugInfo *pData = new DebugInfo();
        // pData->CameraId = (ctx->cam_no == 1 ? 0 : 1);
        // pData->TimeStampMs = epochTimeStamp_ms;
        memcpy(pData->Img, frame.data, frame.size);
        bool res = send_debuginfo(pData);
        //printf("send %ld %d %d\n", pData->TimeStampMs, pData->CameraId, res);
        delete pData;
        pData = nullptr;

        cap.releaseFrame(frame);

        // if (args.verbose) {
        //     printf("frame #%lu seq=%u size=%zu\n",
        //            frame_count, frame.sequence, frame.size);
        // }

        auto now = clock::now();
        auto since_report = std::chrono::duration_cast<std::chrono::milliseconds>(now - t_last_report);
        if (since_report.count() >= 1000) {
            double total_sec = std::chrono::duration_cast<std::chrono::milliseconds>(now - t_start).count() / 1000.0;
            double avg_fps = total_sec > 0 ? frame_count / total_sec : 0;
            printf("[stats] frames=%lu avg_fps=%.2f door_position[%ld %ld %ld %ld]\n", frame_count, avg_fps, g_Feedback_Of_The_Actual_Position_Of_The_Left_Front_Door_391_S.load(), g_Feedback_Of_The_Actual_Position_Of_The_Right_Front_Door_391_S.load(), g_Feedback_Of_The_Actual_Position_Of_The_Left_Rear_Door_391_S.load(), g_Feedback_Of_The_Actual_Position_Of_The_Right_Rear_Door_391_S.load());
            t_last_report = now;
        }
    }

    auto t_end = clock::now();
    double total_sec = std::chrono::duration_cast<std::chrono::milliseconds>(t_end - t_start).count() / 1000.0;

    printf("=========================================================\n");
    printf("Capture finished. frames=%lu duration=%.2fs avg_fps=%.2f\n",
           frame_count, total_sec, total_sec > 0 ? frame_count / total_sec : 0);
    if (args.save > 0) {
        printf("Saved %lu frames to %s/\n", save_count, args.out_dir.c_str());
    }
    if (args.can_enabled && CanComm::is_open()) {
        printf("CAN received: %d frames\n", CanComm::receive_count());
    }
    printf("=========================================================\n");

    cap.stopStreaming();
    cap.close();

    if (args.can_enabled) {
        CanComm::can_stop();
    }
    return 0;
}