/********************************************************************************
  * @file           : main.c
  * @version        : Mowing Robot Team
  * @brief          : 割草机器人UWB定位标签主程序
 *******************************************************************************/
/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "usb_device.h"
#include "led.h"
#include "usart.h"
#include "gpio.h"
#include "spi.h"
#include "tim.h"
#include "usbd_customhid.h"
#include "dw1000.h"
// Decawave相关头文件
#include "deca_device_api.h"
#include "deca_regs.h"
#include "deca_spi.h"
#include "port.h"

/* Private includes ----------------------------------------------------------*/
uint8_t report[64];                               // USBHid发送缓存

/* Private define ------------------------------------------------------------*/
#define debug_printf                              // 配置调试打印模式

#define HAND_TAG                    0             // 为了区分车上的标签和手拿的标签 0 : 车上的标签
																									//                                1 : 手持的标签
uint8_t Car_moving =                1;            // 当前的工作模式 0 : 接收和发送手持标签信息 
                                                  //                1 : 接收自身和基站距离信息    
uint8_t ReInitDW1000Flag = 0;																									


/* Private variables ---------------------------------------------------------*/
uint8_t uartRxBuffer[20];
uint8_t buff[100], buff_idx = 0, buff_done = 0, buff_len = 0;
uint8_t flag_10ms = 0, flag_100ms = 0, flag_500ms = 0, flag_1s = 0, counter = 0;

/* External variables --------------------------------------------------------*/
extern USBD_HandleTypeDef hUsbDeviceFS;

/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);

/**
  * @brief  The application entry point.
  * @retval int
  */
