#include <string.h>
#include <stdio.h>
#include <stdlib.h>
#include <stdarg.h>
#include <stdarg.h>

#include "Debug.h"
#include "main.h"
#include "bsp.h"
#include "dc_motor.h"

#if (USE_DEBUG != 0)
#define DEBUG_CMD_CNT					8
const DBGFunType DebugFun[DEBUG_CMD_CNT];

static char buff[BUFSIZE];
volatile uint16_t iw=0;  /* buffer write index*/
static uint16_t ir=0;  /* buffer read index*/
static uint16_t DebugRxTimeOut1mSCnt;
static char RxBuff[128];
static uint8_t RxLen = 0;

void vcom_Print(uint8_t sLen);

void vcom_Send( char *format, ... )
{
  va_list args;
  va_start(args, format);
  uint8_t len;
  char tempBuff[DEBUG_BUFF_SIZE_MAX];
	//uint32_t primask_bit;
	uint8_t offset = 0;
  //primask_bit = __get_PRIMASK();
  //__disable_irq();
  
  /*convert into string at buff[0] of length iw*/
  len = vsprintf(&tempBuff[0], format, args);

	while(offset < len) {
		if((len - offset) < BUFSIZE) {
			memcpy(&buff[0], &tempBuff[offset], len - offset);
			vcom_Print(len - offset);
			offset = len;
		}
		else {
			memcpy(&buff[0], &tempBuff[offset], BUFSIZE);
			offset += BUFSIZE;
			vcom_Print(BUFSIZE);
		}
	}

  //__set_PRIMASK(primask_bit);
	//__enable_irq();
	
  va_end(args);
}

// 定义一个函数vcom_Print，用于打印字符串
void vcom_Print(uint8_t sLen)
{
  // 定义一个字符指针CurChar
  char* CurChar;
  // 初始化ir
  ir = 0;
  // 当ir小于sLen时，循环执行
  while(ir < sLen)
  {
    // 获取字符指针CurChar
    CurChar = &buff[ir++];
    // 调用DebugUartSend函数，发送CurChar指向的字符，次数为1
		DebugUartSend((uint8_t*)CurChar, 1);
  }
}

// 定义一个函数vcom_Send2，用于发送字符串
void vcom_Send2(uint8_t *sData, uint16_t len)
{
  // 遍历sData，将每个元素发送到DebugUartSend
	for(int i = 0; i < len; i++) {
		DebugUartSend(&sData[i], 1);
	}
}

void DebugLoopHandler(void)
{
	int argc = 0;
	char *argv[10], *argp;
	
	if(DebugRxTimeOut1mSCnt > 0 || RxLen == 0)
		return;
	
	//for(argv[argc] = strtok(RxBuff, " "); argv[argc] != NULL; argv[++argc] = strtok(NULL, " "));
	// 解析字符串，将参数放入argv数组中
	argp = strtok(RxBuff, " ");
	for(int i = 0; i < 10; i++) {
		if(argp != NULL) {
			argv[argc++] = argp;
			argp = strtok(NULL, " ");
		}
		else if(argc > 0){
			argc--;
			break;
		}
	}

	// 获取第一个参数，并将其赋值给argp
	argp = strtok(argv[argc], "'\r'");
	// 如果argp不为空，则将argp的值赋值给argv[argc]，并将argc加1
	if(argp != NULL) {
		argv[argc++] = argp;
	}
	// 否则，将RxLen设置为0，并返回
	else {
		RxLen = 0;
		return;
	}
	
	// 遍历DebugFun数组，查找argv[0]是否与DBGCmd匹配，若匹配则调用DBGExec函数
	for(int i = 0; i < DEBUG_CMD_CNT; i++) {
		if(strcmp(argv[0], DebugFun[i].DBGCmd) == 0)
			DebugFun[i].DBGExec(argc, argv);
	}
    
	memset(RxBuff, 0x00, 128);
	RxLen = 0;
}



uint32_t RxTimeCurrent;
uint32_t RxTimeLast = 0;

// 函数：Dbg1msRoutine
// 功能：1ms调试时钟routine
void Dbg1msRoutine(void)
{
	// 如果1ms调试时钟计数器大于0
	if(DebugRxTimeOut1mSCnt > 0)
		// 1ms调试时钟计数器减1
		DebugRxTimeOut1mSCnt--;
}

