#include <math.h>
#include <string.h>
#include "esp_log.h"
#include "freertos/FreeRTOS.h"
#include "freertos/semphr.h"
#include "nvs.h"
#include "fingerprint.h"
#include "matcher.h"
#include "template.h"

_Static_assert(TEMPLATE_NUM_BANDS == NUM_BANDS,
               "template.h was generated with a different NUM_BANDS: rerun tools/make_template.py");

static const char *TAG = "matcher";
#define NVS_NS "buzz"
#define TPL_KEY "tpl"
#define TPL_MAGIC 0x42555A54u   // 'BUZT'

typedef struct {
    uint32_t magic;
    uint32_t num_bands;
    uint32_t frame_count;
    float fp[NUM_BANDS];
} tpl_blob_t;

static SemaphoreHandle_t s_mx;
static float s_tpl[NUM_BANDS];
static tpl_source_t s_src = TPL_NONE;
static uint32_t s_count;

static bool valid_unit_vector(const float *v)
{
    float s = 0;
    for (int i = 0; i < NUM_BANDS; i++) {
        if (!isfinite(v[i])) return false;
        s += v[i] * v[i];
    }
    return fabsf(sqrtf(s) - 1.0f) < 0.01f;
}

static bool load_nvs(void)
{
    nvs_handle_t h;
    if (nvs_open(NVS_NS, NVS_READONLY, &h) != ESP_OK) return false;
    tpl_blob_t b;
    size_t len = sizeof(b);
    esp_err_t e = nvs_get_blob(h, TPL_KEY, &b, &len);
    nvs_close(h);
    if (e != ESP_OK || len != sizeof(b) || b.magic != TPL_MAGIC ||
        b.num_bands != NUM_BANDS || !valid_unit_vector(b.fp)) {
        return false;
    }
    memcpy(s_tpl, b.fp, sizeof(s_tpl));
    s_count = b.frame_count;
    s_src = TPL_NVS;
    return true;
}

static void load_builtin_or_none(void)
{
#if TEMPLATE_VALID
    memcpy(s_tpl, TEMPLATE_FP, sizeof(s_tpl));
    if (valid_unit_vector(s_tpl)) {
        s_count = TEMPLATE_FRAME_COUNT;
        s_src = TPL_BUILTIN;
        return;
    }
    ESP_LOGE(TAG, "compiled-in template is not a unit vector, ignoring it");
#endif
    memset(s_tpl, 0, sizeof(s_tpl));
    s_count = 0;
    s_src = TPL_NONE;
}

esp_err_t matcher_init(void)
{
    s_mx = xSemaphoreCreateMutex();
    if (!s_mx) return ESP_ERR_NO_MEM;
    if (!load_nvs()) load_builtin_or_none();
    ESP_LOGI(TAG, "template source: %s (%lu frames)", matcher_source_name(), (unsigned long)s_count);
    return ESP_OK;
}

bool matcher_has_template(void) { return s_src != TPL_NONE; }
tpl_source_t matcher_source(void) { return s_src; }
uint32_t matcher_frame_count(void) { return s_count; }

const char *matcher_source_name(void)
{
    switch (s_src) {
    case TPL_NVS: return "learned on-device (NVS)";
    case TPL_BUILTIN: return "compiled-in template.h";
    default: return "NONE (needs template)";
    }
}

bool matcher_suggested_threshold(float *thr)
{
#if TEMPLATE_HAS_SUGGESTED_THRESHOLD
    *thr = TEMPLATE_SUGGESTED_THRESHOLD;
    return true;
#else
    (void)thr;
    return false;
#endif
}

float matcher_score(const float fp[NUM_BANDS])
{
    float s = 0.0f;
    xSemaphoreTake(s_mx, portMAX_DELAY);
    if (s_src != TPL_NONE) s = features_dot(fp, s_tpl);
    xSemaphoreGive(s_mx);
    return s;
}

void matcher_get_template(float out[NUM_BANDS])
{
    xSemaphoreTake(s_mx, portMAX_DELAY);
    memcpy(out, s_tpl, sizeof(s_tpl));
    xSemaphoreGive(s_mx);
}

esp_err_t matcher_set_template(const float fp[NUM_BANDS], uint32_t frame_count)
{
    if (!valid_unit_vector(fp)) return ESP_ERR_INVALID_ARG;
    nvs_handle_t h;
    esp_err_t err = nvs_open(NVS_NS, NVS_READWRITE, &h);
    if (err != ESP_OK) return err;
    tpl_blob_t b = { .magic = TPL_MAGIC, .num_bands = NUM_BANDS, .frame_count = frame_count };
    memcpy(b.fp, fp, sizeof(b.fp));
    err = nvs_set_blob(h, TPL_KEY, &b, sizeof(b));
    if (err == ESP_OK) err = nvs_commit(h);
    nvs_close(h);
    if (err != ESP_OK) {
        ESP_LOGE(TAG, "saving template: %s", esp_err_to_name(err));
        return err;
    }
    xSemaphoreTake(s_mx, portMAX_DELAY);
    memcpy(s_tpl, fp, sizeof(s_tpl));
    s_count = frame_count;
    s_src = TPL_NVS;
    xSemaphoreGive(s_mx);
    return ESP_OK;
}

esp_err_t matcher_reset(void)
{
    nvs_handle_t h;
    esp_err_t err = nvs_open(NVS_NS, NVS_READWRITE, &h);
    if (err != ESP_OK) return err;
    err = nvs_erase_key(h, TPL_KEY);
    if (err == ESP_ERR_NVS_NOT_FOUND) err = ESP_OK;
    if (err == ESP_OK) err = nvs_commit(h);
    nvs_close(h);
    if (err != ESP_OK) return err;
    xSemaphoreTake(s_mx, portMAX_DELAY);
    load_builtin_or_none();
    xSemaphoreGive(s_mx);
    return ESP_OK;
}