/*! ----------------------------------------------------------------------------
 * @file    dstwr.c
 * @brief   Double-sided two-way ranging (DS TWR) example code
 *
 * @author Skytrack Studio. https://skytracksoft.com
 *
 * @copyright (C)Skytrack Studio. 2025-2026
 *
 */

#include "deca_probe_interface.h"
#include <config_options.h>
#include <deca_device_api.h>
#include <deca_spi.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"

#include <example_selection.h>
#include <port.h>
#include <shared_defines.h>
#include <shared_functions.h>
#include "dstwr.h"
#include "esp_mac.h"
#include "esp_log.h"
#include "esp_timer.h"

#include "display_ssd1306_lvgl.h"

#if defined(TEST_DS_TWR)

extern void test_run_info(unsigned char* data);

#define TAG "DS_TWR"

/* Example application name and version to display on LCD screen. */
#define APP_NAME "DS TWR v1.0"

/* If this define is enabled then the device will sleep->wakeup->restoreconfig*/
#define SLEEP_EN 0

#define DW_STATUS_CHECK_NOWAIT_MAX_ATTEMPTS 1000

/* Default communication configuration. We use default non-STS DW mode. */
static dwt_config_t config = {
    5,                /* Channel number. */
    DWT_PLEN_256,     /* Preamble length. Used in TX only. */
    DWT_PAC16,         /* 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 8 symbol SFD, 1 to use non-standard 8 symbol, 2 for non-standard 16 symbol SFD and 3 for 4z 8 symbol SDF type */
    DWT_BR_6M8,       /* Data rate. */
    DWT_PHRMODE_STD,  /* PHY header mode. */
    DWT_PHRRATE_STD,  /* PHY header rate. */
    4161,    /* SFD timeout (preamble length + 1 + SFD length - PAC size). Used in RX only. */
    DWT_STS_MODE_OFF, /* STS disabled */
    DWT_STS_LEN_64,   /* STS length see allowed values in Enum dwt_sts_lengths_e */
    DWT_PDOA_M0       /* PDOA mode off */
};

/* Inter-ranging delay period, in milliseconds. */
// #define RNG_DELAY_MS 1000

/* Default antenna delay values for 64 MHz PRF. See NOTE 1 below. */
#define TX_ANT_DLY 16385
#define RX_ANT_DLY 16385

/* Length of the common part of the message (up to and including the function code, see NOTE 2 below). */
// #define ALL_MSG_COMMON_LEN 10
/* Indexes to access some of the fields in the frames defined above. */
// #define ALL_MSG_SN_IDX            2
// #define FINAL_MSG_POLL_TX_TS_IDX  10
// #define FINAL_MSG_RESP_RX_TS_IDX  14
// #define FINAL_MSG_FINAL_TX_TS_IDX 18
/* Frame sequence number, incremented after each transmission. */
// static 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. */
#define RX_BUF_LEN 200
static 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. */
//static uint32_t status_reg = 0;

/* Delay between frames, in UWB microseconds. See NOTE 4 below. */
/* This is the delay from the end of the frame transmission to the enable of the receiver, as programmed for the DW IC's wait for response feature. */
// #define POLL_TX_TO_RESP_RX_DLY_UUS (300 + CPU_PROCESSING_TIME)
/* This is the delay from Frame RX timestamp to TX reply timestamp used for calculating/setting the DW IC's delayed TX function.
 * This value is required to be larger than POLL_TX_TO_RESP_RX_DLY_UUS. Please see NOTE 4 for more details. */
// #define RESP_RX_TO_FINAL_TX_DLY_UUS (300 + CPU_PROCESSING_TIME)
/* Receive response timeout. See NOTE 5 below. */
#define RESP_RX_TIMEOUT_UUS 3000
/* Preamble timeout, in multiple of PAC size. See NOTE 7 below. */
#define PRE_TIMEOUT 50

/* Time-stamps of frames transmission/reception, expressed in device time units. */
// static uint64_t poll_tx_ts;
// static uint64_t resp_rx_ts;
// static uint64_t final_tx_ts;

