/*
 * Copyright (c) 2019-2023 Beijing Hanwei Innovation Technology Ltd. Co. and
 * its subsidiaries and affiliates (collectly called MKSEMI).
 *
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form, except as embedded into an MKSEMI
 *    integrated circuit in a product or a software update for such product,
 *    must reproduce the above copyright notice, this list of conditions and
 *    the following disclaimer in the documentation and/or other materials
 *    provided with the distribution.
 *
 * 3. Neither the name of MKSEMI nor the names of its contributors may be used
 *    to endorse or promote products derived from this software without
 *    specific prior written permission.
 *
 * 4. This software, with or without modification, must only be used with a
 *    MKSEMI integrated circuit.
 *
 * 5. Any software provided in binary form under this license must not be
 *    reverse engineered, decompiled, modified and/or disassembled.
 *
 * THIS SOFTWARE IS PROVIDED BY MKSEMI "AS IS" AND ANY EXPRESS OR IMPLIED
 * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 * MERCHANTABILITY, NONINFRINGEMENT, AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL MKSEMI OR CONTRIBUTORS BE LIABLE FOR ANY
 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
 * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#include "mk_trace.h"
#include "mk_clock.h"
#include "mk_power.h"
#include "mk_uwb.h"
#include "mk_radar.h"
#include "mk_sleep_timer.h"
#include "mk_misc.h"
#include "uwb_api.h"
#include "uwb_radar_task.h"
#include "uwb_radar.h"
#if UWB_RADAR_DETECT_PROCESS_EN
#include "mk8000_present_interface.h"
#endif
// #include "board.h"

#define DEBUG_MEM_ADDR 0x02010000
#define RADAR_PRINT_EN 0

/*************************************************************************************************/
/*!
 *  \brief  WSF event handler for uwb radar task.
 *
 *  \param  event   WSF event mask.
 *  \param  msg    WSF message.
 *
 *  \return None.
 */
/*************************************************************************************************/

#if UWB_RADAR_DETECT_PROCESS_EN
static float ref_out[256] = {0.0};
static struct Cf_sto cf_data[64] = {0};

static struct Present_sto present = {
    .cf_data = cf_data,
    .Y = ref_out,
    .index = 0,
    .flag = 0,
};

static struct Pres_para_sto pre_para = {
    .cp_para =
        {
            // choose cfar threshold in each section
            .sec_alp = {35, 33, 20, 10, 10, 15, 15},
            // Choose cfar solution in each section,0:so cfar 1:ca cfar 2:go cfar
            .sec_sol = {1, 1, 0, 0, 0, 1, 1},
            // range index:[start,end,multi taget num in each section]
            .range = {2, 36, 0},
            // the safe section,the reference section
            .ref_x = {5, 4, 2},
            // Apart the range into sen_len sections
            .sec_len = 5,
            // 0:choose max power target
            // 1:choose max power target in each section
            // 2:choose max power target in first section
            .tar_num = 3,
            .nbor = 1,
            .ma_val = 1000,
        },
    // 1:adaptive,High Freq 20/30/80Hz  0:not adaptive 1Hz
    .train = 1,
    // 0: no calibration,
    // 1: take last pulse as reference, >1: take mean(rep pulses) as reference, from frame=0 to frame = rep
    .rep = 1,
    // Number of learning frame
    .frame = 0,
    .norm = 1000,
    .apole = 0.3f,
    .acc = 3,
};
#endif

struct sample16_pack
{
    uint16_t header;
    uint16_t length;
    uint32_t seq;
    float acd_out[PULSE_PERIOD_POINTS(0) * 2];
    uint16_t check_num;
    uint16_t end;
};

struct sample32_pack
{
    uint16_t header;
    uint16_t length;
    uint32_t seq;
    float acd_out[PULSE_PERIOD_POINTS(1) * 2];
    uint16_t check_num;
    uint16_t end;
};

