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

#ifndef MK8000_KF_TOP_H
#define MK8000_KF_TOP_H
#include "mk_common.h"

#define ANGLE_AMBIGUITY_THRES 280

enum KF_CHANNEL_TYPE_T
{
    KF_CHANNEL_TYPE_RANGING = 0,
    KF_CHANNEL_TYPE_ANGLE = 1,
    KF_CHANNEL_TYPE_ANGLE_POSITIVE = 2,
};

typedef struct
{
    float dt;
    float Q;               // covariance of the process noise
    float rR;              // covariance of the obervation noise for ranging
    float aR;              // covariance of the obervation noise for azimuth & elevation
    float r_v_max;         // ranging velocity limit for state estimation
    float a_v_max;         // azimuth/elevation velocity limit for state estimation
    uint16_t r_vlimit_opt; // enable ranging velocity limitation
    uint16_t a_vlimit_opt; // enable azimuth/elevation velocity limitation
} kf_params_t;

extern uint8_t pdoa_3d_ant_layout_get(void);

typedef struct
{
    int row, col;
    float *element;
} Mat;

typedef struct
{
    Mat H;
    Mat F;
    Mat G;
    Mat P_t_tm1;
    Mat X_t_tm1;
    Mat K_t;
    Mat X_t_t;
    float last_post_range;
    float v_max;
    float v_max_default;
    float q_variance;
    uint16_t v_state;
    uint16_t v_max_cnt;
} kf_channel_env_t;

typedef struct
{
    float mat_H[1 * 2];
    float mat_P_t_tm1[2 * 2];
    float mat_X_t_tm1[2 * 1];
    float mat_K_t[2 * 1];
    float mat_X_t_t[2 * 1];
    float mat_F[2 * 2];
    float mat_G[2 * 1];
} kf_channel_mat_value_t;

void MK8000_kf_config(kf_params_t params, kf_channel_env_t *channel_env, kf_channel_mat_value_t *channel_mat_value, size_t cache_len);

void MK8000_kf_processor(float data_meas, uint16_t channel, enum KF_CHANNEL_TYPE_T type, int8_t do_init, float *post_data);

#endif // MK8000_KF_TOP_H