int main(void)
{
	uint32_t frame_len;
  int ret;
  uint32_t resp_tx_time;
  uint32_t poll_tx_ts[TAG_NUM], resp_rx_ts[TAG_NUM], final_tx_ts[TAG_NUM], poll_rx_ts_32, resp_tx_ts_32, final_rx_ts_32;
  uint16_t dis0,dis1,dis2; // 接收到手持标签与其他标签之间的距离
  double tof, distance;
  double Ra, Rb, Da, Db;
  uint16_t dis[TAG_NUM];
  int64_t tof_dtu;

  /* Frames used in the ranging process.
   * The first 10 bytes of those frame are common and are composed of the following fields:
   * - byte 0/1: frame control (0x8841 to indicate a data frame using 16-bit addressing).
   * - byte 2: sequence number, incremented for each new frame.
   * - byte 3/4: PAN ID (0xDECA).
   * - byte 5/6: destination address
   * - byte 7/8: source address
   * - byte 9: function code (specific values to indicate which message it is in the ranging process).
   * The remaining bytes are specific to each message as follows:
   * Poll message:
   * - no more data
   * Response message:
   * - byte 10: activity code (0x02 to tell the initiator to go on with the ranging exchange).
   * - byte 11/12: activity parameter, not used here for activity code 0x02.
   * Final message:
   * - byte 10 -> 13: poll message transmission timestamp.
   * - byte 14 -> 17: response message reception timestamp.
   * - byte 18 -> 21: final message transmission timestamp.
   * All messages end with a 2-byte checksum automatically set by DW1000. */
  uint8_t tx_blink_msg[] = {0x41, 0x88, 0, 0xCA, 0xDE, 'B', 'L', 0, 0, 0x24, 0, 0};
  uint8_t rx_poll_msg[]  = {0x41, 0x88, 0, 0xCA, 0xDE, 'P', 'L', 0, 0, 0x21, 0, 0};
  uint8_t tx_resp_msg[]  = {0x41, 0x88, 0, 0xCA, 0xDE, 'V', 'E', 'W', 'A', 0x10, 0x02, 0, 0, 0, 0};
  uint8_t rx_final_msg[] = {0x41, 0x88, 0, 0xCA, 0xDE, 'F', 'N', 0, 0, 0x23, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
  uint8_t tx_hand_msg[]  = {0x42, 0x68, 0, 0xCA, 0xDE, 'H', 'D', 0, 0, 0x25, 0, 0, 0, 0, 0, 0, 0, 0};       // 手持标签发送帧
  uint8_t rx_hand_msg[]  = {0x42, 0x68, 0, 0xCA, 0xDE, 'H', 'D', 0, 0, 0x25, 0, 0, 0, 0, 0, 0, 0, 0};       // 手持标签接收帧-在小车上
  /* Frame sequence number, incremented after each transmission. */
  uint8_t frame_seq_nb = 0;
  /* Buffer to store received response message.
   * Its size is adjusted to longest frame that this example code is supposed to handle. */
  uint8_t rx_buffer[RX_BUF_LEN];
  /* Hold copy of status register state here for reference so that it can be examined at a debug breakpoint. */
  uint32_t status_reg = 0;
  /* Time-stamps of frames transmission/reception, expressed in device time units.
   * As they are 40-bit wide, we need to define a 64-bit int type to handle them. */
  uint8_t rx_hand_buffer[RX_BUF_LEN];
  uint8_t rx_final_buffer[RX_BUF_LEN];
  uint64_t blink_tx_ts, poll_rx_ts[TAG_NUM], resp_tx_ts, final_rx_ts[TAG_NUM];
  volatile uint8_t a = 0, b = 0, c = 0;
  int tag_id;
  package_t package;
  package.head[0] = 0xa5;
  package.head[1] = 0xa5;
  package.tagNum = TAG_NUM;
		
  HAL_Init();
  // 系统时钟配置
  SystemClock_Config();
  /* 初始化外设 */
  GPIO_Init();
  LED_GPIO_Config();
	TIM3_Init();
	TIM4_Init();
  SPI1_Init();
	USART1_Init();
  /* USB设备初始化，用于USBHid数据通信 */
  USB_DEVICE_Init();
	/* 初始化DW1000芯片 */
	InitDW1000();
	
	/*  定时器和串口参数配置  */
  // 使能定时器3中断
  HAL_TIM_Base_Start_IT(&htim3);
  /*  DW1000的初始化配置  */
  // 串口打印程序版本信息

  while(1)
	{
		if(ReInitDW1000Flag){
			ReInitDW1000Flag = 0;
		}
    // 如果到了定时周期且处于车载运动模式
    if (flag_100ms == 1 && Car_moving)
    {
      for (int i = 0; i < 12; i++){
        rx_buffer[i] = 0;
        rx_final_buffer[i] = 0;
      }

      tx_blink_msg[ALL_MSG_SN_IDX] = frame_seq_nb;
      frame_seq_nb++;
      // 设置延迟参数
      dwt_setrxaftertxdelay(BLINK_TX_TO_POLL_RX_DLY);
      // 设置Poll RX超时参数
      dwt_setrxtimeout(POLL_RX_TIMEOUT_UUS);

      // 写发送信息
      dwt_writetxdata(sizeof(tx_blink_msg), tx_blink_msg, 0);
      dwt_writetxfctrl(sizeof(tx_blink_msg), 0, 1);
      dwt_starttx(DWT_START_TX_IMMEDIATE | DWT_RESPONSE_EXPECTED);
      /* poll rx*/
      while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG 
              | SYS_STATUS_ALL_RX_TO | SYS_STATUS_ALL_RX_ERR))){
      };

      blink_tx_ts = get_tx_timestamp_u64();
      // 接收处理数据
      if (status_reg & SYS_STATUS_RXFCG)
      {
        dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG);
        frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFL_MASK_1023;
        if (frame_len < RX_BUFFER_LEN)
        {
          dwt_readrxdata(rx_buffer, frame_len, 0);
        }
        rx_buffer[ALL_MSG_SN_IDX] = 0;
        tag_id = rx_buffer[TAG_IDX];
        rx_buffer[TAG_IDX] = 0;
        // 读取Poll时间戳
        if (memcmp(rx_buffer, rx_poll_msg, ALL_MSG_COMMON_LEN) == 0){
          poll_rx_ts[tag_id - 1] = get_rx_timestamp_u64();
        }
      }
      else
      {
        dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_TO | SYS_STATUS_ALL_RX_ERR);
        dwt_forcetrxoff();
        dwt_rxreset();
      }

      // 接收基站的距离信息
      for (int i = 1; i < TAG_NUM; i++)
      {
        dwt_setrxtimeout(POLL_RX_TIMEOUT_UUS);
        dwt_rxenable(DWT_START_RX_IMMEDIATE);
        while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | 
              SYS_STATUS_ALL_RX_TO | SYS_STATUS_ALL_RX_ERR))){
        }
        // 接收处理数据，这部分和上面完全一样
        if (status_reg & SYS_STATUS_RXFCG)
        {
          dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG);
          frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFL_MASK_1023;
          if (frame_len < RX_BUFFER_LEN)
          {
            dwt_readrxdata(rx_buffer, frame_len, 0);
          }
          rx_buffer[ALL_MSG_SN_IDX] = 0;
          // 解析出基站的索引
          tag_id = rx_buffer[TAG_IDX];
          rx_buffer[TAG_IDX] = 0;
          // 读取Poll时间戳
          if (memcmp(rx_buffer, rx_poll_msg, ALL_MSG_COMMON_LEN) == 0){
            poll_rx_ts[tag_id - 1] = get_rx_timestamp_u64();
          }
        }
        else
        {
          dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_TO | SYS_STATUS_ALL_RX_ERR);
          dwt_forcetrxoff();
          dwt_rxreset();
        }
      }
      

      resp_tx_time = (blink_tx_ts + (BLINK_TX_TO_RESP_TX_DLY_UUS * UUS_TO_DWT_TIME)) >> 8;
      dwt_setdelayedtrxtime(resp_tx_time);
      dwt_setrxaftertxdelay(RESP_TX_TO_FINAL_RX_DLY_UUS);
      dwt_setrxtimeout(FINAL_RX_TIMEOUT_UUS);
      dwt_writetxdata(sizeof(tx_resp_msg), tx_resp_msg, 0);
      dwt_writetxfctrl(sizeof(tx_resp_msg), 0, 1);
      ret = dwt_starttx(DWT_START_TX_DELAYED | DWT_RESPONSE_EXPECTED);
      if (ret == DWT_ERROR)
      {
        continue;
      };
      /* final rx	*/
      while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG 
            | SYS_STATUS_ALL_RX_TO | SYS_STATUS_ALL_RX_ERR))){
      };

      resp_tx_ts = get_tx_timestamp_u64();
      if (status_reg & SYS_STATUS_RXFCG)
      {

        dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG);
        frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFL_MASK_1023;
        if (frame_len < RX_BUFFER_LEN)
        {
          dwt_readrxdata(rx_final_buffer, frame_len, 0);
        }
        rx_final_buffer[ALL_MSG_SN_IDX] = 0;
        tag_id = rx_final_buffer[TAG_IDX];
        rx_final_buffer[TAG_IDX] = 0;
        if (memcmp(rx_final_buffer, rx_final_msg, 2) == 0)
        {
          final_rx_ts[tag_id - 1] = get_rx_timestamp_u64();
          final_msg_get_ts(&rx_final_buffer[FINAL_MSG_POLL_TX_TS_IDX], &poll_tx_ts[tag_id - 1]);   // 10
          final_msg_get_ts(&rx_final_buffer[FINAL_MSG_RESP_RX_TS_IDX], &resp_rx_ts[tag_id - 1]);   // 14
          final_msg_get_ts(&rx_final_buffer[FINAL_MSG_FINAL_TX_TS_IDX], &final_tx_ts[tag_id - 1]); // 18
        }
      }
      else
      {
        dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_TO | SYS_STATUS_ALL_RX_ERR);
        dwt_forcetrxoff();
        dwt_rxreset();
      }

      for (int i = 1; i < TAG_NUM; i++)
      {
        // 设置接收超时时间
        dwt_setrxtimeout(FINAL_RX_TIMEOUT_UUS);
        dwt_rxenable(DWT_START_RX_IMMEDIATE);
        while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_TO | SYS_STATUS_ALL_RX_ERR))){
        };
        if (status_reg & SYS_STATUS_RXFCG)
        {
          dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG);
          frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFL_MASK_1023;
          // 获取接收信息
          if (frame_len < RX_BUFFER_LEN){
            dwt_readrxdata(rx_final_buffer, frame_len, 0);
          }
          rx_final_buffer[ALL_MSG_SN_IDX] = 0;
          // 从final中解析出标签索引信息
          tag_id = rx_final_buffer[TAG_IDX];
          rx_final_buffer[TAG_IDX] = 0;
          if (memcmp(rx_final_buffer, rx_final_msg, 2) == 0)
          {
            final_rx_ts[tag_id - 1] = get_rx_timestamp_u64();
            final_msg_get_ts(&rx_final_buffer[FINAL_MSG_POLL_TX_TS_IDX], &poll_tx_ts[tag_id - 1]);   // 10
            final_msg_get_ts(&rx_final_buffer[FINAL_MSG_RESP_RX_TS_IDX], &resp_rx_ts[tag_id - 1]);   // 14
            final_msg_get_ts(&rx_final_buffer[FINAL_MSG_FINAL_TX_TS_IDX], &final_tx_ts[tag_id - 1]); // 18
          }
        }
        else
        {
          dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_TO | SYS_STATUS_ALL_RX_ERR);
          dwt_forcetrxoff();
          dwt_rxreset();
        }
      }
			
      // 计算每一个基站距离标签的位置
      for (int i = 0; i < TAG_NUM; i++)
      {
        poll_rx_ts_32  = (uint32_t)poll_rx_ts[i];   
        resp_tx_ts_32  = (uint32_t)resp_tx_ts;      
        final_rx_ts_32 = (uint32_t)final_rx_ts[i];

        Ra = (double)(resp_rx_ts[i]  - poll_tx_ts[i]);
        Rb = (double)(final_rx_ts_32 - resp_tx_ts_32);
        Da = (double)(final_tx_ts[i] - resp_rx_ts[i]);
        Db = (double)(resp_tx_ts_32  - poll_rx_ts_32);
        /* calculate distance */
        tof_dtu = (int64_t)((Ra * Rb - Da * Db) / (Ra + Rb + Da + Db));
        //					tof_dtu = (int64_t)((Ra + Rb - Da - Db)/4);
        tof = tof_dtu * DWT_TIME_UNITS;
        distance = tof * SPEED_OF_LIGHT;

        dis[i] = distance * 100.0 + 0.5;
        package.dis[i] = dis[i];
        printf("tag%d %d   ", i + 1, dis[i]);
      }
			printf("Car_moving=%d\r\n",Car_moving);
      // 如果是手持标签，则通过消息把距离消息发出去
      if(HAND_TAG){
        /* hand tx*/
        //dwt_setrxaftertxdelay(BLINK_TX_TO_POLL_RX_DLY);
        //dwt_setrxtimeout(POLL_RX_TIMEOUT_UUS);
        hand_msg_set(&tx_hand_msg[Hand_MSG_1_IDX], dis[0]); // 10
        hand_msg_set(&tx_hand_msg[Hand_MSG_2_IDX], dis[1]); // 12
        hand_msg_set(&tx_hand_msg[Hand_MSG_3_IDX], dis[2]); // 14	
        printf("tag0=%d, tag1=%d, tag2=%d \r\n",dis[0], dis[1], dis[2]);
        dwt_writetxdata(sizeof(tx_hand_msg), tx_hand_msg, 0);
        dwt_writetxfctrl(sizeof(tx_hand_msg), 0, 1);
        ret = dwt_starttx(DWT_START_TX_IMMEDIATE);
        if (ret == DWT_SUCCESS) {
        while (!(dwt_read32bitreg(SYS_STATUS_ID) & SYS_STATUS_TXFRS)){};	
          dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_TXFRS);
        } 	
      }
			// USB发送二进制数据
      UsbSendPackageReport(&package, report);
      // 串口发送二进制数据
      //HAL_UART_Transmit(&huart3, &package.head[0], sizeof(package), 0xFFFF);

      flag_100ms = 0;
    }
    // 如果工作模式为接收标签定位模式