struct sample64_pack
{
    uint16_t header;
    uint16_t length;
    uint32_t seq;
    float acd_out[PULSE_PERIOD_POINTS(2) * 2];
    uint16_t check_num;
    uint16_t end;
};

struct sample128_pack
{
    uint16_t header;
    uint16_t length;
    uint32_t seq;
    float acd_out[PULSE_PERIOD_POINTS(3) * 2];
    uint16_t check_num;
    uint16_t end;
};

struct sample256_pack
{
    uint16_t header;
    uint16_t length;
    uint32_t seq;
    float acd_out[PULSE_PERIOD_POINTS(4) * 2];
    uint16_t check_num;
    uint16_t end;
};

struct sample_1tnr_pack
{
    uint16_t header;
    uint16_t length;
    uint32_t seq;
    float acd_out[OUT_FRAME_LEN * UWB_RADAR_RX_PORT_NUM];
    uint16_t check_num;
    uint16_t end;
};

union xfer_sample_pack
{
#define SAMPLE_PACK_FIX_LENGTH 12
    struct sample16_pack sample16;
    struct sample32_pack sample32;
    struct sample64_pack sample64;
    struct sample128_pack sample128;
    struct sample256_pack sample256;
    struct sample_1tnr_pack sample_1tnr;
};

static union xfer_sample_pack xfer_pack = {
    .sample_1tnr.header = 0x5555,
};

#if UWB_RADAR_DETECT_PROCESS_EN == 0 && UWB_RADAR_UCI_EN == 0
static void xfer_done(void *dev, uint32_t err_code)
{
    // board_led_off(BOARD_LED_1);
}
#endif

