/*
 * 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_power.h"
#include "mk_uwb.h"
#include "mk_clock.h"
#include "uwb_api.h"
#include "aoa.h"
#include "lib_aoa.h"
#if PDOA_3D_EN
#include "lib_pdoa_3d.h"
#endif

#include "board.h"

#if PDOA_3D_EN
#define PDOA_3D_SUPPORT_NUM 100
#define PDOA_3D_TIMEOUT_MS 2000
static struct PDOA_3D_MAC_ADDR_T mac_addr_cache[PDOA_3D_SUPPORT_NUM];
static struct PDOA_3D_PDOA_DATA_T pdoa_data_cache[PDOA_3D_SUPPORT_NUM];
#endif

struct AOA_ENV_T aoa_env;

static struct AOA_CB_T aoa_cb;

static struct UWB_OP_T op = {
    .session_configure = aoa_configure,
    .session_start = aoa_start,
    .session_stop = NULL,
    .session_local_addr_set = NULL,
    .session_peer_addr_set = NULL,
    .session_responder_addr_add = NULL,
    .session_responder_list_clr = NULL,
    .session_set_ccc_ursk = NULL,
};

static void aoa_tx_process(struct MAC_HW_REPORT_T *tx_report);
static void aoa_rx_process(struct MAC_HW_REPORT_T *rx_report);
void app_session_init(void);

//------------------------------------------------------------------------------
int aoa_init(uint8_t handle_id)
{
    /* store handler ID */
    aoa_cb.handle_id = handle_id;

    /* init rx queue */
    WSF_QUEUE_INIT(&aoa_cb.msg_queue);

    LOG_INFO(TRACE_MODULE_APP, "AoA lib version: %s\r\n", MK8000_get_aoalib_version());
    return 0;
}

int aoa_deinit(void)
{
    return 0;
}

// This function will be called by the uwbapi_session_init()
void app_session_init(void)
{
    // register process handler for MAC TX done and RX done
    mac_register_process_handler(aoa_tx_process, aoa_rx_process);

    uwbs_handler_init(&op);
}

void aoa_configure(void)
{
    aoa_env.stage = AOA_IDLE;
    aoa_env.ranging_period = MS_TO_PHY_TIMER_COUNT(uwb_app_config.session_param.ranging_interval);
    aoa_env.lost_cnt = 0;

    uwbs_configure(PHY_TX | PHY_RX, uwb_app_config.session_param.tx_power_level);

    if (uwb_app_config.session_param.device_role == DEV_ROLE_INITIATOR)
    {
        phy_rx_sts_switch_mode_set(uwb_app_config.ppdu_params.sts_pkt_cfg, STS_NEVER_SWITCH, 0, 0);
    }
    else
    {
        phy_rx_sts_switch_mode_set(uwb_app_config.ppdu_params.sts_pkt_cfg, STS_SWITCH_EVERY_4SYM, 0, 0);
    }

#if (ANT_PATTERN == ANT_PATTERN_SQUARE)
    struct AOA_ANGLE_SPAN_T aoa_span;
#if AOA_3D_EN
    aoa_span.Ndim = 2;
    aoa_span.el_low = 0;
    aoa_span.el_high = 90;
    aoa_span.el_step = 5;
    aoa_span.az_low = 0;
    aoa_span.az_high = 359;
    aoa_span.az_step = 3;
#else
    aoa_span.Ndim = 2;
    aoa_span.el_low = 90;
    aoa_span.el_high = 90;
    aoa_span.el_step = 5;
    aoa_span.az_low = 0;
    aoa_span.az_high = 359;
    aoa_span.az_step = 1;
#endif
    aoa_angle_search_span_set(&aoa_span);
#endif

#if AOA_EN
    aoa_aux_info_set(AOA_AUX_ANT_IQ_RSSI_PDOA_AOA_FOM);
    aoa_steering_vector_set((const float *)((uint32_t)((uwb_app_config.ppdu_params.ch_num == 9) ? svec_ch9_ptr : svec_ch5_ptr) | SRAM_BASE));
#else
    aoa_aux_info_set(AOA_AUX_ANT_IQ_RSSI);
#endif

    aoa_param_config();

#if PDOA_3D_EN
    pdoa_3d_param_config(ANT_PATTERN, ANT_LAYOUT, PDOA_3D_AMBIGUITY_LEVEL_HIGH, mac_addr_cache, pdoa_data_cache, PDOA_3D_SUPPORT_NUM, PDOA_3D_TIMEOUT_MS);
#endif

#if FILTER_EN
    if (uwb_app_config.filter_en)
    {
        loc_post_filter_config(uwb_app_config.session_param.ranging_interval, 0, 1);
    }
#endif
}