#if HAND_TAG == 0
    if(!Car_moving){
			dwt_setrxtimeout(0);
      dwt_rxenable(DWT_START_RX_IMMEDIATE);
      /* hand rx*/
			//printf("wait while \r\n");
      while(!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & 
				  (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_TO | SYS_STATUS_ALL_RX_ERR)) && (!Car_moving)){
			};
				
      if (status_reg & SYS_STATUS_RXFCG)
      {
				dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG);
				frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFL_MASK_1023;
        if (frame_len < RX_BUFFER_LEN)
        {
          dwt_readrxdata(rx_hand_buffer, frame_len, 0);
        }
				rx_hand_buffer[ALL_MSG_SN_IDX] = 0;
        // 校验信息是否正确
        if (memcmp(rx_hand_buffer, rx_hand_msg, 2) == 0)
        {
          // 从rx_hand_buffer中解析出距离信息(3个标签)
          hand_msg_get(&rx_hand_buffer[Hand_MSG_1_IDX], &dis0);  // 10
          hand_msg_get(&rx_hand_buffer[Hand_MSG_2_IDX], &dis1);  // 12
          hand_msg_get(&rx_hand_buffer[Hand_MSG_3_IDX], &dis2);  // 14
          package.dis[0] = dis0;
          package.dis[1] = dis1;
          package.dis[2] = dis2;
					
					UsbSendPackageReport(&package, report);
#ifdef debug_printf
				  printf("dis0=%d, dis1=%d, dis2=%d Car_moving=%d\r\n",dis0, dis1, dis2,Car_moving);
#endif
        }
        // 延时一段时间，过快读取会导致卡死
        HAL_Delay(2);
      }else{
        dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_TO | SYS_STATUS_ALL_RX_ERR);
        dwt_forcetrxoff();
        dwt_rxreset();
      }
    }