/* Values for the PG_DELAY and TX_POWER registers reflect the bandwidth and power of the spectrum at the current
 * temperature. These values can be calibrated prior to taking reference measurements. See NOTE 8 below. */
extern dwt_txconfig_t txconfig_options;

static EUI64 node_list[10];
void display_node_info(EUI64 node_id, uint8_t seq8, double distance);

/*! ------------------------------------------------------------------------------------------------------------------
 * @fn dstwr()
 *
 * @brief Application entry point.
 *
 * @param  none
 *
 * @return none
 */
int dstwr(void)
{
    /* Display application name on LCD. */
    test_run_info((unsigned char*)APP_NAME);
    for (int i = 0; i < (int)(sizeof(node_list) / sizeof(node_list[0])); i++) {
        node_list[i] = 0;
    }

    /* Configure SPI rate, DW3000 supports up to 38 MHz */
    port_set_dw_ic_spi_fastrate();

    /* Reset DW IC */
    reset_DWIC(); /* Target specific drive of RSTn line into DW IC low for a period. */

    Sleep(2); // Time needed for DW3000 to start up (transition from INIT_RC to IDLE_RC

    /* Probe for the correct device driver. */
    dwt_probe((struct dwt_probe_s*)&dw3000_probe_interf);

    while (!dwt_checkidlerc()) /* Need to make sure DW IC is in IDLE_RC before proceeding */ {};

    if (dwt_initialise(DWT_DW_INIT) == DWT_ERROR)
    {
        test_run_info((unsigned char*)"INIT FAILED     ");
        while (1) {};
    }

    /* Configure DW IC. See NOTE 14 below. */
    /* if the dwt_configure returns DWT_ERROR either the PLL or RX calibration has failed the host should reset the device */
    if (dwt_configure(&config))
    {
        test_run_info((unsigned char*)"CONFIG FAILED     ");
        while (1) {};
    }

    /* Configure the TX spectrum parameters (power, PG delay and PG count) */
    dwt_configuretxrf(&txconfig_options);

    /* Apply default antenna delay value. See NOTE 1 below. */
    dwt_setrxantennadelay(RX_ANT_DLY);
    dwt_settxantennadelay(TX_ANT_DLY);

    /* Set expected response's delay and timeout. See NOTE 4, 5 and 7 below.
     * As this example only handles one incoming frame with always the same delay and timeout, those values can be set here once for all. */
    //dwt_setrxaftertxdelay(POLL_TX_TO_RESP_RX_DLY_UUS);
    dwt_setrxaftertxdelay(0);
    // dwt_setrxtimeout(RESP_RX_TIMEOUT_UUS);
    // dwt_setpreambledetecttimeout(PRE_TIMEOUT);

    /* Next can enable TX/RX states output on GPIOs 5 and 6 to help debug, and also TX/RX LEDs
     * Note, in real low power applications the LEDs should not be used. */
    dwt_setlnapamode(DWT_LNA_ENABLE | DWT_PA_ENABLE);
    dwt_setleds(DWT_LEDS_ENABLE | DWT_LEDS_INIT_BLINK);

    dwt_setpreambledetecttimeout(0);
    dwt_setrxtimeout(0);


   // 获取 EUI64
    uint8_t current_mac[6];
    EUI64 g_EUI64; // EUI
    esp_efuse_mac_get_default(current_mac);
    g_EUI64 = current_mac[0];
    g_EUI64 += (uint64_t)(current_mac[1]) << 8;
    g_EUI64 += (uint64_t)(current_mac[2]) << 16;
    g_EUI64 += (uint64_t)(0xFF) << 24;
    g_EUI64 += (uint64_t)(0xFE) << 32;
    g_EUI64 += (uint64_t)(current_mac[3]) << 40;
    g_EUI64 += (uint64_t)(current_mac[4]) << 48;
    g_EUI64 += (uint64_t)(current_mac[5]) << 56;


    int64_t last_poll_time_us = 0;
    uint8_t poll_seq = 0;
    while (1)
    {

        if (esp_timer_get_time() - last_poll_time_us > 1000000) {
            // 每秒发送一次 poll 消息
            last_poll_time_us = esp_timer_get_time();

            dwt_forcetrxoff();

            TWR_BROADCAST_POLL_MESSAGE poll_msg;
            poll_msg.frameCtrl[0] = 0x01 | 0x40;	// frame type 0x1 == data, PanID 被压缩, 即目标和源PanID相同
            poll_msg.frameCtrl[1] = 0x08 | 0xC0;	// 16 bits dest address, src extended address (64 bits)
            poll_msg.seq8 = poll_seq++;
            poll_msg.panId = 0xDECA;
            poll_msg.destId16 = 0xFFFF;
            poll_msg.sourceId = g_EUI64;
            poll_msg.messageType = RTLS_MSG_TYPE_TWR_POLL;

            uint32_t syncFrame_tx_time32 = dwt_readsystimestamphi32();
            syncFrame_tx_time32 = syncFrame_tx_time32 + (((300 + 400) * UUS_TO_DWT_TIME) >> 8);
            syncFrame_tx_time32 &= 0xFFFFFFFE;		// 确认最低位为 0, 时间戳的低9位被 dw1000 忽略, 所以我们要保证低9位是0
            dwt_setdelayedtrxtime(syncFrame_tx_time32);
            int64_t trueTxTS = ((uint64_t)(syncFrame_tx_time32) << 8) + TX_ANT_DLY;		// 要加上发送天线延迟
            memcpy(poll_msg.pollTxTS, &trueTxTS, 5);
            dwt_writetxdata(sizeof(TWR_BROADCAST_POLL_MESSAGE), (uint8_t*)&poll_msg, 0);		// 写帧数据到 dw1000
            dwt_writetxfctrl(sizeof(TWR_BROADCAST_POLL_MESSAGE), 0, 1);									// 配置发送帧控制寄存器
            int err = dwt_starttx(DWT_START_TX_DELAYED | DWT_RESPONSE_EXPECTED);
            if (err == DWT_SUCCESS) {
                int i = 0;
                uint32_t status = 0;
                while ((!((status = dwt_readsysstatuslo()) & DWT_INT_TXFRS_BIT_MASK)) && (i < DW_STATUS_CHECK_NOWAIT_MAX_ATTEMPTS)) {	// 如果 i>1000, 说明发送失败, 这次就算了
                    i++;
                };
                dwt_writesysstatuslo(DWT_INT_RXFCG_BIT_MASK | DWT_INT_TXFRS_BIT_MASK);
                // ESP_LOGI(TAG, "TWR POLL TX OK: sourceId:%016llX seq8:%u i=%u status=0x%08X", EndianSwap64(poll_msg.sourceId), poll_msg.seq8, i, status);
            }
            else {
                ESP_LOGE(TAG, "TWR POLL TX err, status=0x%08X seq8=%u", dwt_readsysstatuslo(), poll_msg.seq8);
            }
            dwt_rxenable(DWT_START_RX_IMMEDIATE);
        }

        /* Continuous RX listen (same as ds_twr_responder). */
        //dwt_rxenable(DWT_START_RX_IMMEDIATE);
        // waitforsysstatus(&status_reg, NULL, (DWT_INT_RXFCG_BIT_MASK | SYS_STATUS_ALL_RX_TO | SYS_STATUS_ALL_RX_ERR), 0);
        uint32_t status = dwt_readsysstatuslo();

        if (status & DWT_INT_RXFCG_BIT_MASK) {
            uint16_t frame_len;

            /* Clear good RX frame event and TX frame sent in the DW IC status register. */
            dwt_writesysstatuslo(DWT_INT_RXFCG_BIT_MASK | DWT_INT_TXFRS_BIT_MASK);

            /* A frame has been received, read it into the local buffer. */
            frame_len = dwt_getframelength(0);
            if (frame_len <= RX_BUF_LEN) {
                dwt_readrxdata(rx_buffer, frame_len, 0);
            }

            if (rx_buffer[0] == 0x41 && rx_buffer[1] == 0xC8) {
                // 广播, 收到的是 POLL 消息
                TWR_BROADCAST_POLL_MESSAGE* pmsg = (TWR_BROADCAST_POLL_MESSAGE*)rx_buffer;
                if (pmsg->messageType == RTLS_MSG_TYPE_TWR_POLL) {
                    // ESP_LOGI(TAG, "TWR POLL RX: sourceId:%016llX seq8:%u", EndianSwap64(pmsg->sourceId), pmsg->seq8);
                    TWR_UNBROADCAST_RESPONSE_MESSAGE resp_msg;
                    resp_msg.frameCtrl[0] = 0x01 | 0x40;	// frame type 0x1 == data, PanID 被压缩, 即目标和源PanID相同
                    resp_msg.frameCtrl[1] = 0x0C | 0xC0;	// 64 bits dest address, src extended address (64 bits)
                    resp_msg.seq8 = pmsg->seq8;
                    resp_msg.panId = pmsg->panId;
                    resp_msg.destId = pmsg->sourceId;
                    resp_msg.sourceId = g_EUI64;
                    resp_msg.messageType = RTLS_MSG_TYPE_TWR_RESPONSE;
                    memcpy(resp_msg.pollTxTS, pmsg->pollTxTS, 5);
                    dwt_readrxtimestamp((uint8_t*)resp_msg.pollRxTS, 0);
                    uint32_t syncFrame_tx_time32 = dwt_readsystimestamphi32();
                    syncFrame_tx_time32 = syncFrame_tx_time32 + (((300 + 400) * UUS_TO_DWT_TIME) >> 8);
                    syncFrame_tx_time32 &= 0xFFFFFFFE;		// 确认最低位为 0, 时间戳的低9位被 dw1000 忽略, 所以我们要保证低9位是0
                    dwt_setdelayedtrxtime(syncFrame_tx_time32);
                    int64_t trueTxTS = ((uint64_t)(syncFrame_tx_time32) << 8) + TX_ANT_DLY;		// 要加上发送天线延迟
                    memcpy(resp_msg.responseTxTS, &trueTxTS, 5);
                    dwt_writetxdata(sizeof(TWR_UNBROADCAST_RESPONSE_MESSAGE), (uint8_t*)&resp_msg, 0);		// 写帧数据到 dw1000
                    dwt_writetxfctrl(sizeof(TWR_UNBROADCAST_RESPONSE_MESSAGE), 0, 1);									// 配置发送帧控制寄存器
                    int err = dwt_starttx(DWT_START_TX_DELAYED | DWT_RESPONSE_EXPECTED);	// send clock sync message
                    if (err == DWT_SUCCESS) {
                        int i = 0;
                        uint32_t status = 0;
                        while ((!((status = dwt_readsysstatuslo()) & DWT_INT_TXFRS_BIT_MASK)) && (i < DW_STATUS_CHECK_NOWAIT_MAX_ATTEMPTS)) {	// 如果 i>1000, 说明发送失败, 这次就算了
                            i++;
                        };
                        // ESP_LOGI(TAG, "TWR RESPONSE TX: sourceId:%016llX seq8:%u", EndianSwap64(resp_msg.sourceId), resp_msg.seq8);
                        dwt_writesysstatuslo(DWT_INT_RXFCG_BIT_MASK | DWT_INT_TXFRS_BIT_MASK);
                    }
                    else {
                        ESP_LOGE(TAG, "TWR RESPONSE TX err, status_reg=0x%08X seq8=%u", dwt_readsysstatuslo(), resp_msg.seq8);
                    }
                }
            }
            else if (rx_buffer[0] == 0x41 && rx_buffer[1] == 0xCC) {
                // 64 位目标地址
                TWR_UNBROADCAST_RESPONSE_MESSAGE* pp = (TWR_UNBROADCAST_RESPONSE_MESSAGE*)rx_buffer;
                if (pp->messageType == RTLS_MSG_TYPE_TWR_RESPONSE) {
                    TWR_UNBROADCAST_RESPONSE_MESSAGE* pmsg = (TWR_UNBROADCAST_RESPONSE_MESSAGE*)rx_buffer;
                    // ESP_LOGI(TAG, "TWR RESPONSE RX: sourceId:%016llX seq8:%u", EndianSwap64(pp->sourceId), pp->seq8);

                    TWR_UNBROADCAST_FINAL_MESSAGE final_msg;
                    final_msg.frameCtrl[0] = 0x01 | 0x40;	// frame type 0x1 == data, PanID 被压缩, 即目标和源PanID相同
                    final_msg.frameCtrl[1] = 0x0C | 0xC0;	// 64 bits dest address, src extended address (64 bits)
                    final_msg.seq8 = pmsg->seq8;
                    final_msg.panId = pmsg->panId;
                    final_msg.destId = pmsg->sourceId;
                    final_msg.sourceId = g_EUI64;
                    final_msg.messageType = RTLS_MSG_TYPE_TWR_FINAL;
                    memcpy(final_msg.pollTxTS, pmsg->pollTxTS, 5);
                    memcpy(final_msg.pollRxTS, pmsg->pollRxTS, 5);
                    memcpy(final_msg.responseTxTS, pmsg->responseTxTS, 5);
                    dwt_readrxtimestamp((uint8_t*)final_msg.responseRxTS, 0);
                    uint32_t syncFrame_tx_time32 = dwt_readsystimestamphi32();
                    syncFrame_tx_time32 = syncFrame_tx_time32 + (((300 + 400) * UUS_TO_DWT_TIME) >> 8);
                    syncFrame_tx_time32 &= 0xFFFFFFFE;		// 确认最低位为 0, 时间戳的低9位被 dw1000 忽略, 所以我们要保证低9位是0
                    dwt_setdelayedtrxtime(syncFrame_tx_time32);
                    int64_t trueTxTS = ((uint64_t)(syncFrame_tx_time32) << 8) + TX_ANT_DLY;		// 要加上发送天线延迟
                    memcpy(final_msg.finalTxTS, &trueTxTS, 5);

                    dwt_writetxdata(sizeof(TWR_UNBROADCAST_FINAL_MESSAGE), (uint8_t*)&final_msg, 0);		// 写帧数据到 dw1000
                    dwt_writetxfctrl(sizeof(TWR_UNBROADCAST_FINAL_MESSAGE), 0, 1);									// 配置发送帧控制寄存器
                    int err = dwt_starttx(DWT_START_TX_DELAYED | DWT_RESPONSE_EXPECTED);	// send clock sync message
                    if (err == DWT_SUCCESS) {
                        int i = 0;
                        uint32_t status = 0;
                        while ((!((status = dwt_readsysstatuslo()) & DWT_INT_TXFRS_BIT_MASK)) && (i < DW_STATUS_CHECK_NOWAIT_MAX_ATTEMPTS)) {	// 如果 i>1000, 说明发送失败, 这次就算了
                            i++;
                        };
                        dwt_writesysstatuslo(DWT_INT_RXFCG_BIT_MASK | DWT_INT_TXFRS_BIT_MASK);
                        // ESP_LOGI(TAG, "TWR FINAL TX: sourceId:%016llX seq8:%u", EndianSwap64(final_msg.sourceId), final_msg.seq8);
                    }
                    else {
                        ESP_LOGE(TAG, "TWR FINAL TX err, status_reg=0x%08X seq8=%u", dwt_readsysstatuslo(), final_msg.seq8);
                    }
                }
                else if (pp->messageType == RTLS_MSG_TYPE_TWR_FINAL) {
                    TWR_UNBROADCAST_FINAL_MESSAGE* pmsg = (TWR_UNBROADCAST_FINAL_MESSAGE*)rx_buffer;
                    // ESP_LOGI(TAG, "TWR FINAL RX: sourceId:%016llX seq8:%u", EndianSwap64(pp->sourceId), pp->seq8);
                    int64_t poll_tx_ts = 0;
                    int64_t poll_rx_ts = 0;
                    int64_t response_tx_ts = 0;
                    int64_t response_rx_ts = 0;
                    int64_t final_tx_ts = 0;
                    int64_t final_rx_ts = 0;
                    memcpy(&poll_tx_ts, pmsg->pollTxTS, 5);
                    memcpy(&poll_rx_ts, pmsg->pollRxTS, 5);
                    memcpy(&response_tx_ts, pmsg->responseTxTS, 5);
                    memcpy(&response_rx_ts, pmsg->responseRxTS, 5);
                    memcpy(&final_tx_ts, pmsg->finalTxTS, 5);
                    dwt_readrxtimestamp((uint8_t*)&final_rx_ts, 0);

                    int64_t round1 = response_rx_ts - poll_tx_ts;
                    int64_t reply1 = response_tx_ts - poll_rx_ts;
                    int64_t round2 = final_rx_ts - response_tx_ts;
                    int64_t reply2 = final_tx_ts - response_rx_ts;
                    if (round1 < 0) round1 += 0x10000000000;
                    if (reply1 < 0) reply1 += 0x10000000000;
                    if (round2 < 0) round2 += 0x10000000000;
                    if (reply2 < 0) reply2 += 0x10000000000;
                    double tof_dtu = ((double)round1 * (double)round2 - (double)reply1 * (double)reply2) / ((double)round1 + (double)round2 + (double)reply1 + (double)reply2);
                    double distance = tof_dtu * DWT_TIME_UNITS * SPEED_OF_LIGHT;
                    // ESP_LOGI(TAG, "--------------------------------");
                    // ESP_LOGI(TAG, "TWR FINAL RX: sourceId:%016llX seq8:%u tof_dtu:%f distance: %f m", EndianSwap64(pp->sourceId), pp->seq8, tof_dtu, distance);
                    // ESP_LOGI(TAG, "round1: %lld reply1: %lld round2: %lld reply2: %lld", (long long)round1, (long long)reply1, (long long)round2, (long long)reply2);
                    // ESP_LOGI(TAG, "TWR FINAL RX: sourceId:%016llX seq8:%u poll_tx_ts:%lld poll_rx_ts:%lld response_tx_ts:%lld response_rx_ts:%lld final_tx_ts:%lld final_rx_ts:%lld", EndianSwap64(pp->sourceId), pp->seq8, (long long)poll_tx_ts, (long long)poll_rx_ts, (long long)response_tx_ts, (long long)response_rx_ts, (long long)final_tx_ts, (long long)final_rx_ts);
                    // ESP_LOGI(TAG, "--------------------------------");
                    display_node_info(pp->sourceId, pp->seq8, distance);
                }
                else {
                    ESP_LOGE(TAG, "RX messageType error: sourceId:%016llX seq8:%u messageType=%u", EndianSwap64(pp->sourceId), pp->seq8, pp->messageType);
                }

            }
            else {
                ESP_LOGE(TAG, "RX messageType error: rx_buffer[0]=0x%02X rx_buffer[1]=0x%02X", rx_buffer[0], rx_buffer[1]);
            }

        }
        else if (status & (SYS_STATUS_ALL_RX_TO | SYS_STATUS_ALL_RX_ERR)) {
            dwt_forcetrxoff();
            dwt_rxenable(DWT_START_RX_IMMEDIATE);
            // ESP_LOGE(TAG, "TWR RX timeout or error, status=0x%08X", status);
        }

        vTaskDelay(1);
    }
    return 0;
}



void display_node_info(EUI64 node_id, uint8_t seq8, double distance)
{
    int i = 0;
    const int node_cap = (int)(sizeof(node_list) / sizeof(node_list[0]));
    while (i < node_cap) {
        if ((node_list[i] == 0) || (node_list[i] == node_id)) {
            break;
        }
        i++;
    }
    if (i < node_cap) {
        node_list[i] = node_id;
        char text[100];
        sprintf(text, "%04X: %3.2f 米 seq:%u", (uint16_t)(EndianSwap64(node_id) | 0xFFFF0000), distance, seq8);
        /* row 0 = MAC；测距从 row 1 起，共 5 行 (y: 0/13/26/39/52) */
        oled_text_set_at(0, oled_compute_cjk_row_y(i + 1), text);
    }
    // ESP_LOGI(TAG, "--------------------------------");
    ESP_LOGI(TAG, "NodeID: %016llX, distance: %3.2f m, seq8: %u", EndianSwap64(node_id), distance, seq8);
    // ESP_LOGI(TAG, "--------------------------------");
}





#endif
