#include "mini212g2.h"
#include "debug.h"
#include "string.h"
#include "qdx_port.h"

#if SENSOR_UART_ENABLE

/* ============================================================
 * Protocol constants
 * ============================================================ */
#define FRAME_HEAD1  0x55
#define FRAME_HEAD2  0xAA
#define FRAME_TAIL   0xF0

/* ACK response: 55 AA 01 00 01 F0 */
#define ACK_LEN 6
static const uint8_t ACK_PATTERN[ACK_LEN] = {0x55, 0xAA, 0x01, 0x00, 0x01, 0xF0};

/* ============================================================
 * DVP output configuration commands
 * (from Mini212G2 serial protocol document)
 * ============================================================ */

/* Digital port type → CMOS */
static const uint8_t CMD_DIGITAL_CMOS[] =
    {0x55, 0xAA, 0x07, 0x02, 0x01, 0x02, 0x00, 0x00, 0x00, 0x02, 0x04, 0xF0};

/* CMOS interface → CMOS8(MSB) */
static const uint8_t CMD_CMOS8_MSB[] =
    {0x55, 0xAA, 0x07, 0x02, 0x01, 0x04, 0x00, 0x00, 0x00, 0x01, 0x01, 0xF0};

/* CMOS content → Y16 */
static const uint8_t CMD_CONTENT_Y16[] =
    {0x55, 0xAA, 0x07, 0x02, 0x01, 0x03, 0x00, 0x00, 0x00, 0x02, 0x05, 0xF0};

/* Frame rate → 30Hz */
static const uint8_t CMD_FPS_30HZ[] =
    {0x55, 0xAA, 0x07, 0x02, 0x01, 0x05, 0x00, 0x00, 0x00, 0x00, 0x01, 0xF0};

/* Save settings */
static const uint8_t CMD_SAVE[] =
    {0x55, 0xAA, 0x07, 0x01, 0x00, 0x04, 0x00, 0x00, 0x00, 0x01, 0x03, 0xF0};

/* Shutter compensation (NUC) */
static const uint8_t CMD_SHUTTER[] =
    {0x55, 0xAA, 0x07, 0x02, 0x01, 0x08, 0x00, 0x00, 0x00, 0x01, 0x0D, 0xF0};

/* Query: digital video page */
static const uint8_t CMD_QUERY_DIGITAL[] =
    {0x55, 0xAA, 0x07, 0x02, 0x01, 0x80, 0x00, 0x00, 0x00, 0x00, 0x84, 0xF0};

/* Query: status page (simplest query, always works) */
static const uint8_t CMD_QUERY_STATUS[] =
    {0x55, 0xAA, 0x07, 0x00, 0x00, 0x80, 0x00, 0x00, 0x00, 0x00, 0x87, 0xF0};

/* ============================================================
 * Low-level UART helpers
 * ============================================================ */

static void Sensor_UART_Init(void)
{
    GPIO_InitTypeDef  gpio = {0};
    USART_InitTypeDef usart = {0};

#if SENSOR_USE_USART3
    /* USART3: PB10=TX  PB11=RX
     * NOTE: This reuses the debug printf port. printf is disabled. */
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART3, ENABLE);
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);

    gpio.GPIO_Pin   = GPIO_Pin_10;
    gpio.GPIO_Speed = GPIO_Speed_50MHz;
    gpio.GPIO_Mode  = GPIO_Mode_AF_PP;
    GPIO_Init(GPIOB, &gpio);

    gpio.GPIO_Pin  = GPIO_Pin_11;
    gpio.GPIO_Mode = GPIO_Mode_IN_FLOATING;
    GPIO_Init(GPIOB, &gpio);
#else
    /* USART2: PA2=TX  PA3=RX */
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART2, ENABLE);
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);

    gpio.GPIO_Pin   = GPIO_Pin_2;
    gpio.GPIO_Speed = GPIO_Speed_50MHz;
    gpio.GPIO_Mode  = GPIO_Mode_AF_PP;
    GPIO_Init(GPIOA, &gpio);

    gpio.GPIO_Pin  = GPIO_Pin_3;
    gpio.GPIO_Mode = GPIO_Mode_IN_FLOATING;
    GPIO_Init(GPIOA, &gpio);
#endif

    usart.USART_BaudRate            = SENSOR_UART_BAUD;
    usart.USART_WordLength          = USART_WordLength_8b;
    usart.USART_StopBits            = USART_StopBits_1;
    usart.USART_Parity              = USART_Parity_No;
    usart.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
    usart.USART_Mode                = USART_Mode_Tx | USART_Mode_Rx;

    USART_Init(SENSOR_UART, &usart);
    USART_Cmd(SENSOR_UART, ENABLE);
}

