#include "camera.h"
#include "model_v10.hpp"

#include <fcntl.h>
#include <unistd.h>
#include <sys/ioctl.h>
#include <sys/mman.h>
#include <linux/i2c-dev.h>
#include <linux/i2c.h>

cv::VideoCapture cap;
cv::Mat raw_mat;
cv::Mat decoded_frame;          // 1/4 解码输出复用 buffer
static int g_decode_mode = -1;  // -1=未检测 0=全BGR回退 1=原始JPEG缩放解码

int screenWidth, screenHeight, newWidth, newHeight;
int fb;
uint16_t *fb_buffer;
PwmController servo(1, 0);

#define calc_scale 2

// ── 模型检测参数 ──
#define ZEBRA_CLASS      3
#define CONE_CLASS       0
static float g_thresh[4] = {0.90f, 0.75f, 0.80f, 0.90f};

// ── 斑马线去抖 ──
#define ZEBRA_MIN_FRAMES  5
#define ZEBRA_FAR_CY      50
static int g_zc_frames = 0;
static int g_zc_min_cy = 120;

// ── 斑马线状态机 ──
enum ZState { Z_NORMAL, Z_STOP, Z_COOLDOWN };
static ZState  g_zstate     = Z_NORMAL;
static time_t  g_ztime      = 0;
static bool    g_zebra_ever = false;

// ── 红绿灯状态机 ──
#define TL_RED_CLASS   1
#define TL_GREEN_CLASS 2
enum TLState { TL_NORMAL, TL_STOP, TL_WAIT_GREEN };
static TLState g_tl_state       = TL_NORMAL;
static int     g_tl_red_frames  = 0;
static int     g_tl_green_frames = 0;

// ── 锥桶检测 ──
static int  g_cone_frames     = 0;
static int  g_cone_last_row   = -1;
static int  g_cone_last_col   = -1;
static bool g_cone_confirmed  = false;
static int  g_cone_hold_ctr   = 0;
static int  g_cone_hold_src_row = -1;
static int  g_cone_hold_src_col = -1;

// ── 模型检测结果缓存 ──
static DetectBoxV10 g_boxes[16];
static int          g_box_count = 0;
static bool         g_lcd_on    = true;

double g_steer_deviation = 0;

// ── I2C 音频 ──
static int i2c_audio_fd = -1;

static bool i2c_audio_open()
{
    if (i2c_audio_fd >= 0) return true;
    i2c_audio_fd = open("/dev/i2c-2", O_RDWR);
    if (i2c_audio_fd < 0) {
        fprintf(stderr, "[ZEBRA] 无法打开 I2C-2: %s\n", strerror(errno));
        return false;
    }
    if (ioctl(i2c_audio_fd, I2C_SLAVE, 0x34) < 0) {
        fprintf(stderr, "[ZEBRA] 无法设置 I2C 从地址 0x34: %s\n", strerror(errno));
        close(i2c_audio_fd); i2c_audio_fd = -1;
        return false;
    }
    return true;
}

