/*******************************************************************************
 * @file    ble_tx_power.c
 * @brief   BLE TX power and RSSI monitor helper
 ******************************************************************************/
#include <zephyr/kernel.h>
#include <zephyr/bluetooth/hci.h>
#include <zephyr/bluetooth/hci_vs.h>
#include <zephyr/bluetooth/conn.h>
#include <zephyr/sys/byteorder.h>
#include <errno.h>

#include "ble_tx_power.h"
#include "debug_print.h"

#define BLE_TX_POWER_NORMAL_DBM        4        // NORMAL: 평상시에는 전류 소모를 낮추기 위해 +4 dBm을 기본값으로 사용
#define BLE_TX_POWER_STRESS_DBM        8        // BOOST: RSSI 또는 TX 지연이 나빠지면 일시적으로 +8 dBm으로 boost
#define BLE_RSSI_MONITOR_ENABLED       1
#define BLE_RSSI_STRESS_DBM            (-80)
#define BLE_RSSI_STRESS_COUNT          3
#define BLE_RSSI_MONITOR_INTERVAL_MS   500
#define BLE_TX_SLOW_MS                 50
#define BLE_TX_CRITICAL_MS             100
#define BLE_TX_STUCK_MS                200
#define BLE_TX_SLOW_COUNT              3
#define BLE_TX_BUSY_STRESS_RETRY       4
#define BLE_TX_RECOVER_HOLD_MS         10000

extern int bt_hci_get_conn_handle(const struct bt_conn *conn, uint16_t *conn_handle);

/* HCI vendor command는 bt_conn 포인터 대신 controller handle을 사용 */
static uint16_t current_conn_handle;
static bool current_conn_handle_valid;
static int8_t current_tx_power_dbm = BLE_TX_POWER_NORMAL_DBM;
static uint8_t low_rssi_count;
static uint8_t slow_tx_count;
static int64_t last_stress_ms;
static struct k_work_delayable tx_power_monitor_work;

/* Nordic vendor HCI command로 advertising/connection TX power 직접 설정 */
static int ble_tx_power_write(uint8_t handle_type, uint16_t handle, int8_t dbm, const char *reason)
{
    struct net_buf *buf;
    struct net_buf *rsp;
    struct bt_hci_cp_vs_write_tx_power_level *cp;
    struct bt_hci_rp_vs_write_tx_power_level *rp;
    int err;

    buf = bt_hci_cmd_alloc(K_NO_WAIT);
    if (buf == NULL)
    {
        return -ENOBUFS;
    }

    cp = net_buf_add(buf, sizeof(*cp));
    cp->handle_type = handle_type;
    cp->handle = sys_cpu_to_le16(handle);
    cp->tx_power_level = dbm;

    err = bt_hci_cmd_send_sync(BT_HCI_OP_VS_WRITE_TX_POWER_LEVEL, buf, &rsp);
    if (err)
    {
        DBG_ERR("[BLE] TX power set fail reason=%s err=%d\r\n", reason, err);
        return err;
    }

    rp = (void *)rsp->data;
    if (rp->status != 0U)
    {
        err = -EIO;
        DBG_ERR("[BLE] TX power set rejected reason=%s status=0x%02x\r\n", reason, rp->status);
    }
    else
    {
        if (handle_type == BT_HCI_VS_LL_HANDLE_TYPE_CONN)
        {
            current_tx_power_dbm = rp->selected_tx_power;
        }
        DBG_CORE("[BLE] TX power %d dBm reason=%s\r\n", rp->selected_tx_power, reason);
    }

    net_buf_unref(rsp);
    return err;
}

/* 연결 handle이 유효할 때만 connection TX power 변경 */
static int ble_set_conn_tx_power(int8_t dbm, const char *reason)
{
    // 연결 전이거나 이미 목표 출력이면 HCI 명령을 보내지 않음
    if (!current_conn_handle_valid || current_tx_power_dbm == dbm)
    {
        return 0;
    }

    return ble_tx_power_write(BT_HCI_VS_LL_HANDLE_TYPE_CONN, current_conn_handle, dbm, reason ? reason : "unknown");
}

static void ble_tx_power_stress(const char *reason)
{
    // stress 발생 시각을 기록해서 이후 NORMAL 복귀 시점 판단
    last_stress_ms = k_uptime_get();
    (void)ble_set_conn_tx_power(BLE_TX_POWER_STRESS_DBM, reason);
}