// 定义函数DbgUsartRxIrqCallback，用于处理串口接收中断
void DbgUsartRxIrqCallback(void)
{
    // 如果1分钟超时计数器为0，则清空RxLen
    if(DebugRxTimeOut1mSCnt == 0) 
        RxLen = 0;
    
    // 重置1分钟超时计数器
    DebugRxTimeOut1mSCnt = 3;
    // 读取串口接收到的数据
    uint8_t Data = DbgUartRec();
    // 如果RxLen小于128，则将数据添加到RxBuff中
    if(RxLen < 128) {
        RxBuff[RxLen++] = Data;
    }
//    DBG_LOG("RxBuff = %s\n", RxBuff);
//    RS485_Flag=1;//接收标志为置1
}

void DebugTest(int argc, char *argv[])
{
    // 如果参数个数小于2，则返回
    if(argc < 2)
        return;
    // 如果参数为“?”，则打印帮助信息
    if(strcmp(argv[1], "?") == 0)
    {
        DBG_LOG("Debug Cmd:\r\n\
      disp task on/off -->Open the task display.\r\n");
        return;
    }
    // 如果参数为“on”，则打印“Test ON”
    if(strcmp(argv[1], "on") == 0)
    {
//        TestOn();
        DBG_LOG("Test ON\r\n");
    }
    // 如果参数为“off”，则打印“Test OFF”
    else if(strcmp(argv[1], "off") == 0)
    {
//        TestOFF();
        DBG_LOG("Test OFF\r\n");
    }
}



#if 0
void DebugCmdGps(int argc, char *argv[])
{
	if(argc < 2)
		return;
	
	if(strcmp(argv[1], "?") == 0) {
		DBG_LOG("Debug Cmd:\r\n\
      gps on/off -->Turn the gps on or off.\r\n");
		return;
	}

	if(strcmp(argv[1], "on") == 0) {
		GPSStart();
	}
	else if(strcmp(argv[1], "off") == 0) {
		GPSStop();
	}
}
#endif


void DebugCmdHelp(int argc, char *argv[])
{
	DBG_LOG("Debug Cmd:\r\n\
        disp task on/off -->Open the task display.\r\n\
        help\
        motor\
		  level\
		  wave\
        ");
}

#if 0
//// 定义一个无参函数DebugReadData，用于读取数据
//void DebugReadData(int argc, char *argv[])
//{
//	// 定义一个无符号32位整数ADReadStart，初始值为0
//	uint32_t ADReadStart = 0;	
//	// 定义一个ADData_t类型的数组ADBuff，大小为640
//	ADData_t ADBuff[640]; 
//	
//	// 当ADReadStart小于SDCARD_BLOCK_MAX时，循环执行
//	while(ADReadStart < SDCARD_BLOCK_MAX) {
//		// 使用SDCardReadBlocks函数读取数据，从ADReadStart开始，读取10个块，数据存储在ADBuff中
//		SDCardReadBlocks(ADReadStart, 10, (uint8_t *)ADBuff);
//		// 遍历ADBuff数组，将每个元素打印出来
//		for(int i = 0; i < 640; i++) {
//			DBG_LOG("%d\t%d\t%d\t%d\r\n", ADBuff[i].AD0, ADBuff[i].AD1, ADBuff[i].AD2, ADBuff[i].AD3);
//		}
//		// ADReadStart加10
//		ADReadStart += 10;
//	}
//}