void uwb_radar_handler(wsfEventMask_t event, const void *param)
{
    // const wsfMsgHdr_t *msg = (const wsfMsgHdr_t *)param;
    // board_led_on(BOARD_LED_2);

    if (uwb_is_radar_mode())
    {
        uint16_t data_len;

        switch (uwb_app_config.session_type)
        {
            case SESSION_TYPE_VENDOR_RANGING_AND_RADAR:
            {
                switch (event)
                {
                    case UWB_RADAR_START_SAMPLING_EVT:
                    {
                        /* Creat radar event */
                        uwb_radar_creat_event();
                        phy_sts_pkt_cfg_set(3);
                        radar_enable();
                        power_on_radio(1, 1);
                        radar_start(radar_env.rx_ant_idx);
                        power_mode_request(POWER_UNIT_APP, POWER_MODE_SLEEP);
                    }
                    break;

                    case UWB_RADAR_SAMPLING_COMPLETE_EVT:
                    {
                        /* Data preprocessing, XIP 19.8ms */
                        power_off_radio();
                        radar_disable();
                        power_mode_request(POWER_UNIT_APP, POWER_MODE_POWER_DOWN);

                        radar_data_process((uint32_t *)DEBUG_MEM_ADDR, xfer_pack.sample_1tnr.acd_out, &data_len);
                        uwb_radar_destroy_event();

#if UWB_RADAR_UCI_EN
                        uwbapi_report_radar_raw_data(data_len, (uint8_t *)xfer_pack.sample_1tnr.acd_out);
#else
#if UWB_RADAR_DETECT_PROCESS_EN
                        // board_led_on(BOARD_LED_1);
                        mcu_target_detect_process(xfer_pack.sample_1tnr.acd_out, PULSE_PERIOD_POINTS(UWB_RADAR_PULSE_PERIOD), &pre_para, &present);
                        /* Object detection, the result shows */
                        if (present.flag)
                        {
                            uint16_t distance = (uint16_t)(((float)present.index * 0.15f) * 100);
                            LOG_INFO(TRACE_MODULE_APP | TRACE_NO_OPTION, "Presence: YES, Radar Distance: %ucm\r\n", distance);
                        }
                        else
                        {
                            LOG_INFO(TRACE_MODULE_APP | TRACE_NO_OPTION, "Presence: NO, Radar Distance: 0cm\r\n");
                        }
                        // board_led_off(BOARD_LED_1);

#else
                        // board_led_on(BOARD_LED_1);
                        LOG_INFO(TRACE_MODULE_APP, "Radar output data len: %u\r\n", data_len);
                        xfer_pack.sample_1tnr.length = data_len + SAMPLE_PACK_FIX_LENGTH;
                        xfer_pack.sample_1tnr.seq = radar_env.rx_ant_idx;
                        uint16_t check_num = uwb_radar_data_checksum((const uint8_t *)xfer_pack.sample_1tnr.acd_out, data_len);

#if UWB_RADAR_1TNR_MODE == 1
                        xfer_pack.sample_1tnr.check_num = check_num;
                        xfer_pack.sample_1tnr.end = 0xAAAA;
#else
                        if (uwb_app_config.session_param.uwb_radar_pulse_period == UWB_RADAR_PULSE_PERIOD_16NS)
                        {
                            xfer_pack.sample16.check_num = check_num;
                            xfer_pack.sample16.end = 0xAAAA;
                        }
                        else if (uwb_app_config.session_param.uwb_radar_pulse_period == UWB_RADAR_PULSE_PERIOD_32NS)
                        {
                            xfer_pack.sample32.check_num = check_num;
                            xfer_pack.sample32.end = 0xAAAA;
                        }
                        else if (uwb_app_config.session_param.uwb_radar_pulse_period == UWB_RADAR_PULSE_PERIOD_64NS)
                        {
                            xfer_pack.sample64.check_num = check_num;
                            xfer_pack.sample64.end = 0xAAAA;
                        }
                        else if (uwb_app_config.session_param.uwb_radar_pulse_period == UWB_RADAR_PULSE_PERIOD_128NS)
                        {
                            xfer_pack.sample128.check_num = check_num;
                            xfer_pack.sample128.end = 0xAAAA;
                        }
                        else if (uwb_app_config.session_param.uwb_radar_pulse_period == UWB_RADAR_PULSE_PERIOD_256NS)
                        {
                            xfer_pack.sample256.check_num = check_num;
                            xfer_pack.sample256.end = 0xAAAA;
                        }
#endif
                        uart_send(DUMP_DATA_PORT, (uint8_t *)(&xfer_pack), xfer_pack.sample_1tnr.length, xfer_done);
#endif
#endif
                    }
                    break;
                }
            }
            break;

            case SESSION_TYPE_VENDOR_ALONE_RADAR:
            {
                const wsfMsgHdr_t *msg = (const wsfMsgHdr_t *)param;
                if (msg != NULL)
                {
                    if (msg->event == UWB_RADAR_ALONE_WAKEUP_MSG)
                    {
                        uwb_radar_alone_continue_trigger();
                    }
                }
                else
                {
                    switch (event)
                    {
                        case UWB_RADAR_START_SAMPLING_EVT:
                        {
                            phy_sts_pkt_cfg_set(3);
                            radar_enable();
                            power_on_radio(1, 1);
                            radar_start(radar_env.rx_ant_idx);
                            power_mode_request(POWER_UNIT_APP, POWER_MODE_SLEEP);
                        }
                        break;

                        case UWB_RADAR_SAMPLING_COMPLETE_EVT:
                        {
                            /* Data preprocessing, XIP 19.8ms */
                            power_off_radio();
                            radar_disable();
                            power_mode_request(POWER_UNIT_APP, POWER_MODE_POWER_DOWN);

                            radar_data_process((uint32_t *)DEBUG_MEM_ADDR, xfer_pack.sample_1tnr.acd_out, &data_len);

#if UWB_RADAR_UCI_EN
                            uwbapi_report_radar_raw_data(data_len, (uint8_t *)xfer_pack.sample_1tnr.acd_out);
#else
#if RADAR_PRINT_EN
                            LOG_INFO(TRACE_MODULE_APP | TRACE_NO_OPTION, "--------------%u--------------\r\n", data_len);
                            for (uint8_t i = 0; i < PULSE_PERIOD_POINTS(UWB_RADAR_PULSE_PERIOD); i++)
                            {
                                LOG_INFO(TRACE_MODULE_APP | TRACE_NO_OPTION, "%.2f %.2f\n", xfer_pack.sample_1tnr.acd_out[i * 2],
                                         xfer_pack.sample_1tnr.acd_out[i * 2 + 1]);
                            }
#endif
#if UWB_RADAR_DETECT_PROCESS_EN
                            // board_led_on(BOARD_LED_1);
                            mcu_target_detect_process(xfer_pack.sample_1tnr.acd_out, PULSE_PERIOD_POINTS(UWB_RADAR_PULSE_PERIOD), &pre_para, &present);
                            /* Object detection, the result shows */
                            if (present.flag)
                            {
                                uint16_t distance = (uint16_t)(((float)present.index * 0.15f) * 100);
                                LOG_INFO(TRACE_MODULE_APP | TRACE_NO_OPTION, "\r\nPresence: YES, Radar Distance: %ucm\r\n", distance);
                            }
                            else
                            {
                                LOG_INFO(TRACE_MODULE_APP | TRACE_NO_OPTION, "\r\nPresence: NO, Radar Distance: 0cm\r\n");
                            }
                            // board_led_off(BOARD_LED_1);
#else
                            // board_led_on(BOARD_LED_1);
                            LOG_INFO(TRACE_MODULE_APP, "Radar output data len: %u\r\n", data_len);
                            xfer_pack.sample_1tnr.length = data_len + SAMPLE_PACK_FIX_LENGTH;
                            xfer_pack.sample_1tnr.seq = radar_env.rx_ant_idx;
                            uint16_t check_num = uwb_radar_data_checksum((const uint8_t *)xfer_pack.sample_1tnr.acd_out, data_len);
#if UWB_RADAR_1TNR_MODE == 1
                            xfer_pack.sample_1tnr.check_num = check_num;
                            xfer_pack.sample_1tnr.end = 0xAAAA;
#else
                            if (uwb_app_config.session_param.uwb_radar_pulse_period == UWB_RADAR_PULSE_PERIOD_16NS)
                            {
                                xfer_pack.sample16.check_num = check_num;
                                xfer_pack.sample16.end = 0xAAAA;
                            }
                            else if (uwb_app_config.session_param.uwb_radar_pulse_period == UWB_RADAR_PULSE_PERIOD_32NS)
                            {
                                xfer_pack.sample32.check_num = check_num;
                                xfer_pack.sample32.end = 0xAAAA;
                            }
                            else if (uwb_app_config.session_param.uwb_radar_pulse_period == UWB_RADAR_PULSE_PERIOD_64NS)
                            {
                                xfer_pack.sample64.check_num = check_num;
                                xfer_pack.sample64.end = 0xAAAA;
                            }
                            else if (uwb_app_config.session_param.uwb_radar_pulse_period == UWB_RADAR_PULSE_PERIOD_128NS)
                            {
                                xfer_pack.sample128.check_num = check_num;
                                xfer_pack.sample128.end = 0xAAAA;
                            }
                            else if (uwb_app_config.session_param.uwb_radar_pulse_period == UWB_RADAR_PULSE_PERIOD_256NS)
                            {
                                xfer_pack.sample256.check_num = check_num;
                                xfer_pack.sample256.end = 0xAAAA;
                            }
#endif
                            uart_send(DUMP_DATA_PORT, (uint8_t *)(&xfer_pack), xfer_pack.sample_1tnr.length, xfer_done);
#endif
#endif
                        }
                        break;
                    }
                }
            }
            break;
        }
    }
    else
    {
        radar_disable();
        power_off_radio();
        power_mode_request(POWER_UNIT_APP, POWER_MODE_POWER_DOWN);
        uwb_radar_force_destroy_event();
        if (SESSION_TYPE_VENDOR_ALONE_RADAR == uwb_app_config.session_type)
        {
            uwb_radar_alone_timer_clr();
        }
    }
    // board_led_off(BOARD_LED_2);
}