#include <math.h>
#include "orx_power.h"

float orx_power_combine_db(uint16 fagc_cw, uint16 rms)
{
    /* 环路方向(dpd_rx_rfagc_1x.vhd:179-187): 输入升高 -> 实测高于设定点
     * -> agcctrlword 递增 -> gain_index = ~cw[15:8] 递减 -> 增益降低。
     * 故 cw 高字节随输入功率单调递增, 与 rms 项同向, 可直接相加。 */
    if (rms == 0)
        rms = 1;                       /* 防 log10(0) 产生 -inf */
    return 0.5f * (float)((fagc_cw >> 8) & 0xff)
         + 10.0f * log10f((float)rms);
}

uint16 orx_median5(const uint16 *v)
{
    uint16 a[5];
    int i, j;

    for (i = 0; i < 5; i++)
        a[i] = v[i];

    for (i = 1; i < 5; i++) {          /* 插入排序, n=5 足够 */
        uint16 key = a[i];
        for (j = i - 1; j >= 0 && a[j] > key; j--)
            a[j + 1] = a[j];
        a[j + 1] = key;
    }
    return a[2];
}

uint8 orx_fssi_percent(float p_db, float ref_db, int is_vhf)
{
    float pct = (p_db - ref_db) * 100.0f / ORX_FSSI_SPAN_DB;
    int   f;

    if (pct < 0.0f)   pct = 0.0f;
    if (pct > 100.0f) pct = 100.0f;
    f = (int)pct;

    /* 以下两段是 EMV7 时代的显示微调, 按现状保留(见 spec 3.3)。
     * 注意它们作用在线性映射之后, 会吃掉量程顶部 20%(VHF 38%),
     * 故 ref_db 必须按微调后的最终显示值标定。 */
    if (f > 20) f -= 20;
    else        f = 0;

    if (is_vhf && f > 18)
        f -= 18;

    return (uint8)f;
}

uint8 orx_fwd_percent(float p_db, float ref_db)
{
    float pct = 100.0f * powf(10.0f, (p_db - ref_db) / 10.0f);

    if (pct < 0.0f)   pct = 0.0f;
    if (pct > 200.0f) pct = 200.0f;
    return (uint8)pct;
}

uint8 orx_fssi_hysteresis(uint8 prev, uint8 now)
{
    int d = (int)now - (int)prev;

    if (d < 0)
        d = -d;
    return (d < 2) ? prev : now;
}