//// 定义一个函数SetTime，参数为int argc和char *argv[]
//void SetTime(int argc, char *argv[])
//{
//    // 定义一个字符数组test，并初始化为0
//    char test[10] = 0;
//    // 定义一个整数num，并初始化为0
//    int num = 0;
//    // 如果argc小于2，则返回
//    if(argc < 2)
//        return;
//    // 如果argv[1]等于"?"，则打印提示信息
//    if(strcmp(argv[1], "?") == 0)
//    {
//        DBG_LOG("SetTime Cmd:\r\n\
//      disp task on/off -->Open the task display.\r\n");
//        return;
//    }
//    // 将argv[1]的值复制到test
//    memcpy(test, (char *)argv[1], 10);
//    // 将test转换为整数
//    num = atoi(test);
//    // 调用TimeSync函数，并将num作为参数传入
//    TimeSync(num);
//}
#endif
extern uint8_t KeyASta;
extern uint8_t KeyBSta;
extern uint8_t RunSta;
void MotorSwitch(int argc, char *argv[])
{
    // 如果参数个数小于2，则返回
    if(argc < 2)
        return;
    // 如果第一个参数为？，则打印帮助信息
    if(strcmp(argv[1], "?") == 0)
    {
        DBG_LOG("DCMotor Cmd:\r\n\
      motor F/R/S -->Turn the motor on or off.\r\n");
        return;
    }
    // 如果第一个参数为on，则开启SonarWork
    if(strcmp(argv[1], "R") == 0)
    {
        KeyASta = true;
        Motor_Control(MOTOR_CMD_REVERSE);
        DBG_LOG("Motor FORWARD\r\n");
    }
    // 如果第一个参数为off，则关闭SonarWork
    else if(strcmp(argv[1], "F") == 0)
    {
        KeyBSta = true;
        Motor_Control(MOTOR_CMD_FORWARD);
        DBG_LOG("Motor REVERSE\r\n");
    }
    else if(strcmp(argv[1], "S") == 0)
    {
        KeyASta = false;
        KeyBSta = false;
        Motor_Control(MOTOR_CMD_STOP);
        DBG_LOG("Motor OFF\r\n");
    }
}
extern void GradeControl(ControlLevel_m level);
void LevelSwitch(int argc, char *argv[])
{
    // 如果参数个数小于2，则返回
    if(argc < 2)
        return;
    // 如果参数为“?”，则打印帮助信息
    if(strcmp(argv[1], "?") == 0)
    {
        DBG_LOG("level Cmd:\r\n\
      level 1/2/3/4/5 -->Select control level.\r\n");
        return;
    }
    // 如果参数为“1”，则打印“Set control level: 1”
    if(strcmp(argv[1], "1") == 0)
    {
		App.Para.Layout.ControlLevel = LEVEL_1;//等级1
		SAVE_APP_PARA((uint32_t *)&App.Para);
		DBG_LOG("Set control level: 1\r\n");
		DBG_LOG("SystemReset...\r\n");
		NVIC_SystemReset();
    }
     // 如果参数为“2”，则打印“Set control level: 2”
    else if(strcmp(argv[1], "2") == 0)
    {
		App.Para.Layout.ControlLevel = LEVEL_2;//等级2
		SAVE_APP_PARA((uint32_t *)&App.Para);
		DBG_LOG("Set control level: 2\r\n");
		DBG_LOG("SystemReset...\r\n");
		NVIC_SystemReset();
    }
	 // 如果参数为“3”，则打印“Set control level: 1”
    else if(strcmp(argv[1], "3") == 0)
    {
		App.Para.Layout.ControlLevel = LEVEL_3;//等级3
		SAVE_APP_PARA((uint32_t *)&App.Para);
		DBG_LOG("Set control level: 3\r\n");
		DBG_LOG("SystemReset...\r\n");
		NVIC_SystemReset();
    }
	// 如果参数为“4”，则打印“Set control level: 4”
    else if(strcmp(argv[1], "4") == 0)
    {
		App.Para.Layout.ControlLevel = LEVEL_4;//等级4
		SAVE_APP_PARA((uint32_t *)&App.Para);
		DBG_LOG("Set control level: 4\r\n");
		DBG_LOG("SystemReset...\r\n");
		NVIC_SystemReset();
    }
	 // 如果参数为“5”，则打印“Set control level: 5”
   else  if(strcmp(argv[1], "5") == 0)
    {
		App.Para.Layout.ControlLevel = LEVEL_5;//等级5
		SAVE_APP_PARA((uint32_t *)&App.Para);
		DBG_LOG("Set control level: 5\r\n");
		DBG_LOG("SystemReset...\r\n");
		NVIC_SystemReset();
    }
}

