#include "system/includes.h"
#include "system/timer.h"
#include "app_log.h"
#include "r60abd1.h"
#include "radar_uart.h"
#include <string.h>


/*
 * R60ABD1协议格式：
 *
 * [0]      0x53
 * [1]      0x59
 * [2]      CTRL
 * [3]      CMD
 * [4]      DATA_LEN_H
 * [5]      DATA_LEN_L
 * [6..]    DATA
 * [-3]     CHECKSUM
 * [-2]     0x54
 * [-1]     0x43
 *
 * full_len = 9 + data_len
 */

#define R60_FRAME_HEADER1       0x53
#define R60_FRAME_HEADER2       0x59
#define R60_FRAME_TAIL1         0x54
#define R60_FRAME_TAIL2         0x43

#define R60_CTRL_PRESENCE       0x80
#define R60_CTRL_BREATH         0x81
#define R60_CTRL_SLEEP          0x84
#define R60_CTRL_HEART          0x85

#define R60_PARSE_BUF_SIZE      128


#ifndef R60ABD1_DEBUG_LOG
#define R60ABD1_DEBUG_LOG       1
#endif


#if R60ABD1_DEBUG_LOG
#define R60_LOG(...)            APP_LOG(__VA_ARGS__)
#else
#define R60_LOG(...)
#endif


static u8 r60_parse_buf[R60_PARSE_BUF_SIZE];
static u16 r60_parse_len;

static u32 r60_rx_bytes;
static u32 r60_valid_frame_count;
static u32 r60_data_update_count;
static u32 r60_checksum_error_count;

static struct radar_r60abd1_data radar_data;

static OS_MUTEX radar_data_mutex;
static u8 radar_data_mutex_ready;


/*
 * R60坐标格式：
 *
 * bit15    = 符号位
 * bit14..0 = 数值绝对值
 */
static s16 r60_parse_signed_coordinate(u16 raw_value)
{
    s16 value = (s16)(raw_value & 0x7fff);

    if (raw_value & 0x8000) {
        value = -value;
    }

    return value;
}


/*
 * 协议层初始化。
 *
 * 必须在：
 * radar_r60abd1_input_bytes()
 * radar_r60abd1_get_data()
 *
 * 之前调用。
 */
int radar_r60abd1_init(void)
{
    memset(r60_parse_buf, 0, sizeof(r60_parse_buf));
    memset(&radar_data, 0, sizeof(radar_data));

    r60_parse_len = 0;
    r60_rx_bytes = 0;
    r60_valid_frame_count = 0;
    r60_data_update_count = 0;
    r60_checksum_error_count = 0;

    if (!radar_data_mutex_ready) {
        os_mutex_create(&radar_data_mutex);
        radar_data_mutex_ready = 1;
    }

    R60_LOG("[R60] init success\n");

    return 0;
}


/*
 * 完整帧合法性校验。
 */
static int r60_validate_frame(const u8 *frame, u16 frame_len)
{
    u8 checksum = 0;
    u16 data_len;
    u16 i;

    if (!frame || frame_len < 9) {
        return -1;
    }

    if (frame[0] != R60_FRAME_HEADER1 ||
        frame[1] != R60_FRAME_HEADER2) {
        return -2;
    }

    data_len =
        ((u16)frame[4] << 8) |
        frame[5];

    if ((u16)(9 + data_len) != frame_len) {
        return -3;
    }

    if (frame[frame_len - 2] != R60_FRAME_TAIL1 ||
        frame[frame_len - 1] != R60_FRAME_TAIL2) {
        return -4;
    }

    /*
     * checksum:
     *
     * frame[0] 到 DATA最后一个字节之和。
     * 不包含checksum本身和帧尾。
     */
    for (i = 0; i < frame_len - 3; i++) {
        checksum += frame[i];
    }

    if (checksum != frame[frame_len - 3]) {

        if (radar_data_mutex_ready) {

            os_mutex_pend(&radar_data_mutex, 0);

            r60_checksum_error_count++;
            radar_data.checksum_errors =
                r60_checksum_error_count;

            os_mutex_post(&radar_data_mutex);

        } else {

            r60_checksum_error_count++;
        }

        R60_LOG("[R60] checksum error: calc=%02X recv=%02X\n",
                checksum,
                frame[frame_len - 3]);

        return -5;
    }

    return 0;
}


/*
 * 业务解析。
 *
 * 只负责：
 * 1. CTRL/CMD解释
 * 2. 更新RAM中的最新状态
 *
 * 不做：
 * Flash
 * 网络上传
 */