static void Sensor_SendBytes(const uint8_t *data, uint8_t len)
{
    for (uint8_t i = 0; i < len; i++) {
        while (!USART_GetFlagStatus(SENSOR_UART, USART_FLAG_TXE));
        USART_SendData(SENSOR_UART, data[i]);
    }
    while (!USART_GetFlagStatus(SENSOR_UART, USART_FLAG_TC));
}

/**
 * Read response bytes until FRAME_TAIL seen or timeout.
 * timeout_ms ≈ approximate milliseconds (uses Delay_Us polling).
 * Returns number of bytes actually read.
 */
static int Sensor_ReadResp(uint8_t *buf, uint8_t max_len, uint16_t timeout_ms)
{
    uint8_t idx = 0;
    /* Poll with ~0.1 ms granularity */
    for (uint32_t i = 0; i < (uint32_t)timeout_ms * 10; i++) {
        if (USART_GetFlagStatus(SENSOR_UART, USART_FLAG_RXNE)) {
            buf[idx++] = (uint8_t)USART_ReceiveData(SENSOR_UART);
            if (idx >= max_len) return idx;
            if (idx >= 2 && buf[idx - 1] == FRAME_TAIL) return idx;
        } else {
            Delay_Us(100);
        }
    }
    return idx;
}

static void Sensor_FlushRx(void)
{
    while (USART_GetFlagStatus(SENSOR_UART, USART_FLAG_RXNE))
        (void)USART_ReceiveData(SENSOR_UART);
}

/* ============================================================
 * Public API
 * ============================================================ */

/* ============================================================
 * Debug inspection variables (watch in JTAG debugger)
 * ============================================================ */
volatile uint8_t  sensor_init_ok       = 0;
volatile uint8_t  sensor_init_fail     = 0;
volatile uint8_t  sensor_last_resp[32] = {0};
volatile uint8_t  sensor_last_resp_len = 0;
volatile uint8_t  sensor_comm_test     = 0;

int Mini212G2_SendCmd(const uint8_t *cmd, uint8_t len)
{
    uint8_t resp[8];
    Sensor_FlushRx();
    Sensor_SendBytes(cmd, len);

    int n = Sensor_ReadResp(resp, sizeof(resp), 200);
    /* Save last response for debugger inspection */
    sensor_last_resp_len = (uint8_t)n;
    for (int j = 0; j < n && j < 32; j++)
        sensor_last_resp[j] = resp[j];

    if (n == ACK_LEN && memcmp(resp, ACK_PATTERN, ACK_LEN) == 0)
        return 0;

    DBG_INIT("Sensor resp(%d):", n);
    for (int j = 0; j < n; j++) printf(" %02x", resp[j]);
    printf("\r\n");
    return -1;
}

#endif /* SENSOR_UART_ENABLE (UART helpers + SendCmd) */