void DBGWave_on_off(int argc, char *argv[])
{
	 // 如果参数个数小于2，则返回
    if(argc < 2)
        return;
    // 如果参数为“?”，则打印帮助信息
    if(strcmp(argv[1], "?") == 0)
    {
        DBG_LOG("wave Cmd:\r\n\
      wave on/off -->Turn on or off waveform output.\r\n");
        return;
    }
    // 如果参数为“on”，则打印“Test ON”
    if(strcmp(argv[1], "on") == 0)
    {
		 App.Para.Layout.DBGWaveOut = true;
		 SAVE_APP_PARA((uint32_t *)&App.Para);
        DBG_LOG("Waveform output on.\r\n");
    }
    // 如果参数为“off”，则打印“Test OFF”
    else if(strcmp(argv[1], "off") == 0)
    {
		 App.Para.Layout.DBGWaveOut = false;
		 SAVE_APP_PARA((uint32_t *)&App.Para);
        DBG_LOG("Waveform output off.\r\n");
    }
}
const DBGFunType DebugFun[DEBUG_CMD_CNT] = {
	"help",				DebugCmdHelp,
   "motor",          MotorSwitch,
	"level",				LevelSwitch,
	"wave",				DBGWave_on_off,
};

/*=========================================================================================
* 函数名称: Uart3ErrLog_xxx
* 功能描述: 控制盒报错(01 04)→复位→恢复 观测打印。接收中断中仅推事件入环形缓冲(不打印),
*           由主循环Uart3ErrLogLoopHandler()统一打印, 完整呈现"触发报错→置复位标志→
*           复位过程多包回应→复位完成恢复正常"的时间线。
* 协议:     01 04 d2 d3 = 报错帧(d2/d3为错误码, 如过热); 01 01 h_hi h_lo = 高度帧(清除复位标志)
*=========================================================================================*/
#define U3ERR_EVT_MAX       16      /* 事件缓冲条数 */
#define U3ERR_STATUS_NUM    12      /* 主状态机状态个数 */

/* 事件类型 */
#define U3ERR_T_FIRST       0       /* 首次信息帧(复位标志 0→1) */
#define U3ERR_T_REPLY       1       /* 复位中回应信息帧(复位标志已置位) */
#define U3ERR_T_DONE        2       /* 复位完成(收到高度帧, 复位标志 1→0) */
#define U3ERR_T_UNKNOWN     3       /* 未知帧(非高度/信息帧, 用于捕捉过热等其它报错) */

typedef struct{
	uint8_t  Type;              /* 事件类型 */
	uint8_t  Bits;              /* BIT0/BIT1 输出电平(bit0/bit1) */
	uint8_t  Status;            /* 事件时刻主状态机状态 */
	uint8_t  Frame[4];          /* 原始4字节帧 */
	uint16_t Height;            /* type2: 高度帧数值(mm) */
	uint16_t Repeat;            /* 连发合并的重复包数(不含首包) */
	uint32_t Tick;              /* 事件时刻(ms, 上电起) */
}U3ErrEvt_t;

/* 连发合并状态: 相同帧500ms内只计数不重复入队, 防止过热帧持续广播刷爆缓冲 */
static uint8_t  m_U3ErrLastType = 0xFF;         /* 上一帧类型(0xFF=无) */
static uint8_t  m_U3ErrLastFrame[4];            /* 上一帧内容 */
static uint16_t m_U3ErrLastHeight = 0;          /* 上一帧高度(type2) */
static uint32_t m_U3ErrLastTick = 0;            /* 上一帧时刻 */
static uint16_t m_U3ErrRepeat = 0;              /* 上一帧已合并的重复包数 */

static const char *m_U3ErrStatusName[U3ERR_STATUS_NUM] = {
	"IDLE", "ON", "OFF", "START_BEEP", "MOTOR_INIT", "ZERO_MARKING",
	"WAIT_MOTOR_INIT", "WAIT_MOTOR_CALI_UP", "WAIT_MOTOR_CALI_DOWN",
	"WAIT_STEADY", "END_BEEP", "HEIGHT_CONTROL"
};

static U3ErrEvt_t m_U3ErrEvt[U3ERR_EVT_MAX];    /* 事件环形缓冲 */
static volatile uint16_t m_U3ErrWr = 0;         /* 写指针(中断) */
static volatile uint16_t m_U3ErrRd = 0;         /* 读指针(主循环) */
static volatile uint32_t m_U3ErrLostCnt = 0;    /* 缓冲满时丢弃条数 */