/* 마지막 stress 이후 일정 시간 stress 없는 경우 NORMAL 출력으로 회복 */
static void ble_tx_power_try_recover(void)
{
    // 이미 NORMAL이면 불필요한 HCI 명령 피함
    if ((current_tx_power_dbm != BLE_TX_POWER_NORMAL_DBM) &&
        ((k_uptime_get() - last_stress_ms) >= BLE_TX_RECOVER_HOLD_MS))
    {
        (void)ble_set_conn_tx_power(BLE_TX_POWER_NORMAL_DBM, "link-recovered");
    }
}

/* 컨트롤러에 실제 적용된 연결 TX power 확인 */
static int ble_read_conn_tx_power(int8_t *dbm)
{
    struct net_buf *buf;
    struct net_buf *rsp;
    struct bt_hci_cp_vs_read_tx_power_level *cp;
    struct bt_hci_rp_vs_read_tx_power_level *rp;
    int err;

    // 연결 handle 없이는 controller에 연결 TX power 조회 불가
    if (!current_conn_handle_valid || dbm == NULL)
    {
        return -ENOTCONN;
    }

    buf = bt_hci_cmd_alloc(K_NO_WAIT);
    if (buf == NULL)
    {
        return -ENOBUFS;
    }

    cp = net_buf_add(buf, sizeof(*cp));
    cp->handle_type = BT_HCI_VS_LL_HANDLE_TYPE_CONN;
    cp->handle = sys_cpu_to_le16(current_conn_handle);

    err = bt_hci_cmd_send_sync(BT_HCI_OP_VS_READ_TX_POWER_LEVEL, buf, &rsp);
    if (err)
    {
        return err;
    }

    rp = (void *)rsp->data;
    if (rp->status != 0U)
    {
        err = -EIO;
    }
    else
    {
        *dbm = rp->tx_power_level;
    }

    net_buf_unref(rsp);
    return err;
}

/* 현재 연결의 RSSI를 읽어 링크 품질 판단에 사용 */
static int ble_read_conn_rssi(int8_t *rssi)
{
    struct net_buf *buf;
    struct net_buf *rsp;
    struct bt_hci_cp_read_rssi *cp;
    struct bt_hci_rp_read_rssi *rp;
    int err;

    // RSSI 조회도 연결 handle이 있어야 가능
    if (!current_conn_handle_valid || rssi == NULL)
    {
        return -ENOTCONN;
    }

    buf = bt_hci_cmd_alloc(K_NO_WAIT);
    if (buf == NULL)
    {
        return -ENOBUFS;
    }

    cp = net_buf_add(buf, sizeof(*cp));
    cp->handle = sys_cpu_to_le16(current_conn_handle);

    err = bt_hci_cmd_send_sync(BT_HCI_OP_READ_RSSI, buf, &rsp);
    if (err)
    {
        return err;
    }

    rp = (void *)rsp->data;
    if (rp->status != 0U)
    {
        err = -EIO;
    }
    else
    {
        *rssi = rp->rssi;
    }

    net_buf_unref(rsp);
    return err;
}

/* 연결 중에는 주기적으로 RSSI를 확인하고, 필요하면 TX power boost */
static void tx_power_monitor_handler(struct k_work *work)
{
    int8_t rssi = 0;
    int err;

    ARG_UNUSED(work);

    if (!current_conn_handle_valid)
    {
        return;
    }

    if (!BLE_RSSI_MONITOR_ENABLED)
    {
        return;
    }

    err = ble_read_conn_rssi(&rssi);

    // RSSI를 못 읽는 상황도 링크 품질 저하로 간주
    if (err)
    {
        ble_tx_power_stress("rssi-silence");
    }
    else if (rssi <= BLE_RSSI_STRESS_DBM)
    {
        // 순간적인 RSSI 흔들림은 무시하고, 연속 low RSSI일 때만 boost
        if (low_rssi_count < UINT8_MAX)
        {
            low_rssi_count++;
        }

        if (low_rssi_count >= BLE_RSSI_STRESS_COUNT)
        {
            ble_tx_power_stress("rssi-low");
        }
    }
    else
    {
        // RSSI가 회복되면 연속 low RSSI 카운터 초기화
        low_rssi_count = 0U;
    }

    // TX가 없어도 monitor 주기마다 NORMAL 복귀 여부 확인
    ble_tx_power_try_recover();

    // 연결이 유지되는 동안 monitor work 계속 반복
    k_work_schedule(&tx_power_monitor_work, K_MSEC(BLE_RSSI_MONITOR_INTERVAL_MS));
}

