/*******************************************************************************
  * @filename          : dw1000.c
  * @brief             : dw1000芯片测距
  * @author            : zhanli
  * @date              : 2024/08/22
  *****************************************************************************/
/* Includes ------------------------------------------------------------------*/
#include "stm32f1xx_hal.h"
#include "deca_device_api.h"
#include "dw1000.h"
#include "port.h"
#include "stdio.h"

dwt_config_t config = {
		2,                /* Channel number. */
		DWT_PRF_64M,      /* Pulse repetition frequency. */
		DWT_PLEN_128,     /* Preamble length. Used in TX only. */
		DWT_PAC8,         /* Preamble acquisition chunk size. Used in RX only. */
		9,                /* TX preamble code. Used in TX only. */
		9,                /* RX preamble code. Used in RX only. */
		1,                /* 0 to use standard SFD, 1 to use non-standard SFD. */
		DWT_BR_6M8,       /* Data rate. */
		DWT_PHRMODE_STD,  /* PHY header mode. */
		(128 + 1 + 8 - 8) /* SFD timeout (preamble length + 1 + SFD length - PAC size). Used in RX only. */
};


void InitDW1000(){
  /* 重置和初始化DW1000, 初始化时，DW1000时钟必须暂时设置为晶振速度,初始化后，可以增加
    * SPI速率以获得最佳性能 */
  reset_DW1000();
  // 降低DW1000的SPI速率
  port_set_dw1000_slowrate();
  if (dwt_initialise(DWT_LOADUCODE) == DWT_ERROR){
    printf("DW1000 init failed.\r\n");
  }else{
    printf("DW1000 init success.\r\n");
  }
  // 恢复DW1000的SPI速率
  port_set_dw1000_fastrate();
  /* 配置DW1000 */
  dwt_configure(&config);
  // 配置DW1000的天线延迟参数
  dwt_setrxantennadelay(RX_ANT_DLY);
  dwt_settxantennadelay(TX_ANT_DLY);
  printf("DW1000 is configured.\r\n");
}

/**
 * @fn     get_tx_timestamp_u64()
 * @brief  获取64bit TX时间戳变量，数假设发送还是接收时的时间戳的长度为40位
 * @param  none
 * @return  64-bit 读取时间戳.
 */
uint64_t get_tx_timestamp_u64(void)
{
  uint8_t ts_tab[5];
  uint64_t ts = 0;
  int i;
  dwt_readtxtimestamp(ts_tab);
  for (i = 4; i >= 0; i--)
  {
    ts <<= 8;
    ts |= ts_tab[i];
  }
  return ts;
}

/**
 * @fn     get_rx_timestamp_u64()
 * @brief  获取64bit RX时间戳变量，数假设发送还是接收时的时间戳的长度为40位
 * @param  none
 * @return  64-bit 读取时间戳.
 */
uint64_t get_rx_timestamp_u64(void)
{
  uint8_t ts_tab[5];
  uint64_t ts = 0;
  int i;
  dwt_readrxtimestamp(ts_tab);
  for (i = 4; i >= 0; i--){
    ts <<= 8;
    ts |= ts_tab[i];
  }
  return ts;
}

/**
 * @fn     final_msg_set_ts()
 * @brief  Fill a given timestamp field in the final message with the given value. 
 *         In the timestamp fields of the final message, the least significant 
 *         byte is at the lower address.
 * @param  ts_field  pointer on the first byte of the timestamp field to fill
 *         ts  timestamp value
 * @return none
 */
void final_msg_get_ts(const uint8_t *ts_field, uint32_t *ts)
{
  *ts = 0;
  for (int i = 0; i < FINAL_MSG_TS_LEN; i++){
    *ts += ts_field[i] << (i * 8);
  }
}

void hand_msg_get(const uint8_t *dis_field, uint16_t *dis)
{
  *dis = 0;
  for (int i = 0; i < Hand_MSG_LEN; i++){
    *dis += dis_field[i] << (i * 8);
  }
}
void final_msg_set_ts(uint8_t *ts_field, uint64_t ts)
{
  for (int i = 0; i < FINAL_MSG_TS_LEN; i++)
  {
    ts_field[i] = (uint8)ts;
    ts >>= 8;
  }
}

void hand_msg_set(uint8_t *dis_field, uint16_t dis)
{
  for (int i = 0; i < Hand_MSG_LEN; i++){
    dis_field[i] = (uint8)dis;
    dis >>= 8;
  }
}

void MsgSetData(uint8_t *msg, uint16_t dis, int len){
  for (int i = 0; i < Hand_MSG_LEN; i++){
    msg[i] = (uint8)dis;
    dis >>= 8;
  }
}
void MsgGetData(const uint8_t *msg, uint16_t *data, int len){
  *data = 0;
  for (int i = 0; i < len; i++){
    *data += msg[i] << (i * 8);
  }
}
