#include "cm_iomux.h"
#include "cm_gpio.h"
#include "stdio.h"
#include "stdlib.h"
#include "stdarg.h"
#include "cm_os.h"
#include "cm_mem.h"
#include "cm_sys.h"
#include "cm_uart.h"

#include "app_common.h"
#include "app_uart.h"
#include "control_out.h"
#include "radar.h"
#include "local_tts.h"


#define RADAR_DEBUG_ENABLE 0

#if RADAR_DEBUG_ENABLE
#include "app_uart.h"
#define DEBUG(fmt, args...) app_printf("[RADAR]" fmt, ##args)
#else
#define DEBUG(fmt, ...)
#endif

static osThreadId_t os_RADAR_ThreadId;
radar_data_t radar_data;
osMutexId_t radar_mutex; // 互斥锁
uint8_t  old_state = 0;
/******************************************************************************
 * Name:    CRC-16/MODBUS
 * Poly:    0x8005  ( x16+x15+x2+1 )
 * Init:    0xFFFF
 * Refin:   True
 * Refout:  True
 * Xorout:  0x0000
 *****************************************************************************/
uint16_t crc16_modbus(uint8_t *data, uint16_t length){
    uint8_t i;
    uint16_t crc = 0xFFFF; // Initial value
    while(length--){
        crc ^= *data++;
        for (i = 0; i < 8; ++i){
            if (crc & 1){
                crc = (crc >> 1) ^ 0xA001; // 0xA001 = reverse 0x8005
            }else{
                crc = (crc >> 1);
            }
        }
    }
    return crc;
}

// 自动刹车或减速判断处理函数
static void radar_AUTO_BrakeORSpeedCut(uint8_t radar_id , uint16_t Car_Distance){ //距离数据30~4500mm
    // 自动刹车距离
    uint16_t Brake_Distance     = ((radar_id == RADAR_ID_Back )? SYS_CONF_BRAKE_LIMIT_B : SYS_CONF_BRAKE_LIMIT);
    // 自动减速距离
    uint16_t Speed_Cut_Distance = ((radar_id == RADAR_ID_Back )? SYS_CONF_SPEED_CUT_LIMIT_B : SYS_CONF_SPEED_CUT_LIMIT);
      

    if((35 < Car_Distance) && Car_Distance < Brake_Distance){//小于自动刹车距离时
        if(!sys_sta.A_brake){
            // local_tts_set(5, 15, CM_LOCAL_TTS_DIGIT_AUTO);
            local_tts_text_play("刹车",0,0);
            // Time_Event_Blink(Buzzer_Event,100,100,0xffff,NULL);
        }
        sys_sta.A_brake =1;//使能自动刹车
        sys_sta.A_Speed_Cut=1;
    }else if((35 < Car_Distance) && Car_Distance < Speed_Cut_Distance){//小于自动减速距离时
        if(sys_sta.A_brake || !sys_sta.A_Speed_Cut){
            // local_tts_set(5, 15, CM_LOCAL_TTS_DIGIT_AUTO);
            local_tts_text_play("减速",0,0);
            // Time_Event_Blink(Buzzer_Event,200,500,0xffff,NULL);
        }
        sys_sta.A_brake =0;
        sys_sta.A_Speed_Cut=1;//使能自动减速
    }else{
        if(sys_sta.A_brake || sys_sta.A_Speed_Cut){
            // local_tts_text_play("自动刹车",0,0);
            // Time_Event_Off(Buzzer_Event,NULL);
        }
        sys_sta.A_brake =0;
        sys_sta.A_Speed_Cut=0;
    }
   
  
}

static uint8_t radar_CMDSend_cnt=0; // 发送命令计数
static uint8_t radar_CMDReceive_cnt=0; // 接收命令计数

//发送读取实时值数据命令
void radar_Sendcmd(uint8_t radar_id , uint8_t radar_mode){
    uint8_t len=0;
    uint8_t data[8];

    data[len++] = radar_id;
    data[len++] = 0x03;
    data[len++] = 0x01;
    data[len++] = radar_mode ? 0x01 : 0x00; // 0x01:实时值(120ms) 0x00:处理值(520ms)
    data[len++] = 0x00;
    data[len++] = 0x01;

    uint16_t crc16 =crc16_modbus(data,len);
    data[len++] = crc16 & 0xff;
    data[len++] = (crc16 >> 8) & 0xff;
    uart0_send_msg(data,len,0);
    radar_CMDSend_cnt++;
}

