/*
 * proxvec.c -- Proximity Vector Engine implementation
 *
 * SPDX-License-Identifier: GPL-2.0-or-later
 *
 * Converts smoothed RSSI values from bledsp into abstract grid
 * coordinates, tracks per-target velocity via a finite-state machine,
 * and determines alarm state.
 *
 * Coordinate model:
 *   - Center of grid = the H2 device (the "holder")
 *   - Grid range: [0, PROXVEC_GRID_SIZE) in both axes
 *   - Center = (GRID_SIZE/2, GRID_SIZE/2)
 *   - Distance from center = "radius" in grid units
 *   - RSSI maps to radius via a log-distance path-loss model
 *   - Azimuth is pseudo-random at first detection (BLE has no
 *     directional info) and drifts slightly based on RSSI delta
 *     to give visual feedback of movement direction.
 */

#include "proxvec.h"
#include "log_manager.h"

#include <string.h>
#include <stdlib.h>
#include <math.h>

/* ------------------------------------------------------------------ */
/*  Internal state                                                     */
/* ------------------------------------------------------------------ */

static proxvec_target_t targets[PROXVEC_MAX_TARGETS];
static int              target_count;
static int              alarm_radius = PROXVEC_ALARM_RADIUS;
static uint32_t         update_tick;

/* ------------------------------------------------------------------ */
/*  Helpers                                                            */
/* ------------------------------------------------------------------ */

/* Log-distance path-loss model (fixed-point, x10 to avoid floats).
 *
 *   distance = 10 ^ ((TxPower - RSSI) / (10 * n))
 *
 * We compute this with a lookup table to avoid libm on the target.
 * Table maps (TxPower - RSSI) in dB to a grid-radius value.
 * Range: 0 dB (on top of us) to 80 dB (far away).
 *
 * grid_radius = scale * 10^((delta_dB * 10) / (10 * n * 10))
 *             = scale * 10^(delta_dB / n10)
 * where n10 = PROXVEC_PATH_LOSS_N (= 25, representing n=2.5).
 *
 * We precompute: for delta_dB 0..80, radius = (GRID_SIZE/2 - 10) * (1 - dB/100)
 * This is a linear approximation that's good enough for a tripwire alarm.
 */
#define DB_TO_GRID_MAX  80

static int8_t db_to_grid[DB_TO_GRID_MAX + 1];

static void build_db_table(void)
{
    int max_r = PROXVEC_GRID_SIZE / 2 - 10;
    for (int dB = 0; dB <= DB_TO_GRID_MAX; dB++) {
        /* Linear mapping: 0 dB -> radius 5, 80 dB -> radius max_r */
        int r = 5 + (max_r - 5) * dB / DB_TO_GRID_MAX;
        if (r > max_r) r = max_r;
        db_to_grid[dB] = (int8_t)r;
    }
}

/* Convert RSSI to grid radius.  Clamp to table bounds. */
static int rssi_to_radius(int8_t rssi)
{
    int delta = PROXVEC_TX_POWER_DEFAULT / 10 - (int)rssi;
    /* Tx is ~4 dBm, so delta = 4 - rssi.
     * For rssi -40: delta = 44 -> close
     * For rssi -90: delta = 94 -> far (clamped to 80) */
    if (delta < 0) delta = 0;
    if (delta > DB_TO_GRID_MAX) delta = DB_TO_GRID_MAX;
    return (int)db_to_grid[delta];
}

/* Deterministic pseudo-random angle from MAC address.
 * Uses a simple hash to spread devices around the circle. */
static int mac_to_angle(const uint8_t *mac)
{
    uint32_t h = (uint32_t)mac[0] | ((uint32_t)mac[1] << 8) |
                 ((uint32_t)mac[2] << 16) | ((uint32_t)mac[3] << 24);
    h ^= h >> 16;
    h *= 0x45d9f3b;
    h ^= h >> 16;
    return (int)(h % 360);
}

/* Find a target slot by MAC, or allocate a new one. */
static int find_target(const uint8_t *mac)
{
    for (int i = 0; i < target_count; i++) {
        if (memcmp(targets[i].mac, mac, 6) == 0)
            return i;
    }
    if (target_count >= PROXVEC_MAX_TARGETS) {
        /* Evict the oldest LOST target */
        for (int i = 0; i < target_count; i++) {
            if (targets[i].state == TRACK_LOST) {
                memset(&targets[i], 0, sizeof(proxvec_target_t));
                memcpy(targets[i].mac, mac, 6);
                return i;
            }
        }
        return -1;
    }
    int idx = target_count++;
    memset(&targets[idx], 0, sizeof(proxvec_target_t));
    memcpy(targets[idx].mac, mac, 6);
    return idx;
}

/* Map RSSI delta to azimuth drift (degrees).
 * A strengthening signal (negative delta) drifts the angle
 * slightly to give visual impression of approach direction. */
static int delta_to_azimuth_drift(int8_t rssi_delta)
{
    /* Clamp to [-5, +5] degrees per tick */
    int drift = (int)rssi_delta / 4;
    if (drift < -5) drift = -5;
    if (drift >  5) drift =  5;
    return -drift;  /* invert: stronger signal -> negative delta -> positive drift */
}

/* Map smoothed RSSI to intensity [0, 255].
 * -40 dBm (very close) -> 255; -100 dBm (far) -> 20 */
static uint8_t rssi_to_intensity(int8_t rssi)
{
    int t = (rssi + 100) * 255 / 60;
    if (t < 20)  t = 20;
    if (t > 255) t = 255;
    return (uint8_t)t;
}

/* ------------------------------------------------------------------ */
/*  Public API                                                         */
/* ------------------------------------------------------------------ */