int Mini212G2_Init(void)
{
#if !SENSOR_UART_ENABLE
    DBG_INIT("Sensor: UART disabled, pre-configured via USB\r\n");
    DBG_INIT("Sensor: CMOS output, CMOS8(MSB), Y16, 30Hz\r\n");
    return 0;
#else
    int ok = 0, fail = 0;

    Sensor_UART_Init();
    DBG_INIT("Sensor: UART%d baud=%d\r\n",
           (int)(SENSOR_UART == USART3 ? 3 : 2), (int)SENSOR_UART_BAUD);

    /* Sensor needs several seconds to boot after power-on */
    DBG_INIT("Sensor: Waiting 3s for boot...\r\n");
    Delay_Ms(3000);

    /* === Communication test: send status query === */
    {
        uint8_t test_resp[32];
        Sensor_FlushRx();
        Sensor_SendBytes(CMD_QUERY_STATUS, sizeof(CMD_QUERY_STATUS));
        int tn = Sensor_ReadResp(test_resp, sizeof(test_resp), 500);
        sensor_last_resp_len = (uint8_t)tn;
        for (int j = 0; j < tn && j < 32; j++)
            sensor_last_resp[j] = test_resp[j];
        sensor_comm_test = (tn > 0) ? 1 : 0xFF;
        /* Breakpoint here to check sensor_comm_test and sensor_last_resp */
    }
    Delay_Ms(200);

    DBG_INIT("Sensor: Configuring CMOS/DVP output...\r\n");

    /* 1. Shutter compensation */
    if (Mini212G2_SendCmd(CMD_SHUTTER, sizeof(CMD_SHUTTER)) == 0)
        { ok++; DBG_INIT("Sensor: Shutter OK\r\n"); }
    else
        { fail++; DBG_ERR("Sensor: Shutter FAIL\r\n"); }
    Delay_Ms(200);

    /* 2. Digital port → CMOS */
    if (Mini212G2_SendCmd(CMD_DIGITAL_CMOS, sizeof(CMD_DIGITAL_CMOS)) == 0)
        { ok++; DBG_INIT("Sensor: Digital->CMOS OK\r\n"); }
    else
        { fail++; DBG_ERR("Sensor: Digital->CMOS FAIL\r\n"); }
    Delay_Ms(200);

    /* 3. CMOS interface → 8-bit MSB */
    if (Mini212G2_SendCmd(CMD_CMOS8_MSB, sizeof(CMD_CMOS8_MSB)) == 0)
        { ok++; DBG_INIT("Sensor: CMOS8(MSB) OK\r\n"); }
    else
        { fail++; DBG_ERR("Sensor: CMOS8(MSB) FAIL\r\n"); }
    Delay_Ms(200);

    /* 4. CMOS content → Y16 (raw 16-bit thermal) */
    if (Mini212G2_SendCmd(CMD_CONTENT_Y16, sizeof(CMD_CONTENT_Y16)) == 0)
        { ok++; DBG_INIT("Sensor: Y16 OK\r\n"); }
    else
        { fail++; DBG_ERR("Sensor: Y16 FAIL\r\n"); }
    Delay_Ms(200);

    /* 5. Frame rate → 30Hz */
    if (Mini212G2_SendCmd(CMD_FPS_30HZ, sizeof(CMD_FPS_30HZ)) == 0)
        { ok++; DBG_INIT("Sensor: 30Hz OK\r\n"); }
    else
        { fail++; DBG_ERR("Sensor: 30Hz FAIL\r\n"); }
    Delay_Ms(200);

    /* 6. Save settings to flash */
    if (Mini212G2_SendCmd(CMD_SAVE, sizeof(CMD_SAVE)) == 0)
        { ok++; DBG_INIT("Sensor: Save OK\r\n"); }
    else
        { fail++; DBG_ERR("Sensor: Save FAIL\r\n"); }
    Delay_Ms(500);

    DBG_INIT("Sensor: %d ok, %d fail\r\n", ok, fail);
    sensor_init_ok   = (uint8_t)ok;
    sensor_init_fail = (uint8_t)fail;

    /* Print current digital video status for verification */
    Mini212G2_PrintDigitalVideoStatus();

    return (fail == 0) ? 0 : -1;
#endif /* SENSOR_UART_ENABLE */
}

#if SENSOR_UART_ENABLE
void Mini212G2_PrintDigitalVideoStatus(void)
{
    uint8_t resp[32];
    Sensor_FlushRx();
    Sensor_SendBytes(CMD_QUERY_DIGITAL, sizeof(CMD_QUERY_DIGITAL));

    int n = Sensor_ReadResp(resp, sizeof(resp), 500);
    if (n < 24) {
        DBG_ERR("Sensor: Query failed (got %d bytes)\r\n", n);
        return;
    }

    /* Parse per protocol spec (Page 11-12):
     * Byte5 = ext-sync, Byte6 = digital port type, Byte7 = CMOS content,
     * Byte8 = CMOS interface, Byte9 = frame rate, Byte11 = clock phase */
    static const char *port_names[] = {
        "OFF", "USB2.0", "CMOS", "BT1120", "BT656",
        "USB+UART", "LCD", "LVDS", "LCD+DVP", "UVC+CDC"
    };
    static const char *content_names[] = {
        "YUV422", "YUV422+Param", "Y16", "Y16+Param",
        "Y16+YUV422", "Y16+Param+YUV422"
    };
    static const char *iface_names[] = {"CMOS16", "CMOS8(MSB)", "CMOS8(LSB)"};
    static const char *fps_names[]   = {"30Hz", "25Hz", "9Hz", "50Hz"};

    uint8_t port_type  = resp[6];
    uint8_t content    = resp[7];
    uint8_t iface      = resp[8];
    uint8_t fps        = resp[9];
    uint8_t clk_phase  = resp[11];

    DBG_INIT("Sensor Digital Video Status:\r\n");
    DBG_INIT("  Port: %s (%d)\r\n",
           (port_type < 10) ? port_names[port_type] : "?", (int)port_type);
    DBG_INIT("  Content: %s (%d)\r\n",
           (content < 6) ? content_names[content] : "?", (int)content);
    DBG_INIT("  Interface: %s (%d)\r\n",
           (iface < 3) ? iface_names[iface] : "?", (int)iface);
    DBG_INIT("  FPS: %s (%d)\r\n",
           (fps < 4) ? fps_names[fps] : "?", (int)fps);
    DBG_INIT("  ClkPhase: %s (%d)\r\n",
           (clk_phase == 0) ? "Rising" : "Falling", (int)clk_phase);
}

int Mini212G2_ShutterCompensation(void)
{
    return Mini212G2_SendCmd(CMD_SHUTTER, sizeof(CMD_SHUTTER));
}
#endif /* SENSOR_UART_ENABLE */