static void aoa_timer_callback(void *dev, uint32_t time)
{
    aoa_env.anchor_point += aoa_env.ranging_period;
    aoa_env.sequence_num++;
    if (uwb_app_config.session_param.device_role == DEV_ROLE_INITIATOR)
    {
        // Power on radio
        power_on_radio(1, 0);

        // SP3 payload len = 0
        mac_tx(EVT_MODE_MAC_ASAP_PHY_FIX, aoa_env.anchor_point, 0, NULL, 0);
        mac_start();

        LOG_INFO(TRACE_MODULE_APP, "AoA Initiator SEQ NUM %u\r\n", aoa_env.sequence_num);
    }
    else
    {
        // Change Rx main antenna
        uint8_t main_ant = 3;
        phy_rx_sts_switch_mode_set(uwb_app_config.ppdu_params.sts_pkt_cfg, STS_SWITCH_EVERY_4SYM, 1, main_ant);
        aoa_param_update(main_ant);
        // Power on radio
        power_on_radio(0, 1);
        sts_lsp_store();
        if (aoa_env.stage == AOA_SYNC)
        {
            mac_rx(EVT_MODE_MAC_PHY_ASAP, 0, aoa_env.ranging_period);
        }
        else
        {
            mac_rx(EVT_MODE_MAC_ASAP_PHY_FIX, aoa_env.anchor_point - UWB_RX_OPEN_IN_ADVANCE, UWB_RX_WINDOW);
        }
        mac_start();
        LOG_INFO(TRACE_MODULE_APP, "AoA Responder SEQ NUM %u\r\n", aoa_env.sequence_num);
    }
}

void aoa_start(void)
{
    aoa_env.anchor_point = phy_timer_count_get();
    if (uwb_app_config.session_param.device_role == DEV_ROLE_INITIATOR)
    {
        aoa_env.stage = AOA_POLL;
    }
    else
    {
        aoa_env.stage = AOA_SYNC;
    }
    phy_timer_target_set(aoa_env.anchor_point + aoa_env.ranging_period - UWB_EVT_PREFETCH_TIME, aoa_timer_callback);
    aoa_env.sequence_num = 0;
}

void aoa_process(const struct MAC_HW_REPORT_T *ind)
{
    if (uwb_app_config.session_param.device_role == DEV_ROLE_INITIATOR)
    {
        phy_timer_target_set(aoa_env.anchor_point + aoa_env.ranging_period - UWB_EVT_PREFETCH_TIME, aoa_timer_callback);
    }
    else
    {
        if (ind->err_code == UWB_RX_OK)
        {
            sts_lsp_store_stop();
            // update anchor point
            aoa_env.anchor_point = ind->timestamp - phy_shr_duration();
            aoa_env.stage = AOA_POLL;
            aoa_env.lost_cnt = 0;

            if (aoa_done_flag)
            {
                aoa_done_flag = 0;
                aoa_done(ind);
            }
        }
        else
        {
            if (aoa_env.stage == AOA_SYNC)
            {
                sts_lsp_store();
                mac_rx(EVT_MODE_MAC_PHY_ASAP, 0, aoa_env.ranging_period);
                mac_start();
                return;
            }
            else
            {
                aoa_env.lost_cnt++;
                if (aoa_env.lost_cnt > 3)
                {
                    aoa_env.stage = AOA_SYNC;
                }
            }
        }
        phy_timer_target_set(aoa_env.anchor_point + aoa_env.ranging_period - UWB_EVT_PREFETCH_TIME, aoa_timer_callback);
    }

    power_off_radio();
}

void aoa_done(const struct MAC_HW_REPORT_T *rx)
{
    struct AOA_DONE_IND_T *ind;

    if ((ind = WsfMsgAlloc(sizeof(struct AOA_DONE_IND_T) + rx->pkt_len)) != NULL)
    {
        ind->hdr.event = AOA_DONE_MSG;
        ind->status = rx->err_code;
        ind->rssi = rx->rssi;
        ind->snr = rx->snr;

        if ((rx->pkt_len) && (rx->pkt_data != NULL))
        {
            memcpy(ind->rx_data, rx->pkt_data, rx->pkt_len);
        }
        ind->rx_len = rx->pkt_len;

        // Send the message
        WsfMsgSend(aoa_cb.handle_id, ind);
    }
}

static void aoa_tx_process(struct MAC_HW_REPORT_T *tx_report)
{
    aoa_process(tx_report);
}

static void aoa_rx_process(struct MAC_HW_REPORT_T *rx_report)
{
    aoa_process(rx_report);
}