void proxvec_init(void)
{
    memset(targets, 0, sizeof(targets));
    target_count = 0;
    update_tick  = 0;
    build_db_table();
    LOG_INF("proxvec: initialised (alarm_radius=%d)", alarm_radius);
}

void proxvec_update(const bledsp_device_t *devices, int count)
{
    uint32_t now_ms;
    struct timespec ts;
    clock_gettime(CLOCK_MONOTONIC, &ts);
    now_ms = (uint32_t)(ts.tv_sec * 1000U + ts.tv_nsec / 1000000U);

    update_tick++;

    /* Mark all existing targets as potentially lost */
    for (int i = 0; i < target_count; i++) {
        if (targets[i].state != TRACK_LOST) {
            /* Will be re-confirmed below if device still active */
        }
    }

    /* Update or create targets from the device table */
    for (int d = 0; d < count; d++) {
        int idx = find_target(devices[d].mac);
        if (idx < 0) continue;

        proxvec_target_t *t = &targets[idx];
        t->prev_radius = t->radius;
        t->age_ticks++;

        /* Convert RSSI to grid radius */
        t->radius = (int16_t)rssi_to_radius(devices[d].rssi_smoothed);

        /* Compute radial velocity */
        t->velocity = t->radius - t->prev_radius;

        /* Compute x, y from radius and angle */
        int base_angle;
        if (t->age_ticks == 1) {
            /* First detection: assign a pseudo-random angle from MAC */
            t->x = PROXVEC_GRID_SIZE / 2;  /* will be overwritten */
            t->y = PROXVEC_GRID_SIZE / 2;
        }

        base_angle = mac_to_angle(devices[d].mac);
        /* Apply azimuth drift based on RSSI delta */
        base_angle += delta_to_azimuth_drift(devices[d].rssi_delta) * t->age_ticks;
        base_angle = ((base_angle % 360) + 360) % 360;

        /* Polar to cartesian, centered on grid */
        int cx = PROXVEC_GRID_SIZE / 2;
        int cy = PROXVEC_GRID_SIZE / 2;
        double rad = (double)base_angle * 3.14159265 / 180.0;
        t->x = cx + (int16_t)((double)t->radius * cos(rad));
        t->y = cy + (int16_t)((double)t->radius * sin(rad));

        /* Clamp to grid */
        if (t->x < 0) t->x = 0;
        if (t->y < 0) t->y = 0;
        if (t->x >= PROXVEC_GRID_SIZE) t->x = PROXVEC_GRID_SIZE - 1;
        if (t->y >= PROXVEC_GRID_SIZE) t->y = PROXVEC_GRID_SIZE - 1;

        /* Intensity from RSSI */
        t->intensity = rssi_to_intensity(devices[d].rssi_smoothed);

        /* State machine transitions */
        if (t->age_ticks == 1) {
            t->state = TRACK_NEW;
        } else {
            switch (t->state) {
            case TRACK_NEW:
                t->state = TRACK_STATIONARY;
                break;
            case TRACK_STATIONARY:
                if (t->velocity < -2)      t->state = TRACK_APPROACHING;
                else if (t->velocity > 2)  t->state = TRACK_RECEDING;
                break;
            case TRACK_APPROACHING:
                if (t->velocity >= 0)       t->state = TRACK_STATIONARY;
                else if (t->velocity > 2)   t->state = TRACK_RECEDING;
                break;
            case TRACK_RECEDING:
                if (t->velocity <= 0)       t->state = TRACK_STATIONARY;
                else if (t->velocity < -2)  t->state = TRACK_APPROACHING;
                break;
            case TRACK_LOST:
                t->state = TRACK_NEW;  /* re-acquired */
                t->age_ticks = 1;
                break;
            }
        }

        /* Alarm check */
        t->alarm = (t->radius <= alarm_radius) ? 1 : 0;
    }

    /* Mark stale targets as LOST */
    for (int i = 0; i < target_count; i++) {
        if (targets[i].state != TRACK_LOST) {
            int found = 0;
            for (int d = 0; d < count; d++) {
                if (memcmp(targets[i].mac, devices[d].mac, 6) == 0) {
                    found = 1;
                    break;
                }
            }
            if (!found) {
                /* Check stale timeout */
                /* We don't store last_seen in target, use age as proxy */
                if (targets[i].age_ticks > 3) {  /* missed 3+ polls */
                    targets[i].state = TRACK_LOST;
                    targets[i].alarm = 0;
                    targets[i].intensity = 0;
                }
            }
        }
    }
}

int proxvec_get_targets(proxvec_target_t *out, int max)
{
    int written = 0;
    for (int i = 0; i < target_count && written < max; i++) {
        if (targets[i].state != TRACK_LOST) {
            out[written++] = targets[i];
        }
    }
    return written;
}

int proxvec_get_alarm_count(void)
{
    int count = 0;
    for (int i = 0; i < target_count; i++) {
        if (targets[i].alarm) count++;
    }
    return count;
}

int proxvec_get_track_count(void)
{
    int count = 0;
    for (int i = 0; i < target_count; i++) {
        if (targets[i].state != TRACK_LOST) count++;
    }
    return count;
}

void proxvec_reset(void)
{
    memset(targets, 0, sizeof(targets));
    target_count = 0;
    update_tick  = 0;
}

void proxvec_set_alarm_radius(int16_t r) { alarm_radius = r; }
int16_t proxvec_get_alarm_radius(void) { return alarm_radius; }

void proxvec_shutdown(void)
{
    memset(targets, 0, sizeof(targets));
    target_count = 0;
    LOG_INF("proxvec: shut down");
}