#endif
  }
}

/**
  * @brief  System Clock Configuration
  * @retval None
  */
void SystemClock_Config(void)
{
  RCC_OscInitTypeDef RCC_OscInitStruct = {0};
  RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};
  RCC_PeriphCLKInitTypeDef PeriphClkInit = {0};

  /** Initializes the RCC Oscillators according to the specified parameters
  * in the RCC_OscInitTypeDef structure.
  */
  RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
  RCC_OscInitStruct.HSEState = RCC_HSE_ON;
  RCC_OscInitStruct.HSEPredivValue = RCC_HSE_PREDIV_DIV1;
  RCC_OscInitStruct.HSIState = RCC_HSI_ON;
  RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
  RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
  RCC_OscInitStruct.PLL.PLLMUL = RCC_PLL_MUL9;
  if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
  {
    Error_Handler();
  }

  /** Initializes the CPU, AHB and APB buses clocks
  */
  RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
                              |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
  RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
  RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
  RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2;
  RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;

  if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK)
  {
    Error_Handler();
  }
  PeriphClkInit.PeriphClockSelection = RCC_PERIPHCLK_USB;
  PeriphClkInit.UsbClockSelection = RCC_USBCLKSOURCE_PLL_DIV1_5;
  if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInit) != HAL_OK)
  {
    Error_Handler();
  }
}

