#include "app_uds.h"
#include <string.h>
#include "stdio.h"
extern CAN_HandleTypeDef hcan;

uint32_t frame_seq_uds_task = 0;

/* ===================== Bootloader 启动检查 ===================== */

/* Bootloader 启动检查, 必须在 main() 的 HAL_Init 之前调用
 *
 * 原因: 此时 FreeRTOS 未启动, SysTick/PendSV/外设中断均未运行
 * 跳转环境最干净, 直接设 VTOR + MSP + 跳转即可
 *
 * 优先级1: 编程请求(APP收到0x10 0x02 → 写0x12121212 → 复位)
 *   → 停留 Bootloader, HAL_Init后清除标志
 * 优先级2: 升级标志
 *   flag=UPGRADING → 停留 Bootloader(断电续传)
 *   flag=COMPLETE/NONE → 检查栈顶 → 跳转 App
 */
void uds_bootloader_check(void)
{
    /* 如果当前已在App地址运行, 不跳转(避免App无限跳转自己)
     * 关键: App版编译的固件同样包含此函数
     * 若不加此判断, App启动后看到flag=COMPLETE会再次跳转0x08006000(自己)
     * → 无限重启循环, 永远无法正常进入App业务 */
    // if (iap_is_app_mode())
    //     return;

    /* 优先级1: 检查编程请求(APP→Bootloader通信) */
    // if (iap_check_prog_request())
    //     return;  /* 有编程请求, 停留 Bootloader */

    uint32_t prog_req = flash_read_word(IAP_PARAM_ADDR + IAP_PARAM_OFF_PROG_REQ);
    if(prog_req == IAP_PROG_REQUEST_MAGIC){
        return;
    }

    /* 优先级2: 检查升级标志 */
    uint32_t flag = iap_check_flag();

    /* flag=UPGRADING: 停留 Bootloader, 等待 IAP 升级 */
    if (flag == IAP_FLAG_UPGRADING)
        return;

    /* flag=COMPLETE 或 NONE: 尝试跳转 App */
    uint32_t app_sp = *(volatile uint32_t *)IAP_APP_ADDR;
    if ((app_sp & 0xFFFE0000) == 0x20000000)
    {
        /* 栈顶合法, 直接跳转(无需关中断, 此时没有中断在运行) */
        SCB->VTOR = IAP_APP_ADDR;
        __set_MSP(app_sp);
        uint32_t app_pc = *(volatile uint32_t *)(IAP_APP_ADDR + 4);
        ((void (*)(void))app_pc)();
    }

    /* 栈顶不合法, 清除标志, 继续运行 Bootloader */
    if (flag == IAP_FLAG_COMPLETE)
        iap_write_flag(IAP_FLAG_NONE);
}


static void System_Reset(void)
{
    // 2. 关闭全局中断，防止复位过程被干扰[reference:12]
    __set_FAULTMASK(1);
    // 或者使用 __disable_irq();
    // 3. 执行软件复位[reference:13]
    HAL_NVIC_SystemReset();
    // 4. 程序通常不会执行到这里
    while(1);
}

void uds_frame_handle(CanFrame_t *f)
{
    CanFrame_t resp = {0};
    resp.id = f->id;
#if 1
    if(f->id == 0x180)
    {
        uint8_t uds_cmd = f->data[0];
        uint8_t uds_sub_cmd = f->data[1];
        switch(uds_cmd)
        {
            case 0x10:
                switch(uds_sub_cmd)
                {
                    case 0x01:
                        // 处理 UDS 指令 0x01 0x01
                        resp.data[0] = 0x50;
                        resp.data[1] = 0x01;
                        memset(&resp.data[2], 0, 6);
                        resp.dlc = 8;
                        if(hwCanSend(resp.id, resp.data, resp.dlc)){
                            frame_seq_uds_task++;
                        }
                        break;
                    case 0x02:
                        // 处理 UDS 指令 0x01 0x02
                        //复位BMS
                        System_Reset();
                        // resp.data[0] = 0x50;    
                        // resp.data[1] = 0x02;
                        // memset(&resp.data[2], 0, 6);
                        // resp.dlc = 8;
                        // hwCanSend(resp.id, resp.data, resp.dlc);
                        break;
                    case 0x03:
                        // 处理 UDS 指令 0x01 0x03
                        resp.data[0] = 0x50;
                        resp.data[1] = 0x03;
                        memset(&resp.data[2], 0, 6);
                        resp.dlc = 8;
                        hwCanSend(resp.id, resp.data, resp.dlc);
                        break;
                    default:
                        break;
                }
                break;
            case 0x14:
                resp.data[0] = 0x54;
                memset(&resp.data[1], 0, 7);
                resp.dlc = 8;
                hwCanSend(resp.id, resp.data, resp.dlc);
                break;
            case 0x28:
                switch(uds_sub_cmd)
                {
                    case 0x01:
                        // 处理 UDS 指令 0x02 0x01
                        resp.data[0] = 0x68;
                        resp.data[1] = 0x01;
                        resp.data[2] = 0x03;
                        memset(&resp.data[3], 0, 5);
                        resp.dlc = 8;
                        hwCanSend(resp.id, resp.data, resp.dlc);
                        break;
                    case 0x02:
                        // 处理 UDS 指令 0x02 0x02
                        //复位BMS
                        resp.data[0] = 0x68;
                        resp.data[1] = 0x02;
                        resp.data[2] = 0x03;
                        memset(&resp.data[3], 0, 5);
                        resp.dlc = 8;
                        hwCanSend(resp.id, resp.data, resp.dlc);
                        break;
                    case 0x03:
                        // 处理 UDS 指令 0x02 0x03
                        resp.data[0] = 0x68;
                        resp.data[1] = 0x03;
                        resp.data[2] = 0x03;
                        memset(&resp.data[3], 0, 5);
                        resp.dlc = 8;
                        hwCanSend(resp.id, resp.data, resp.dlc);
                        break;
                    default:
                        break;
                }
                break;
            case 0x85:
                switch(uds_sub_cmd)
                {
                    case 0x01:
                        // 处理 UDS 指令 0x08 0x01
                        resp.data[0] = 0xC5;
                        resp.data[1] = 0x01;
                        memset(&resp.data[2], 0, 6);
                        resp.dlc = 8;
                        hwCanSend(resp.id, resp.data, resp.dlc);
                        break;
                    case 0x02:
                        // 处理 UDS 指令 0x08 0x02
                        resp.data[0] = 0xC5;
                        resp.data[1] = 0x02;
                        memset(&resp.data[2], 0, 6);
                        resp.dlc = 8 ;
                        hwCanSend(resp.id, resp.data, resp.dlc);
                        break;
                    default:
                        break;
                }
                break;
                default:
                    resp.data[0] = 0x7F;
                    resp.data[1] = f->data[0];
                    resp.data[2] = f->data[1];
                    memset(&resp.data[3], 0, 5);
                    resp.dlc = 8;
                    hwCanSend(resp.id, resp.data, resp.dlc);
                    break;
        }
    }
#else
    if(f->id == 0x180)
    {
        uint8_t uds_cmd = f->data[0];
        uint8_t uds_sub_cmd = f->data[1];
        switch(uds_cmd)
        {
            default:
                    resp.id = f->id;
                    resp.data[0] = 0x7F;
                    resp.data[1] = f->data[0];
                    resp.data[2] = f->data[1];
                    memset(&resp.data[3], 0, 5);
                    resp.dlc = 8;
                    hwCanSend(resp.id, resp.data, resp.dlc);
                break;
        }
    }
#endif
}