int CameraInit(uint8_t camera_id, double dest_fps, int width, int height)
{
    servo.setPeriod(3000000);
    servo.setDutyCycle(1500000);
    servo.enable();

    fb = open("/dev/fb0", O_RDWR);
    if (fb == -1) { std::cerr << "无法打开帧缓冲区设备" << std::endl; return -1; }

    struct fb_var_screeninfo vinfo;
    if (ioctl(fb, FBIOGET_VSCREENINFO, &vinfo) == -1) {
        std::cerr << "无法获取帧缓冲区信息" << std::endl; close(fb); return -1;
    }
    screenWidth = vinfo.xres; screenHeight = vinfo.yres;
    size_t fb_size = vinfo.yres_virtual * vinfo.xres_virtual * vinfo.bits_per_pixel / 8;
    fb_buffer = (uint16_t *)mmap(NULL, fb_size, PROT_READ | PROT_WRITE, MAP_SHARED, fb, 0);
    if (fb_buffer == MAP_FAILED) {
        std::cerr << "无法映射帧缓冲区到内存" << std::endl; close(fb); return -1;
    }

    cap.open(0, cv::CAP_V4L2);
    if (!cap.isOpened()) cap.open(0);
    cap.set(cv::CAP_PROP_FOURCC, cv::VideoWriter::fourcc('M', 'J', 'P', 'G'));
    cap.set(cv::CAP_PROP_FRAME_WIDTH, 640);
    cap.set(cv::CAP_PROP_FRAME_HEIGHT, 480);
    cap.set(cv::CAP_PROP_CONVERT_RGB, 0);   // 后端直接返回原始 MJPEG 字节流, 由我们做 1/4 解码
    if (!cap.isOpened()) {
        printf("无法打开摄像头\n");
        munmap(fb_buffer, fb_size); close(fb); return -1;
    }

    int cameraWidth = cap.get(cv::CAP_PROP_FRAME_WIDTH);
    int cameraHeight = cap.get(cv::CAP_PROP_FRAME_HEIGHT);
    printf("摄像头分辨率: %d x %d\n", cameraWidth, cameraHeight);

    double widthRatio = static_cast<double>(screenWidth) / cameraWidth;
    double heightRatio = static_cast<double>(screenHeight) / cameraHeight;
    double scale = std::min(widthRatio, heightRatio);
    newWidth = static_cast<int>(cameraWidth * scale);
    newHeight = static_cast<int>(cameraHeight * scale);
    printf("自适应分辨率: %d x %d\n", newWidth, newHeight);

    double fps = cap.get(cv::CAP_PROP_FPS);
    printf("Camera fps:%lf\n", fps);

    line_tracking_width = newWidth / calc_scale;
    line_tracking_height = newHeight / calc_scale;

    if (!model_v10_init("./mild_v12.bin")) {
        printf("[MODEL] 警告: 模型加载失败\n");
    } else {
        printf("[MODEL] Mild v12 模型已加载\n");
    }

    i2c_audio_open();

    return static_cast<int>(1000.0 / std::min(fps, dest_fps));
}

void cameraDeInit(void)
{
    cap.release();
    struct fb_var_screeninfo vinfo;
    if (ioctl(fb, FBIOGET_VSCREENINFO, &vinfo) != -1) {
        size_t fb_size = vinfo.yres_virtual * vinfo.xres_virtual * vinfo.bits_per_pixel / 8;
        munmap(fb_buffer, fb_size);
    }
    close(fb);
    if (i2c_audio_fd >= 0) close(i2c_audio_fd);
    model_v10_deinit();
}

int saved_frame_count = 0;
static bool saveCameraImage(cv::Mat frame, const std::string &directory)
{
    if (frame.empty()) return false;
    std::ostringstream filename;
    filename << directory << "/image_" << std::setw(5) << std::setfill('0') << saved_frame_count << ".jpg";
    saved_frame_count++;
    return cv::imwrite(filename.str(), frame);
}

static void play_zebra_audio()
{
    if (!i2c_audio_open()) {
        printf("[ZEBRA] 语音失败: I2C 未打开\n");
        return;
    }
    ioctl(i2c_audio_fd, I2C_SLAVE, 0x34);

    union i2c_smbus_data d;
    struct i2c_smbus_ioctl_data a;
    __u8 v[] = {0xFF, 0x10};
    d.block[0] = 2; d.block[1] = v[0]; d.block[2] = v[1];
    a.read_write = I2C_SMBUS_WRITE;
    a.command = 0x6E;
    a.size = I2C_SMBUS_I2C_BLOCK_DATA;
    a.data = &d;

    if (ioctl(i2c_audio_fd, I2C_SMBUS, &a) < 0)
        printf("[ZEBRA] 语音失败: %s\n", strerror(errno));
    else
        printf("[ZEBRA] 语音播报已触发\n");
}

// ── LCD Mats 预分配 (避免每帧 new/delete) ──
static cv::Mat lcd_fbImage;
static cv::Mat lcd_resized;
static cv::Mat lcd_colored;