static int r60_parse_frame(const u8 *frame, u16 frame_len)
{
    u8 ctrl;
    u8 cmd;
    u16 data_len;
    int updated = 0;

    if (!frame || frame_len < 9) {
        return 0;
    }

    if (!radar_data_mutex_ready) {
        return 0;
    }

    ctrl = frame[2];
    cmd = frame[3];

    data_len =
        ((u16)frame[4] << 8) |
        frame[5];


    os_mutex_pend(&radar_data_mutex, 0);


    switch (ctrl) {

    /*
     * 人体存在 / 运动 / 距离 / 坐标
     */
    case R60_CTRL_PRESENCE:

        switch (cmd) {

        case 0x01:
        case 0x81:

            if (data_len >= 1) {
                radar_data.presence = frame[6];
                updated = 1;

                R60_LOG("[R60][PARSE] presence=%u\n",
                        radar_data.presence);
            }

            break;


        case 0x02:

            if (data_len >= 1) {
                radar_data.motion = frame[6];
                updated = 1;

                R60_LOG("[R60][PARSE] motion=%u\n",
                        radar_data.motion);
            }

            break;


        case 0x03:

            if (data_len >= 1) {
                radar_data.body_movement = frame[6];
                updated = 1;

                R60_LOG("[R60][PARSE] body_movement=%u\n",
                        radar_data.body_movement);
            }

            break;


        case 0x04:

            if (data_len >= 2) {

                radar_data.distance_cm =
                    ((u16)frame[6] << 8) |
                    frame[7];
                updated = 1;

                R60_LOG("[R60][PARSE] distance=%u cm\n",
                        radar_data.distance_cm);
            }

            break;


        case 0x05:

            if (data_len >= 6) {

                radar_data.pos_x_mm =
                    r60_parse_signed_coordinate(
                        ((u16)frame[6] << 8) |
                        frame[7]);

                radar_data.pos_y_mm =
                    r60_parse_signed_coordinate(
                        ((u16)frame[8] << 8) |
                        frame[9]);

                radar_data.pos_z_mm =
                    r60_parse_signed_coordinate(
                        ((u16)frame[10] << 8) |
                        frame[11]);
                updated = 1;

                R60_LOG("[R60][PARSE] pos=(%d,%d,%d) mm\n",
                        radar_data.pos_x_mm,
                        radar_data.pos_y_mm,
                        radar_data.pos_z_mm);
            }

            break;


        default:
            break;
        }

        break;


    /*
     * 呼吸
     */
    case R60_CTRL_BREATH:

        switch (cmd) {

        case 0x02:
        case 0x82:

            if (data_len >= 1) {

                radar_data.breath_rate_x10 =
                    (u16)frame[6] * 10;
                updated = 1;

                R60_LOG("[R60][PARSE] breath=%u /min\n",
                        radar_data.breath_rate_x10 / 10);
            }

            break;


        case 0x05:

            if (data_len >= 1) {
                radar_data.breath_waveform = frame[6];
                updated = 1;
            }

            break;


        default:
            break;
        }

        break;


    /*
     * 心率
     */
    case R60_CTRL_HEART:

        switch (cmd) {

        case 0x02:
        case 0x82:

            if (data_len >= 1) {

                radar_data.heart_rate_x10 =
                    (u16)frame[6] * 10;
                updated = 1;

                R60_LOG("[R60][PARSE] heart=%u bpm\n",
                        radar_data.heart_rate_x10 / 10);
            }

            break;


        case 0x05:

            if (data_len >= 1) {
                radar_data.heart_waveform = frame[6];
                updated = 1;
            }

            break;


        default:
            break;
        }

        break;


    /*
     * 睡眠
     */
    case R60_CTRL_SLEEP:

        switch (cmd) {

        case 0x0c:
        case 0x8d:

            if (data_len >= 2) {

                radar_data.presence = frame[6];
                radar_data.sleep_state = frame[7];
                updated = 1;

                R60_LOG("[R60][PARSE] sleep presence=%u state=%u\n",
                        radar_data.presence,
                        radar_data.sleep_state);
            }

            break;


        default:
            break;
        }

        break;


    /*
     * 手册已确认、但当前公共状态结构尚未承载的合法帧，例如：
     *
     * ctrl=07 cmd=07
     * ctrl=81 cmd=01
     *
     * 本阶段暂不映射，避免顺带扩展业务字段。
     */
    default:
        break;
    }


    /*
     * 通信统计和业务新鲜度严格分开：
     *
     * frame_count:
     *   所有checksum正确的合法帧都会增加。
     *
     * data_update_count / update_ms:
     *   只有当前parser真正识别并更新业务字段时才推进。
     *
     * 因此07/07、01/01等未知合法帧不会把旧数据“续鲜”。
     */
    radar_data.rx_bytes = r60_rx_bytes;
    radar_data.frame_count = r60_valid_frame_count;
    radar_data.checksum_errors =
        r60_checksum_error_count;

    if (updated) {
        r60_data_update_count++;
        radar_data.data_update_count =
            r60_data_update_count;
        radar_data.update_ms = timer_get_ms();
    } else {
        R60_LOG(
            "[R60] ignored business frame ctrl=%02X cmd=%02X "
            "frame=%u data=%u\n",
            ctrl,
            cmd,
            r60_valid_frame_count,
            r60_data_update_count
        );
    }


    os_mutex_post(&radar_data_mutex);

    return updated;
}