/* RSSI monitor용 delayable work 초기화 */
void ble_tx_power_init(void)
{
    k_work_init_delayable(&tx_power_monitor_work, tx_power_monitor_handler);
}

/* 연결 직후 TX power 상태를 초기화하고 RSSI monitor를 시작 */
void ble_tx_power_on_connected(struct bt_conn *conn)
{
    int8_t cur_txp = 0;
    int txp_ret;

    // 이후 HCI 명령에서 사용할 controller connection handle 저장
    current_conn_handle_valid = (bt_hci_get_conn_handle(conn, &current_conn_handle) == 0);
    current_tx_power_dbm = BLE_TX_POWER_NORMAL_DBM;
    low_rssi_count = 0U;
    slow_tx_count = 0U;
    last_stress_ms = k_uptime_get();

    if (current_conn_handle_valid)
    {
        // 새 연결은 항상 NORMAL (4 dBm) 시작
        (void)ble_tx_power_write(BT_HCI_VS_LL_HANDLE_TYPE_CONN, current_conn_handle, BLE_TX_POWER_NORMAL_DBM, "connected");
    }

    txp_ret = ble_read_conn_tx_power(&cur_txp); // 실제 적용값

    if (txp_ret == 0)
    {
        DBG_CORE("[BLE] TX power (current) = %d dBm\r\n", cur_txp);
    }
    else
    {
        DBG_CORE("[BLE] TX power read fail ret=%d\r\n", txp_ret);
    }

    if (BLE_RSSI_MONITOR_ENABLED)
    {
        // 연결이 유지되는 동안만 RSSI monitor
        k_work_schedule(&tx_power_monitor_work, K_MSEC(BLE_RSSI_MONITOR_INTERVAL_MS));
    }
}

/* 연결 해제 시 monitor와 링크별 상태 초기화 */
void ble_tx_power_on_disconnected(void)
{
    k_work_cancel_delayable(&tx_power_monitor_work);
    current_conn_handle_valid = false;
    current_tx_power_dbm = BLE_TX_POWER_NORMAL_DBM;
    low_rssi_count = 0U;
    slow_tx_count = 0U;
    last_stress_ms = 0;
}

/* advertising handle의 TX power를 NORMAL 값으로 설정 */
void ble_tx_power_apply_advertising(void)
{
    (void)ble_tx_power_write(BT_HCI_VS_LL_HANDLE_TYPE_ADV, 0, BLE_TX_POWER_NORMAL_DBM, "adv");
}

/* ble_service.c가 TX 완료 대기 timeout을 TX power 정책값과 공유 */
uint32_t ble_tx_power_tx_stuck_ms(void)
{
    return BLE_TX_STUCK_MS;
}

/* NUS TX queue가 여러 번 busy이면 링크가 불안정한 상황으로 보고 출력 boost */
void ble_tx_power_on_tx_busy_retry(uint8_t retry_count)
{
    if (retry_count >= BLE_TX_BUSY_STRESS_RETRY)
    {
        ble_tx_power_stress("tx-queue-busy");
    }
}

/* sent callback이 늦게 오면 TX path가 막힌 것으로 보고 boost */
void ble_tx_power_on_tx_complete_timeout(void)
{
    ble_tx_power_stress("tx-complete-stuck");
}

/* TX 완료 시간이 길어지는 것도 링크 품질 저하 신호로 사용 */
void ble_tx_power_on_tx_complete(int64_t elapsed_ms)
{
    if (elapsed_ms >= BLE_TX_CRITICAL_MS)
    {
        // 100ms 이상이면 단발성이라도 즉시 boost
        ble_tx_power_stress("tx-complete-slow100");
        slow_tx_count = 0U;
    }
    else if (elapsed_ms >= BLE_TX_SLOW_MS)
    {
        // 50ms 이상은 연속 발생할 때만 boost
        if (slow_tx_count < UINT8_MAX)
        {
            slow_tx_count++;
        }

        if (slow_tx_count >= BLE_TX_SLOW_COUNT)
        {
            ble_tx_power_stress("tx-complete-slow50");
        }
    }
    else
    {
        // 정상 속도로 전송되면 slow 카운터 지우고 복귀 가능성 확인
        slow_tx_count = 0U;
        ble_tx_power_try_recover();
    }
}