int CameraHandler(void)
{
    struct timespec t0,t1,t2,t3,t4,t5;

    // ── 1. 取帧 ──
    clock_gettime(CLOCK_MONOTONIC,&t0);
    cap.read(raw_mat);
    clock_gettime(CLOCK_MONOTONIC,&t1);
    if (raw_mat.empty()) return -1;

    // CONVERT_RGB=0 检测: 单通道=原始JPEG字节流 → 1/4解码; 三通道=后端已解码 → 回退
    if (g_decode_mode == -1) {
        g_decode_mode = (raw_mat.channels() == 1) ? 1 : 0;
        printf("[CAM] CONVERT_RGB=0 %s, mode=%d\n",
               g_decode_mode == 1 ? "生效->1/4解码160x120" : "未生效->全解码回退640x480",
               g_decode_mode);
    }

    if (g_decode_mode == 1) {
        cv::imdecode(raw_mat, cv::IMREAD_REDUCED_COLOR_4, &decoded_frame);
        if (decoded_frame.empty()) return -1;
        raw_frame = decoded_frame;
    } else {
        raw_frame = raw_mat;
    }

    // ── 2. 保存图像 ──
    if (g_cfg.debug) {
        if (readFlag(saveImg_file)) {
            if (saveCameraImage(raw_frame, "./image"))
                printf("图像%d已保存\n", saved_frame_count);
        }
    }

    // ── 3. 视觉巡线 ──
    image_main();
    clock_gettime(CLOCK_MONOTONIC,&t2);

    // ── 4. 模型推理 (每2帧一次, 640×480直入) ──
    static int infer_skip = 0;
    if (++infer_skip >= 2) {
        infer_skip = 0;
        g_box_count = 0;
        if (model_v10_ready()) {
            g_box_count = model_v10_detect(raw_frame.data, raw_frame.cols, raw_frame.rows,
                                           g_boxes, 16, g_thresh);
        }
    }
    clock_gettime(CLOCK_MONOTONIC,&t3);

    // ── 5. 斑马线去抖+状态机 ──
    bool zebra_near = false;
    bool zebra_seen = false;
    int  zebra_cy   = 0;
    float zebra_cf  = 0;
    g_zebra_ever = false;

    for (int i = 0; i < g_box_count; ++i) {
        if (g_boxes[i].cls == ZEBRA_CLASS) {
            g_zebra_ever = true;
            zebra_cy = (int)g_boxes[i].cy;
            zebra_cf = g_boxes[i].conf;
            if (g_zstate == Z_NORMAL) {
                if (zebra_cy < g_zc_min_cy) g_zc_min_cy = zebra_cy;
                g_zc_frames++;
            }
            zebra_seen = true;

            if (zebra_cy > g_cfg.zebrasee)
                zebra_near = true;
            break;
        }
    }

    if (!zebra_seen) {
        if (g_zc_frames > 0) g_zc_frames = std::max(0, g_zc_frames - 2);
        if (g_zc_frames == 0) g_zc_min_cy = 120;
    }

    time_t now = time(nullptr);
    switch (g_zstate) {
    case Z_NORMAL:
        if (zebra_near) {
            bool enough = (g_zc_frames >= ZEBRA_MIN_FRAMES);
            bool from_far = (g_zc_min_cy <= ZEBRA_FAR_CY);
            if (enough && from_far) {
                play_zebra_audio();
                g_zstate = Z_STOP; g_ztime = now;
                if (g_cfg.debug)
                    printf("[ZEBRA] cy=%d cf=%.2f 停车4s 冷却5s\n", zebra_cy, zebra_cf);
                g_zc_frames = 0; g_zc_min_cy = 120;
            } else if (g_cfg.debug) {
                printf("[ZEBRA] cy=%d 拒绝 f=%d/%d mc=%d/%d\n",
                       zebra_cy, g_zc_frames, ZEBRA_MIN_FRAMES, g_zc_min_cy, ZEBRA_FAR_CY);
            }
        }
        break;
    case Z_STOP:
        if (now - g_ztime >= 4) {
            g_zstate = Z_COOLDOWN; g_ztime = now;
            if (g_cfg.debug) printf("[ZEBRA] 起步\n");
        }
        break;
    case Z_COOLDOWN:
        if (now - g_ztime >= 5) {
            g_zstate = Z_NORMAL;
            if (g_cfg.debug) printf("[ZEBRA] 恢复\n");
        }
        break;
    }

    // ── 5.5 红绿灯状态机 ──
    bool red_seen   = false;
    bool green_seen = false;
    for (int i = 0; i < g_box_count; ++i) {
        if (g_boxes[i].cls == TL_RED_CLASS)   red_seen   = true;
        if (g_boxes[i].cls == TL_GREEN_CLASS) green_seen = true;
    }

    if (red_seen)   g_tl_red_frames++;
    else            g_tl_red_frames   = std::max(0, g_tl_red_frames - 1);
    if (green_seen) g_tl_green_frames++;
    else            g_tl_green_frames = std::max(0, g_tl_green_frames - 1);

    switch (g_tl_state) {
    case TL_NORMAL:
        if (g_tl_red_frames >= 3) {
            g_tl_state = TL_STOP;
            if (g_cfg.debug) printf("[TL] 红灯停车\n");
        }
        break;
    case TL_STOP:
        if (!red_seen) {
            g_tl_state = TL_WAIT_GREEN;
            if (g_cfg.debug) printf("[TL] 等待绿灯\n");
        }
        break;
    case TL_WAIT_GREEN:
        if (g_tl_green_frames >= 3) {
            g_tl_state = TL_NORMAL;
            g_tl_red_frames   = 0;
            g_tl_green_frames = 0;
            if (g_cfg.debug) printf("[TL] 绿灯通行\n");
        }
        break;
    }

    bool tl_block = (g_tl_state != TL_NORMAL);

    // ── 5.6 锥桶检测 + 中线变形 ──
    bool cone_seen = false;
    int  cone_row_keep = -1;
    int  cone_col_keep = -1;

    if (g_zstate != Z_STOP && g_tl_state == TL_NORMAL) {
        for (int i = 0; i < g_box_count; ++i) {
            if (g_boxes[i].cls != CONE_CLASS) continue;
            if (g_boxes[i].conf < g_cfg.cone_thresh) continue;

            int col_lt = g_boxes[i].cx * line_tracking_width  / raw_frame.cols;
            int row_lt = g_boxes[i].cy * line_tracking_height / raw_frame.rows;

            if (row_lt < 10 || row_lt >= line_tracking_height) continue;
            if (left_line[row_lt] == -1 || right_line[row_lt] == -1) continue;

            if (g_cfg.cone_margin) {
                int bl = (g_boxes[i].cx - g_boxes[i].w/2) * line_tracking_width  / raw_frame.cols;
                int br = (g_boxes[i].cx + g_boxes[i].w/2) * line_tracking_width  / raw_frame.cols;
                if (br < left_line[row_lt] || bl > right_line[row_lt]) continue;
            } else {
                if (col_lt < left_line[row_lt] || col_lt > right_line[row_lt]) continue;
            }

            if (row_lt > cone_row_keep) { cone_row_keep = row_lt; cone_col_keep = col_lt; }
            cone_seen = true;
        }
    }

    {
        int row_tol = std::max(2, line_tracking_height / 12);
        int col_tol = std::max(3, line_tracking_width  / 8);
        if (cone_seen) {
            if (g_cone_frames == 0) {
                g_cone_last_row = cone_row_keep;
                g_cone_last_col = cone_col_keep;
                g_cone_frames = 1;
            } else {
                if (std::abs(cone_row_keep - g_cone_last_row) <= row_tol &&
                    std::abs(cone_col_keep - g_cone_last_col) <= col_tol) {
                    g_cone_frames++;
                    g_cone_last_row = cone_row_keep;
                    g_cone_last_col = cone_col_keep;
                } else {
                    g_cone_frames = 1;
                    g_cone_last_row = cone_row_keep;
                    g_cone_last_col = cone_col_keep;
                }
            }
        } else {
            if (g_cone_frames > 0) g_cone_frames = std::max(0, g_cone_frames - 1);
        }
    }

    g_cone_confirmed = (g_cone_frames >= g_cfg.cone_min_frames);

    if (g_cone_confirmed) {
        g_cone_hold_ctr    = 0;
        g_cone_hold_src_row = cone_row_keep;
        g_cone_hold_src_col = cone_col_keep;
    } else if (g_cone_hold_src_row > 0 && g_cone_hold_ctr <= g_cfg.cone_hold_frames) {
        g_cone_hold_ctr++;
    }

    bool cone_active = g_cone_confirmed ||
        (g_cone_hold_ctr > 0 && g_cone_hold_ctr <= g_cfg.cone_hold_frames);

    if (cone_active && g_zstate != Z_STOP && g_tl_state == TL_NORMAL) {
        int src_row = g_cone_confirmed ? cone_row_keep : g_cone_hold_src_row;
        int src_col = g_cone_confirmed ? cone_col_keep : g_cone_hold_src_col;
        int start_row = 10;
        if (src_row > start_row) {
            double total_rows = src_row - start_row;
            double rng = (double)g_cfg.cone_avoid_range;
            double half_w = line_tracking_width / 2.0;
            double mid_at_cone = mid_line[src_row];
            double dir = (src_col < mid_at_cone) ? 1.0 : -1.0;
            double decay = g_cone_confirmed
                ? 1.0
                : (1.0 - (double)g_cone_hold_ctr / g_cfg.cone_hold_frames);

            for (int row = start_row; row <= src_row; row++) {
                double t_raw = (row - start_row) / total_rows;
                double t = std::clamp(t_raw * (total_rows / rng), 0.0, 1.0);
                double push = t * g_cfg.cone_avoid_gain * half_w * dir * decay;
                double new_mid = mid_line[row] + push;
                new_mid = std::clamp(new_mid,
                                     (double)left_line[row] + 2.0,
                                     (double)right_line[row] - 2.0);
                mid_line[row] = (int)(new_mid + 0.5);
            }
        }
    }

    // ── 6. 舵机 ──
    if (g_cfg.start) {
        int foresee = (int)g_cfg.foresee;
        int check_row = foresee / calc_scale;
        if (check_row >= 0 && check_row < line_tracking_height && mid_line[check_row] != -1) {
            double deviation = mid_line[check_row] * calc_scale - newWidth / 2;
            deviation -= g_cfg.center_bias;
            g_steer_deviation = deviation / (newWidth / 2.0);
            if (std::abs(deviation) < g_cfg.deadband) {
                servo.setDutyCycle(1500000);
            } else {
                double norm = deviation / (newWidth / 2.0);
                double offset = norm * g_cfg.steer_gain * 300000;
                double duty_ns = 1500000.0 + offset;
                duty_ns = std::clamp(duty_ns, 1200000.0, 1800000.0);
                servo.setDutyCycle(static_cast<unsigned int>(duty_ns));
            }
        }
    }

    // ── 7. 电机控制 ──
    {
        double spd = target_speed;
        bool cone_slow = g_cone_confirmed ||
            (g_cone_hold_ctr > 0 && g_cone_hold_ctr <= g_cfg.cone_hold_frames);
        if (cone_slow && g_zstate != Z_STOP && g_tl_state == TL_NORMAL) {
            spd *= g_cfg.cone_speed;
        }
        ControlUpdate(spd, g_zstate == Z_STOP || tl_block);
    }
    clock_gettime(CLOCK_MONOTONIC,&t4);

    // ── 8. LCD ──
    static int lcd_check = 0;
    if (--lcd_check < 0) {
        g_lcd_on = g_cfg.showImg;
        lcd_check = 10;
    }

    if (g_lcd_on) {
        lcd_fbImage.create(screenHeight, screenWidth, CV_8UC3);
        lcd_fbImage.setTo(cv::Scalar(0, 0, 0));
        cv::resize(track, lcd_resized, cv::Size(newWidth, newHeight));
        cv::cvtColor(lcd_resized, lcd_colored, cv::COLOR_GRAY2BGR);

        cv::Rect roi((screenWidth - newWidth) / 2, (screenHeight - newHeight) / 2, newWidth, newHeight);
        lcd_colored.copyTo(lcd_fbImage(roi));

        for (int y = 0; y < line_tracking_height; y++) {
            int sLX=static_cast<int>(left_line[y]*calc_scale), sRX=static_cast<int>(right_line[y]*calc_scale);
            int sMX=static_cast<int>(mid_line[y]*calc_scale), sY=static_cast<int>(y*calc_scale);
            cv::line(lcd_fbImage(roi), cv::Point(sLX,sY), cv::Point(sLX,sY), cv::Scalar(0,0,255), calc_scale);
            cv::line(lcd_fbImage(roi), cv::Point(sRX,sY), cv::Point(sRX,sY), cv::Scalar(0,255,0), calc_scale);
            cv::line(lcd_fbImage(roi), cv::Point(sMX,sY), cv::Point(sMX,sY), cv::Scalar(255,0,0), calc_scale);
        }

        float bx=(float)newWidth/160.0f, by=(float)newHeight/120.0f;
        for (int i = 0; i < g_box_count; ++i) {
            if (g_boxes[i].cls == 1 || g_boxes[i].cls == 2) continue;
            int x1=(int)((g_boxes[i].cx-g_boxes[i].w/2)*bx), y1=(int)((g_boxes[i].cy-g_boxes[i].h/2)*by);
            int x2=(int)((g_boxes[i].cx+g_boxes[i].w/2)*bx), y2=(int)((g_boxes[i].cy+g_boxes[i].h/2)*by);
            x1=std::max(0,std::min(newWidth-1,x1)); y1=std::max(0,std::min(newHeight-1,y1));
            x2=std::max(0,std::min(newWidth-1,x2)); y2=std::max(0,std::min(newHeight-1,y2));
            cv::Scalar color(0,255,0);
            if (g_boxes[i].cls == ZEBRA_CLASS) color=cv::Scalar(255,0,255);
            else if (g_boxes[i].cls == CONE_CLASS) color=cv::Scalar(0,165,255);
            cv::rectangle(lcd_fbImage(roi), cv::Point(x1,y1), cv::Point(x2,y2), color, 2);
            char lab[16]; std::snprintf(lab,16,"%d %.0f",g_boxes[i].cls,g_boxes[i].conf*100);
            cv::putText(lcd_fbImage(roi), lab, cv::Point(x1+2,y1+10), cv::FONT_HERSHEY_SIMPLEX,0.3,color,1);
        }

        char ztxt[8];
        int  zidx = 0;
        if (g_tl_state == TL_STOP)        ztxt[zidx++] = 'R';
        else if (g_tl_state == TL_WAIT_GREEN) ztxt[zidx++] = 'G';
        if (g_zstate == Z_STOP)           ztxt[zidx++] = 'S';
        else if (g_zstate == Z_COOLDOWN)   ztxt[zidx++] = 'C';
        else                               ztxt[zidx++] = 'N';
        ztxt[zidx] = '\0';
        cv::putText(lcd_fbImage(roi), ztxt, cv::Point(2,newHeight-4), cv::FONT_HERSHEY_SIMPLEX,0.4,cv::Scalar(0,255,255),1);

        convertMatToRGB565(lcd_fbImage, fb_buffer, screenWidth, screenHeight);
    }

    // ── 9. FPS + 分步计时 ──
    {
        clock_gettime(CLOCK_MONOTONIC,&t5);
        static int fc=0; static timespec tf0; if(fc==0) clock_gettime(CLOCK_MONOTONIC,&tf0);
        fc++;
        if(fc%15==0){ timespec tf1; clock_gettime(CLOCK_MONOTONIC,&tf1);
            double dt=(tf1.tv_sec-tf0.tv_sec)+(tf1.tv_nsec-tf0.tv_nsec)*1e-9;
            double ms_r  =(t1.tv_sec-t0.tv_sec)*1000.0+(t1.tv_nsec-t0.tv_nsec)*1e-6;
            double ms_vis=(t2.tv_sec-t1.tv_sec)*1000.0+(t2.tv_nsec-t1.tv_nsec)*1e-6;
            double ms_mdl=(t3.tv_sec-t2.tv_sec)*1000.0+(t3.tv_nsec-t2.tv_nsec)*1e-6;
            double ms_ctl=(t4.tv_sec-t3.tv_sec)*1000.0+(t4.tv_nsec-t3.tv_nsec)*1e-6;
            printf("fps=%.1f|rd=%.0f vi=%.0f md=%.0f ct=%.0f ms\r",
                   fc/dt, ms_r, ms_vis, ms_mdl, ms_ctl);
            fflush(stdout);
        }
    }
    return 0;
}