/**
  * @brief  把UWB的测距包信息拷贝到缓存中并发送
  * @date   2024/04/17
  * @author Zhanli. 719901725@qq.com
  * @retval None
  */
void UsbSendPackageReport(package_t* pack, uint8_t* report){
  memcpy(report, pack, sizeof(package_t));
  // USB发送报文
	USBD_CUSTOM_HID_SendReport(&hUsbDeviceFS, report, 64);
}


 /**
  * @brief  当 TIM 时间基准周期已经过去时调用此函数。
  * @param  htim: 指向 TIM_HandleTypeDef 结构体的指针，包含了TIM模块的
  *         配置信息。
  * @retval 无
  */
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
  if (htim->Instance == htim3.Instance)
  {
    flag_10ms = 1;
    counter++;
    if (counter % 10 == 0){
			flag_100ms = 1;
			Led_Toggle(LED_PC6);
		}
    if (counter % 50 == 0){
			flag_500ms = 1;
		}
    if (counter % 100 == 0){
      flag_1s = 1;
      counter = 0;
    }
  }
}

/**
  * @brief  错误处理函数，用于处理发生错误的事件
  * @retval None
  */
void Error_Handler(void)
{
  /* User can add his own implementation to report the HAL error 
	   return state 
	 */
  __disable_irq();
  while (1)
  {
  }
}

#ifdef  USE_FULL_ASSERT
/**
  * @brief  Reports the name of the source file and the source line number
  *         where the assert_param error has occurred.
  * @param  file: pointer to the source file name
  * @param  line: assert_param error line source number
  * @retval None
  */
void assert_failed(uint8_t *file, uint32_t line)
{
  /* User can add his own implementation to report the file name and line
     number, ex: printf("Wrong parameters value: file %s on line %d\r\n",
     file, line) */
}
#endif
