/*
 * 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.
 */

/*
 *
 *  Modification History:
 *
 *
 */
#include "MK8000_kf_top.h"
#include "mk_trace.h"
#include "lib_kf.h"
#include "mk_misc.h"

static struct KF_MAC_ADDR_T *addr_arr;
static size_t kf_support_num = 0;
static uint32_t kf_timeout_ms = 0;

static int8_t do_init = 1;

static kf_params_t g_kf_params = {
    0.048f,    // should be equal to update time
    0.53356f,  // Q = (9.8/3)^2/20
    0.0036f,   // ranging_R = 0.06^2
    0.000625f, // azimuth_R = 0.025^2
    0.2f,      // ranging v_max in m/s
    10.0f,     // azimuth v_max in degree/s
    1,         // enable ranging velocity limitation
    1,         // enable azimuth/elevation velocity limitation
};

static uint16_t get_addr_index(uint8_t *mac_addr, uint8_t data_type)
{
    for (uint16_t i = 0; i < kf_support_num; i++)
    {
        if ((memcmp(addr_arr[i].mac_addr, mac_addr, 8) == 0) && (addr_arr[i].data_type == data_type))
        {
            return i;
        }
    }
    uint32_t min_time = addr_arr[0].elapsed_time;
    uint16_t min_time_index = 0;
    for (uint16_t i = 0; i < kf_support_num; i++)
    {
        if (addr_arr[i].elapsed_time < min_time)
        {
            min_time = addr_arr[i].elapsed_time;
            min_time_index = i;
        }
    }
    memcpy(addr_arr[min_time_index].mac_addr, mac_addr, 8);
    addr_arr[min_time_index].data_type = data_type;
    addr_arr[min_time_index].elapsed_time = 0;
    return min_time_index;
}

void loc_post_kf_config(uint32_t update_period_ms,
                        struct KF_MAC_ADDR_T *mac_addr_cache,
                        struct KF_CHANNEL_CACHE_T *kf_channel_cache,
                        struct KF_MAT_VALUE_CACHE_T *mat_value_cache,
                        size_t cache_len,
                        uint32_t timeout_ms)
{
    g_kf_params.dt = (float)(update_period_ms / 1000.0);
    addr_arr = mac_addr_cache;
    memset(addr_arr, 0, sizeof(struct KF_MAC_ADDR_T) * cache_len);
    kf_support_num = cache_len;
    kf_timeout_ms = timeout_ms;

    MK8000_kf_config(g_kf_params, (kf_channel_env_t *)kf_channel_cache, (kf_channel_mat_value_t *)mat_value_cache, cache_len);
}

uint8_t loc_kf_filter(float data_meas, enum KF_DATA_TYPE_T data_type, uint8_t *mac_addr, float *data_post)
{
    if (kf_support_num == 0)
    {
        return 0;
    }
    uint16_t id = get_addr_index(mac_addr, (uint8_t)data_type);
    uint32_t now = sys_tick_ms();
    if (addr_arr[id].elapsed_time == 0 || (now - addr_arr[id].elapsed_time) > kf_timeout_ms)
    {
        do_init = 1;
    }
    addr_arr[id].elapsed_time = now;
    enum KF_CHANNEL_TYPE_T type = KF_CHANNEL_TYPE_RANGING;
    if (data_type == KF_DATA_TYPE_AZIMUTH)
    {
        type = KF_CHANNEL_TYPE_ANGLE;
    }
    else if (data_type == KF_DATA_TYPE_ELEVATION)
    {
        if (pdoa_3d_ant_layout_get() == 0)
        {
            type = KF_CHANNEL_TYPE_ANGLE_POSITIVE;
        }
        else
        {
            type = KF_CHANNEL_TYPE_ANGLE;
        }
    }
    MK8000_kf_processor(data_meas, id, type, do_init, data_post);
    do_init = 0;
    return 1;
}