/*
 * 收到一帧完整数据。
 */
static void r60_frame_received(const u8 *frame, u16 frame_len)
{
    int ret;
    u32 frame_count;

    ret = r60_validate_frame(frame, frame_len);

    if (ret != 0) {

        R60_LOG("[R60] invalid frame, ret=%d\n",
                ret);

        return;
    }


    if (!radar_data_mutex_ready) {
        return;
    }


    os_mutex_pend(&radar_data_mutex, 0);

    r60_valid_frame_count++;

    radar_data.frame_count =
        r60_valid_frame_count;

    frame_count =
        r60_valid_frame_count;

    os_mutex_post(&radar_data_mutex);


    R60_LOG("[R60] valid frame=%u len=%u ctrl=%02X cmd=%02X\n",
            frame_count,
            frame_len,
            frame[2],
            frame[3]);


    r60_parse_frame(frame, frame_len);
}


/*
 * 逐字节组帧。
 */
static void r60_feed_byte(u8 byte)
{
    u16 data_len;
    u16 full_len;


    /*
     * 找0x53。
     */
    if (r60_parse_len == 0) {

        if (byte != R60_FRAME_HEADER1) {
            return;
        }

        r60_parse_buf[0] = byte;
        r60_parse_len = 1;

        return;
    }


    /*
     * 已收到0x53，找0x59。
     */
    if (r60_parse_len == 1) {

        if (byte == R60_FRAME_HEADER2) {

            r60_parse_buf[1] = byte;
            r60_parse_len = 2;

            return;
        }

        /*
         * 53 53：
         * 后一个53仍可能是新帧头。
         */
        if (byte == R60_FRAME_HEADER1) {
            return;
        }

        r60_parse_len = 0;

        return;
    }


    /*
     * parser buffer保护。
     */
    if (r60_parse_len >= sizeof(r60_parse_buf)) {

        R60_LOG("[R60] frame buffer overflow\n");

        r60_parse_len = 0;

        return;
    }


    r60_parse_buf[r60_parse_len++] = byte;


    /*
     * 前6字节收齐后才能读取DATA_LEN。
     */
    if (r60_parse_len < 6) {
        return;
    }


    data_len =
        ((u16)r60_parse_buf[4] << 8) |
        r60_parse_buf[5];


    full_len =
        (u16)(9 + data_len);


    if (full_len > sizeof(r60_parse_buf)) {

        R60_LOG("[R60] invalid frame length: data=%u full=%u\n",
                data_len,
                full_len);

        r60_parse_len = 0;

        return;
    }


    if (r60_parse_len < full_len) {
        return;
    }


    r60_frame_received(
        r60_parse_buf,
        full_len
    );


    r60_parse_len = 0;
}


/*
 * UART层收到数据后调用。
 */
int radar_r60abd1_input_bytes(const u8 *data, u16 len)
{
    u16 i;

    if (!data || !len) {
        return -1;
    }

    if (!radar_data_mutex_ready) {
        return -2;
    }


    /*
     * 统计UART输入字节数。
     */
    os_mutex_pend(&radar_data_mutex, 0);

    r60_rx_bytes += len;
    radar_data.rx_bytes = r60_rx_bytes;

    os_mutex_post(&radar_data_mutex);


    /*
     * 逐字节送入组帧器。
     */
    for (i = 0; i < len; i++) {
        r60_feed_byte(data[i]);
    }

    return 0;
}


/*
 * 获取当前最新雷达状态快照。
 *
 * 可供其他任务调用。
 */
int radar_r60abd1_get_data(struct radar_r60abd1_data *out)
{
    if (!out) {
        return -1;
    }

    if (!radar_data_mutex_ready) {
        return -2;
    }


    os_mutex_pend(&radar_data_mutex, 0);

    memcpy(out,
           &radar_data,
           sizeof(*out));

    os_mutex_post(&radar_data_mutex);


    return 0;
}


/*
 * 向R60ABD1发送原始协议命令。
 *
 * 协议层只负责把完整命令交给UART层发送。
 */
int radar_r60abd1_send_command(const u8 *cmd, u16 len)
{
    if (!cmd || !len) {
        return -1;
    }

    return radar_uart_write(
               cmd,
               len
           );
}