// 数据接收处理函数
void radar_CheckData(uint8_t *data, uint16_t data_len){
    if(data_len != 7){ // 数据长度不正确
        return;
    }
    uint16_t crc16 =crc16_modbus(data, 5);
    if((data[0] == RADAR_ID_Front) || (data[0] == RADAR_ID_Back) ||
       (crc16 == (((uint16_t)data[6] << 8 ) | data[5]))){   // 雷达ID匹配
        radar_data.radar_id = data[0];
        radar_data.distance = ((uint16_t)data[3] << 8 ) | data[4];
        radar_AUTO_BrakeORSpeedCut(radar_data.radar_id ,radar_data.distance);
        DEBUG("radar_id:%#02x,distance:%d", radar_data.radar_id, radar_data.distance);
        radar_CMDReceive_cnt++;
    }
}

// 定时处理数和获取数据
static void RADAR_TaskHandle(void *param){
    uint8_t temp_count =0;
    uint32_t time_count =0;
    while(1){
    
       if(old_state != jt808_term_param_item.set_term_param.RadarEN)
        {
          sys_sta.P_Radar_EN = jt808_term_param_item.set_term_param.RadarEN;
          old_state = jt808_term_param_item.set_term_param.RadarEN;

          if (0==sys_sta.P_Radar_EN)
          {
            local_tts_text_play("雷达关闭",0,0);
          }
          
          if (1==sys_sta.P_Radar_EN)
          {
            local_tts_text_play("雷达开启",0,0);
          }

          if (2==sys_sta.P_Radar_EN)
          {
            local_tts_text_play("雷达临时关闭",0,0);
          }

        }
        DEBUG("radar task init\r\n");    
        
      if(0==sys_sta.P_Radar_EN)
     {
        sys_sta.A_Speed_Cut =0; // 清空自动减速状态
        sys_sta.A_brake =0; // 清空自动刹车状态
        DEBUG("radar close\r\n"); // 任务开始

        continue;
     }
    
     DEBUG("radar open\r\n"); // 任务开始

    osMutexAcquire(radar_mutex, osWaitForever);    
    if (1==sys_sta.P_Radar_EN)
    {
        if(radar_CMDSend_cnt != radar_CMDReceive_cnt){ // 发送命令和接收命令计数不一致时，复位自动刹车和减速状态
            temp_count++;
            if(temp_count > 10){ // 连续10次未收到数据，复位自动刹车和减速状态
                temp_count =0;
                radar_CMDSend_cnt =0;  // 清空发送命令计数
                radar_CMDReceive_cnt =0;  // 清空接收命令计数
                DEBUG("radar anomaly......"); // 雷达异常
                radar_AUTO_BrakeORSpeedCut(radar_data.radar_id , 0); // 复位自动刹车和减速状态
            }
        }
        

        // 进入游客模式开启雷达 //管理员模式优先于游客模式
        if(((0 != sys_sta.O_door_lock) && ((1 == sys_sta.MAG_MODE)&&(1 == sys_sta.PLT_MODE ))) && (1 == sys_sta.P_Radar_EN)){
            // 根据倒车状态确定雷达ID
            // DEBUG("SendCMD:ID=%#02x", (0 == sys_sta.IO_RX_back)?RADAR_ID_Back:RADAR_ID_Front);
            radar_Sendcmd((0 == sys_sta.IO_RX_back)?RADAR_ID_Back:RADAR_ID_Front, RADAR_MODE_Real);
            
           //      DEBUG("radar 1\r\n"); // 任务结束
        }else{
            sys_sta.A_Speed_Cut =0; // 清空自动减速状态
            sys_sta.A_brake =0; // 清空自动刹车状态
                             DEBUG("radar 1\r\n"); // 任务结束

        }
        
      }

    else if (2==sys_sta.P_Radar_EN)
    {
        
      time_count++;
      sys_sta.A_Speed_Cut =0; // 清空自动减速状态
      sys_sta.A_brake =0; // 清空自动刹车状态

        if(time_count>=400)
      {
        sys_sta.P_Radar_EN = 1;
        time_count=0;

    
      }
    

         DEBUG("radar 2\r\n"); // 任务结束

    }

        osMutexRelease(radar_mutex);  
        osDelay(140/5); // 140ms
   }
}

// 雷达测离初始化
void radar_init(void){
    radar_data.radar_id = RADAR_ID_Front;
    radar_data.distance = 0;
    radar_mutex = osMutexNew(NULL);
    osThreadAttr_t radar_task_attr = {
        .name = "uart_tx_task",
       .stack_size = 4096*4,
       .priority= osPriorityBelowNormal
    };
    os_RADAR_ThreadId= osThreadNew(RADAR_TaskHandle, 0, &radar_task_attr);
}