/* 函数名称: Uart3ErrLog_Enqueue
* 功能描述: 将事件落入队列(内部使用, 关中断保护) */
static void Uart3ErrLog_Enqueue(uint8_t type, const uint8_t *pFrame, uint16_t height, uint32_t tick, uint16_t repeat)
{
	U3ErrEvt_t evt;
	evt.Type   = type;
	evt.Bits   = (uint8_t)((GET_BIT0_DOUT() ? 0x01u : 0u) | (GET_BIT1_DOUT() ? 0x02u : 0u));
	evt.Status = (uint8_t)App.Status;
	evt.Frame[0] = pFrame[0];
	evt.Frame[1] = pFrame[1];
	evt.Frame[2] = pFrame[2];
	evt.Frame[3] = pFrame[3];
	evt.Height = height;
	evt.Repeat = repeat;
	evt.Tick   = tick;

	uint32_t primask = __get_PRIMASK();
	__disable_irq();
	uint16_t next = (uint16_t)((m_U3ErrWr + 1U) % U3ERR_EVT_MAX);
	if(next != m_U3ErrRd){                  /* 未满则入队 */
		m_U3ErrEvt[m_U3ErrWr] = evt;
		m_U3ErrWr = next;
	}
	else{                                   /* 满则丢弃当前条并计数 */
		m_U3ErrLostCnt++;
	}
	if(primask == 0U){
		__enable_irq();
	}
}

/* 函数名称: Uart3ErrLog_Push
* 功能描述: 记录一条报错/复位事件(在USART3接收中断中调用)。相同帧500ms内连发只合并计数,
*           由主循环在连发结束(超时)或出现新帧时落一条带"连发N包"的事件, 避免缓冲溢出 */
void Uart3ErrLog_Push(uint8_t type, const uint8_t *pFrame, uint16_t height)
{
	uint32_t tick = SysTick_GetTick();
	/* 连发合并: 与上一帧类型/内容相同且间隔<500ms(type2复位完成除外), 只计数 */
	if((m_U3ErrLastType != 0xFF) && (type == m_U3ErrLastType) && (type != U3ERR_T_DONE) &&
	   (pFrame[0] == m_U3ErrLastFrame[0]) && (pFrame[1] == m_U3ErrLastFrame[1]) &&
	   (pFrame[2] == m_U3ErrLastFrame[2]) && (pFrame[3] == m_U3ErrLastFrame[3]) &&
	   ((tick - m_U3ErrLastTick) < 500U))
	{
		m_U3ErrRepeat++;
		m_U3ErrLastTick = tick;
		return;
	}
	/* 与上一帧不同: 先把滞留的上一帧连同连发计数落队 */
	if(m_U3ErrLastType != 0xFF){
		Uart3ErrLog_Enqueue(m_U3ErrLastType, m_U3ErrLastFrame, m_U3ErrLastHeight, m_U3ErrLastTick, m_U3ErrRepeat);
		m_U3ErrRepeat = 0;
	}
	/* 本帧记为新的滞留帧, 待其连发结束或下一帧到来时落队 */
	m_U3ErrLastType  = type;
	m_U3ErrLastFrame[0] = pFrame[0];
	m_U3ErrLastFrame[1] = pFrame[1];
	m_U3ErrLastFrame[2] = pFrame[2];
	m_U3ErrLastFrame[3] = pFrame[3];
	m_U3ErrLastHeight   = height;
	m_U3ErrLastTick     = tick;
}

/* 函数名称: Uart3ErrLog_BitsName
* 功能描述: BIT0/BIT1输出电平转"升/降/停"描述(ASCII, 避免AC5源码集问题) */
static const char *Uart3ErrLog_BitsName(uint8_t bits)
{
	if((bits & 0x01u) && (bits & 0x02u))  return "STOP";
	if(bits & 0x02u)                      return "UP";
	if(bits & 0x01u)                      return "DOWN";
	return "?";
}

/* 函数名称: Uart3ErrLog_CodeName
* 功能描述: 已知帧内容转义(发现新帧格式后在此扩充) */
static const char *Uart3ErrLog_CodeName(const uint8_t *pFrame)
{
	if(pFrame[0] == 0x01 && pFrame[1] == 0x02 && pFrame[2] == 0x00 && pFrame[3] == 0x10)  return "(OVERHEAT)";
	if(pFrame[2] == 0x01 && pFrame[3] == 0xAA)  return "(RESET)";
	return "";
}

