#include "mini212g2.h"
#include "debug.h"
#include "string.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};

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

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

    RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART2, ENABLE);
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);

    /* TX: PA2 AF push-pull */
    gpio.GPIO_Pin   = GPIO_Pin_2;
    gpio.GPIO_Speed = GPIO_Speed_50MHz;
    gpio.GPIO_Mode  = GPIO_Mode_AF_PP;
    GPIO_Init(GPIOA, &gpio);

    /* RX: PA3 floating input */
    gpio.GPIO_Pin  = GPIO_Pin_3;
    gpio.GPIO_Mode = GPIO_Mode_IN_FLOATING;
    GPIO_Init(GPIOA, &gpio);

    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
 * ============================================================ */

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);
    if (n == ACK_LEN && memcmp(resp, ACK_PATTERN, ACK_LEN) == 0)
        return 0;

    /* Debug: print what we got */
    printf("[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
    printf("[Sensor] UART disabled, assuming pre-configured via USB\r\n");
    printf("[Sensor] Expected: CMOS output, CMOS8(MSB), Y16, 30Hz\r\n");
    return 0;
#else
    int ok = 0, fail = 0;

    Sensor_UART_Init();
    printf("[Sensor] UART2 init (PA2=TX PA3=RX, 115200)\r\n");

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

    printf("[Sensor] Configuring CMOS/DVP output...\r\n");

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

    /* 2. Digital port → CMOS */
    if (Mini212G2_SendCmd(CMD_DIGITAL_CMOS, sizeof(CMD_DIGITAL_CMOS)) == 0)
        { ok++; printf("[Sensor] Digital->CMOS OK\r\n"); }
    else
        { fail++; printf("[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++; printf("[Sensor] CMOS8(MSB) OK\r\n"); }
    else
        { fail++; printf("[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++; printf("[Sensor] Y16 OK\r\n"); }
    else
        { fail++; printf("[Sensor] Y16 FAIL\r\n"); }
    Delay_Ms(200);

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

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

    printf("[Sensor] Config result: %d ok, %d fail\r\n", ok, 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) {
        printf("[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];

    printf("[Sensor] Digital Video Status:\r\n");
    printf("  Port: %s (%d)\r\n",
           (port_type < 10) ? port_names[port_type] : "?", (int)port_type);
    printf("  Content: %s (%d)\r\n",
           (content < 6) ? content_names[content] : "?", (int)content);
    printf("  Interface: %s (%d)\r\n",
           (iface < 3) ? iface_names[iface] : "?", (int)iface);
    printf("  FPS: %s (%d)\r\n",
           (fps < 4) ? fps_names[fps] : "?", (int)fps);
    printf("  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 */