/* 函数名称: Uart3ErrLog_StatusName
* 功能描述: 主状态机状态值转可读名 */
static const char *Uart3ErrLog_StatusName(uint8_t status)
{
	if(status < U3ERR_STATUS_NUM){
		return m_U3ErrStatusName[status];
	}
	return "?";
}

/* 函数名称: Uart3ErrLogLoopHandler
* 功能描述: 主循环中调用, 打印报错→复位→恢复的完整事件时间线(连发帧合并为一条并显示包数) */
void Uart3ErrLogLoopHandler(void)
{
	uint8_t cnt;
	/* 连发已结束(超时500ms无同帧)且有滞留帧 → 先落队一条 */
	if((m_U3ErrLastType != 0xFF) && (m_U3ErrRepeat > 0U) &&
	   ((SysTick_GetTick() - m_U3ErrLastTick) >= 500U))
	{
		Uart3ErrLog_Enqueue(m_U3ErrLastType, m_U3ErrLastFrame, m_U3ErrLastHeight, m_U3ErrLastTick, m_U3ErrRepeat);
		m_U3ErrLastType = 0xFF;
		m_U3ErrRepeat = 0;
	}
	for(cnt = 0; cnt < 8U; cnt++){
		char rep[20];
		if(m_U3ErrRd == m_U3ErrWr){
			break;
		}
		U3ErrEvt_t evt = m_U3ErrEvt[m_U3ErrRd];
		m_U3ErrRd = (uint16_t)((m_U3ErrRd + 1U) % U3ERR_EVT_MAX);
		uint32_t s  = evt.Tick / 1000U;
		uint32_t ms = evt.Tick % 1000U;
		if(evt.Repeat > 0U){
			sprintf(rep, ",burst %u", evt.Repeat + 1U);
		}
		else{
			rep[0] = '\0';
		}
		switch(evt.Type){
			case U3ERR_T_FIRST:     /* 首次信息帧: 置复位标志 */
				DBG_LOG("[T+%3u.%03us][%s|OUT:%s] INFO: code=0x%02X%02X%s -> SetRestFlag%s\r\n",
						s, ms, Uart3ErrLog_StatusName(evt.Status),
						Uart3ErrLog_BitsName(evt.Bits),
						evt.Frame[2], evt.Frame[3],
						Uart3ErrLog_CodeName(evt.Frame), rep);
				break;
			case U3ERR_T_REPLY:     /* 复位中回应信息帧 */
				DBG_LOG("[T+%3u.%03us][%s|OUT:%s] INFO-REPLY: code=0x%02X%02X%s (RestFlag set%s)\r\n",
						s, ms, Uart3ErrLog_StatusName(evt.Status),
						Uart3ErrLog_BitsName(evt.Bits),
						evt.Frame[2], evt.Frame[3],
						Uart3ErrLog_CodeName(evt.Frame), rep);
				break;
			case U3ERR_T_DONE:      /* 复位完成: 收到高度帧 */
				DBG_LOG("[T+%3u.%03us][%s|OUT:%s] Height=%dmm -> RestFlag cleared, normal\r\n",
						s, ms, Uart3ErrLog_StatusName(evt.Status),
						Uart3ErrLog_BitsName(evt.Bits), evt.Height);
				break;
			case U3ERR_T_UNKNOWN:   /* 未知帧: 过热等其它报错可能在此 */
				DBG_LOG("[T+%3u.%03us][%s|OUT:%s] UNKNOWN: %02X %02X %02X %02X %s(ATTENTION)%s\r\n",
						s, ms, Uart3ErrLog_StatusName(evt.Status),
						Uart3ErrLog_BitsName(evt.Bits),
						evt.Frame[0], evt.Frame[1], evt.Frame[2], evt.Frame[3],
						Uart3ErrLog_CodeName(evt.Frame), rep);
				break;
			default:
				break;
		}
	}
	if((m_U3ErrRd == m_U3ErrWr) && (m_U3ErrLostCnt > 0U)){  /* 缓冲曾溢出 */
		uint32_t tick = SysTick_GetTick();
		DBG_LOG("[T+%3u.%03us] NOTE: ErrLog buf overflow, lost %u events\r\n",
				tick / 1000U, tick % 1000U, m_U3ErrLostCnt);
		m_U3ErrLostCnt = 0;
	}
}

#endif