#include "ld2420_radar.h"
#include <string.h>
#include <math.h>
#include <stdio.h>

/*
    =========================================================
    LD2420 Radar — реализация
    Energy mode + конфигурация через командный протокол
    =========================================================
*/

/* ---- constexpr definitions ----------------------------- */

constexpr uint8_t Ld2420Radar::CMD_HDR[4];
constexpr uint8_t Ld2420Radar::CMD_FTR[4];
constexpr uint8_t Ld2420Radar::DATA_HDR[4];
constexpr uint8_t Ld2420Radar::DATA_FTR[4];


/* ====================================================
   ВСПОМОГАТЕЛЬНЫЕ ФУНКЦИИ КОМАНДНОГО ПРОТОКОЛА
   ==================================================== */

/*
    Формат команды:
      CMD_HDR (4) | length (2, LE) | payload (N) | CMD_FTR (4)
    length = кол-во байт payload
*/
bool Ld2420Radar::_send_cmd(const uint8_t* payload, uint8_t payload_len) {
    /* Заголовок */
    _serial->write(CMD_HDR, 4);

    /* Длина payload (uint16 LE) */
    _serial->write((uint8_t)(payload_len & 0xFF));
    _serial->write((uint8_t)(payload_len >> 8));

    /* Payload */
    _serial->write(payload, payload_len);

    /* Футер */
    _serial->write(CMD_FTR, 4);
    _serial->flush();

    return _wait_ack();
}

/*
    Ждём ACK — фрейм начинающийся с FD FC FB FA.
    Не парсим полностью, просто проверяем что он пришёл.
*/
bool Ld2420Radar::_wait_ack() {
    uint32_t deadline = millis() + ACK_TIMEOUT_MS;
    uint8_t  hdr_match = 0;

    while (millis() < deadline) {
        if (!_serial->available()) { delay(1); continue; }

        uint8_t b = _serial->read();

        if (b == CMD_HDR[hdr_match]) {
            hdr_match++;
            if (hdr_match == 4) {
                /* Заголовок найден — ACK получен, читаем остаток */
                uint32_t t = millis() + 50;
                while (millis() < t) {
                    if (_serial->available()) _serial->read();
                }
                return true;
            }
        } else {
            hdr_match = (b == CMD_HDR[0]) ? 1 : 0;
        }
    }
    return false;
}

void Ld2420Radar::_flush_rx() {
    uint32_t t = millis() + 100;
    while (millis() < t) {
        if (_serial->available()) _serial->read();
        else delay(1);
    }
}


/* ====================================================
   КОНФИГУРАЦИЯ РАДАРА
   ==================================================== */

bool Ld2420Radar::_configure() {
    Serial.println("[LD2420] Starting configuration...");

    /* ---- 1. Войти в config mode ---- */
    /*  Payload: CMD(2) + protocol_version(2) = FF 00 02 00  */
    {
        uint8_t p[] = {0xFF, 0x00, 0x02, 0x00};
        bool ok = false;
        for (uint8_t i = 0; i < MAX_RETRIES && !ok; i++) {
            _flush_rx();
            ok = _send_cmd(p, sizeof(p));
        }
        if (!ok) {
            Serial.println("[LD2420] ERROR: cannot enter config mode");
            return false;
        }
        Serial.println("[LD2420] Config mode entered");
    }

    /* ---- 2. Установить Energy mode ---- */
    /*
        CMD_WRITE_SYS_PARAM = 0x0012
        Payload: 12 00 | param_id(2) | param_value(4)
        param_id для mode = 0x0000
        Energy mode value  = 0x0004
    */
    {
        uint8_t p[] = {
            0x12, 0x00,              // команда SET_SYS_PARAM
            0x00, 0x00,              // param: system mode
            0x04, 0x00, 0x00, 0x00  // value: Energy mode
        };
        bool ok = false;
        for (uint8_t i = 0; i < MAX_RETRIES && !ok; i++) ok = _send_cmd(p, sizeof(p));
        if (!ok) Serial.println("[LD2420] WARN: set Energy mode failed");
        else     Serial.println("[LD2420] Energy mode set");
    }

    /* ---- 3. Установить max_gate ---- */
    /*
        CMD_WRITE_SYS_PARAM = 0x0012
        param: 0x0001 = max gate
    */
    {
        uint8_t g = _cfg.max_gate;
        if (g > 15) g = 15;
        uint8_t p[] = {
            0x12, 0x00,
            0x01, 0x00,              // param: max gate
            g,    0x00, 0x00, 0x00
        };
        bool ok = false;
        for (uint8_t i = 0; i < MAX_RETRIES && !ok; i++) ok = _send_cmd(p, sizeof(p));
        if (!ok) Serial.println("[LD2420] WARN: set max_gate failed");
        else {
            Serial.print("[LD2420] max_gate set to ");
            Serial.println(g);
        }
    }

    /* ---- 4. Установить таймаут ---- */
    /*
        param: 0x0004 = timeout
    */
    {
        uint16_t t = _cfg.radar_timeout_s;
        uint8_t p[] = {
            0x12, 0x00,
            0x04, 0x00,              // param: timeout
            (uint8_t)(t & 0xFF), (uint8_t)(t >> 8), 0x00, 0x00
        };
        bool ok = false;
        for (uint8_t i = 0; i < MAX_RETRIES && !ok; i++) ok = _send_cmd(p, sizeof(p));
        if (!ok) Serial.println("[LD2420] WARN: set timeout failed");
        else {
            Serial.print("[LD2420] timeout set to ");
            Serial.print(t);
            Serial.println(" s");
        }
    }

    /* ---- 5. Записать пороги для каждого ворота ---- */
    /*
        CMD_WRITE_ABD_PARAM = 0x0007
        Payload: 07 00 | gate_move_reg(2) | move_val(4) | gate_still_reg(2) | still_val(4)
        Записываем оба порога одной командой на каждое ворото.
    */
    Serial.println("[LD2420] Writing gate thresholds...");
    bool thresh_ok = true;

    for (uint8_t gate = 0; gate <= _cfg.max_gate; gate++) {
        uint16_t move_reg  = (uint16_t)(GATE_MOVE_REG_BASE  + gate);
        uint16_t still_reg = (uint16_t)(GATE_STILL_REG_BASE + gate);
        uint32_t mv = _cfg.move_threshold;
        uint32_t sv = _cfg.still_threshold;

        uint8_t p[] = {
            0x07, 0x00,
            (uint8_t)(move_reg  & 0xFF), (uint8_t)(move_reg  >> 8),
            (uint8_t)(mv        & 0xFF), (uint8_t)((mv >> 8)  & 0xFF),
            (uint8_t)((mv >> 16) & 0xFF), (uint8_t)((mv >> 24) & 0xFF),
            (uint8_t)(still_reg & 0xFF), (uint8_t)(still_reg >> 8),
            (uint8_t)(sv        & 0xFF), (uint8_t)((sv >> 8)  & 0xFF),
            (uint8_t)((sv >> 16) & 0xFF), (uint8_t)((sv >> 24) & 0xFF),
        };

        bool ok = false;
        for (uint8_t i = 0; i < MAX_RETRIES && !ok; i++) ok = _send_cmd(p, sizeof(p));
        if (!ok) {
            Serial.print("[LD2420] WARN: threshold gate ");
            Serial.print(gate);
            Serial.println(" failed");
            thresh_ok = false;
        }
    }

    if (thresh_ok) Serial.println("[LD2420] Thresholds written");

    /* ---- 6. Выйти из config mode и перезапустить ---- */
    {
        uint8_t p[] = {0xFE, 0x00};
        bool ok = false;
        for (uint8_t i = 0; i < MAX_RETRIES && !ok; i++) ok = _send_cmd(p, sizeof(p));
        if (!ok) Serial.println("[LD2420] WARN: disable config failed");
        else     Serial.println("[LD2420] Config mode exited");
    }

    /* Небольшая пауза — радар применяет настройки */
    delay(500);

    Serial.println("[LD2420] Configuration complete");
    return true;
}


/* ====================================================
   BEGIN
   ==================================================== */

bool Ld2420Radar::begin(HardwareSerial& serial, const RadarConfig& config) {
    _serial = &serial;
    _cfg    = config;

    if (_cfg.activity_avg_window_s < 1)            _cfg.activity_avg_window_s = 1;
    if (_cfg.activity_avg_window_s > ACT_AVG_MAX)  _cfg.activity_avg_window_s = ACT_AVG_MAX;
    if (_cfg.activity_trend_window_min < 1)         _cfg.activity_trend_window_min = 1;
    if (_cfg.activity_trend_window_min > TREND_MAX) _cfg.activity_trend_window_min = TREND_MAX;
    if (_cfg.total_energy_max == 0)                 _cfg.total_energy_max = 1;

    serial.begin(_cfg.baud_rate, SERIAL_8N1,
                 (int)_cfg.uart_rx_pin,
                 (int)_cfg.uart_tx_pin);

    /* Даём радару время проснуться */
    delay(1000);
    _flush_rx();

    _configured = _configure();
    return _configured;
}


/* ====================================================
   UPDATE
   ==================================================== */

void Ld2420Radar::update() {
    if (!_serial) return;

    while (_serial->available()) {
        _feed_byte((uint8_t)_serial->read());
    }

    uint32_t now_ms = millis();

    _online = (_last_frame_ms > 0) &&
              (now_ms - _last_frame_ms < _cfg.stale_after_ms);

    _presence = (_last_on_ms > 0) &&
                (now_ms - _last_on_ms < _cfg.presence_hold_ms);

    _tick_activity_avg(now_ms);
    _tick_trend(now_ms);
}


/* ====================================================
   ПАРСЕР ENERGY-ФРЕЙМОВ
   ==================================================== */

void Ld2420Radar::_feed_byte(uint8_t b) {
    if (!_synced) {
        /* Сдвигаем окно поиска заголовка */
        _rx_buf[0] = _rx_buf[1];
        _rx_buf[1] = _rx_buf[2];
        _rx_buf[2] = _rx_buf[3];
        _rx_buf[3] = b;

        if (_rx_buf[0] == DATA_HDR[0] &&
            _rx_buf[1] == DATA_HDR[1] &&
            _rx_buf[2] == DATA_HDR[2] &&
            _rx_buf[3] == DATA_HDR[3]) {
            _synced = true;
            _rx_idx = 4;
        }
        return;
    }

    _rx_buf[_rx_idx++] = b;

    if (_rx_idx >= FRAME_SIZE) {
        if (_validate_data_footer()) {
            _process_frame();
        } else if (_cfg.enable_debug_frames) {
            Serial.print("[LD2420] bad footer at: ");
            Serial.printf("%02X %02X %02X %02X\n",
                _rx_buf[FRAME_FOOTER_OFF],   _rx_buf[FRAME_FOOTER_OFF+1],
                _rx_buf[FRAME_FOOTER_OFF+2], _rx_buf[FRAME_FOOTER_OFF+3]);
        }
        _synced = false;
        _rx_idx = 0;
    }
}

bool Ld2420Radar::_validate_data_footer() const {
    return _rx_buf[FRAME_FOOTER_OFF + 0] == DATA_FTR[0] &&
           _rx_buf[FRAME_FOOTER_OFF + 1] == DATA_FTR[1] &&
           _rx_buf[FRAME_FOOTER_OFF + 2] == DATA_FTR[2] &&
           _rx_buf[FRAME_FOOTER_OFF + 3] == DATA_FTR[3];
}

void Ld2420Radar::_process_frame() {
    uint32_t now_ms = millis();
    _last_frame_ms  = now_ms;

    /* Presence — поле из фрейма только для debug, решение принимаем сами */
    uint8_t pres_raw = _rx_buf[FRAME_PRESENCE];

    /* Distance */
    uint16_t dist_cm = (uint16_t)_rx_buf[FRAME_DIST] |
                       (uint16_t)_rx_buf[FRAME_DIST + 1] << 8;
    float raw_m = (float)dist_cm * 0.01f;
    /* дистанция обновляется ниже после вычисления has_target */

    /* Gate energies + activity_score_current */
    /* Каждое ворото — uint16_t LE, 2 байта */
    /* Суммируем только от min_gate до max_gate — исключаем шум ближней зоны */
    uint32_t total_energy = 0;
    uint8_t gate_min = _cfg.min_gate;
    uint8_t gate_max = (_cfg.max_gate < LD2420_TOTAL_GATES - 1)
                     ? _cfg.max_gate : LD2420_TOTAL_GATES - 1;
    for (uint8_t g = 0; g < LD2420_TOTAL_GATES; g++) {
        uint8_t lo = _rx_buf[FRAME_GATES + g * 2];
        uint8_t hi = _rx_buf[FRAME_GATES + g * 2 + 1];
        _gate_energy[g] = (uint16_t)lo | ((uint16_t)hi << 8);
        if (g >= gate_min && g <= gate_max) {
            total_energy += _gate_energy[g];
        }
    }

    /*
        Вычисляем presence самостоятельно по энергиям ворот.
        Ворото активно если его энергия > presence_thresholds[g].
        Пороги g0 и g1 = 0 — они всегда исключены.
    */
    uint8_t active_gates = 0;
    for (uint8_t g = _cfg.min_gate; g <= gate_max; g++) {
        if (_cfg.presence_thresholds[g] > 0 &&
            _gate_energy[g] > _cfg.presence_thresholds[g]) {
            active_gates++;
        }
    }
    bool has_target = (active_gates >= _cfg.presence_min_active_gates);
    if (has_target) _last_on_ms = now_ms;

    /* Дистанция: обновляем только если has_target и dist >= min_gate */
    {
        float min_dist_m = (float)_cfg.min_gate * 0.70f;
        bool dist_valid  = has_target && dist_cm > 0 && raw_m >= min_dist_m;
        if (dist_valid) {
            if (_dist_ema < 0.0f) _dist_ema = raw_m;
            else _dist_ema = _cfg.distance_ema_alpha * raw_m
                           + (1.0f - _cfg.distance_ema_alpha) * _dist_ema;
            _distance_m = _dist_ema;
        } else if (!has_target) {
            _dist_ema   = -1.0f;
            _distance_m = 0.0f;
        }
        /* has_target=true но dist невалидна — держим последнее значение */
    }

    float score_f = (float)total_energy / (float)_cfg.total_energy_max * 10.0f;
    if (score_f > 10.0f) score_f = 10.0f;
    _activity_score_current = (uint8_t)(score_f + 0.5f);

    if (_cfg.enable_debug_frames) {
        Serial.printf("[LD2420] pres_raw=%u active_gates=%u dist_cm=%u energy=%lu score=%u\n",
            pres_raw, active_gates, dist_cm, (unsigned long)total_energy, _activity_score_current);
    }
}


/* ====================================================
   ТИКИ НАКОПЛЕНИЯ
   ==================================================== */

void Ld2420Radar::_tick_activity_avg(uint32_t now_ms) {
    if (_act_avg_last_ms == 0) { _act_avg_last_ms = now_ms; return; }
    if (now_ms - _act_avg_last_ms < 1000) return;
    _act_avg_last_ms = now_ms;

    uint8_t win = _cfg.activity_avg_window_s;
    _act_avg_buf[_act_avg_idx] = _activity_score_current;
    _act_avg_idx = (_act_avg_idx + 1) % win;
    if (_act_avg_count < win) _act_avg_count++;

    uint32_t sum = 0;
    for (uint8_t i = 0; i < _act_avg_count; i++) sum += _act_avg_buf[i];
    _activity_score = (uint8_t)(sum / _act_avg_count);
}

void Ld2420Radar::_tick_trend(uint32_t now_ms) {
    if (_trend_last_ms == 0) { _trend_last_ms = now_ms; return; }
    if (now_ms - _trend_last_ms < 60000u) return;
    _trend_last_ms = now_ms;

    uint8_t win = _cfg.activity_trend_window_min;
    _trend_buf[_trend_idx] = _activity_score;
    _trend_idx = (_trend_idx + 1) % win;
    if (_trend_count < win) _trend_count++;

    _compute_trend();
}

void Ld2420Radar::_compute_trend() {
    uint8_t n = _trend_count;
    if (n < 2) { _trend_slope = 0; _trend_std_dev = 0.0f; return; }

    uint8_t win = _cfg.activity_trend_window_min;
    float sx = 0, sy = 0, sxx = 0, sxy = 0, mean_y = 0;

    for (uint8_t i = 0; i < n; i++) {
        uint8_t idx = (uint8_t)((_trend_idx - n + i + win) % win);
        float x = (float)i, y = (float)_trend_buf[idx];
        sx += x; sy += y; sxx += x*x; sxy += x*y; mean_y += y;
    }

    float fn = (float)n, denom = fn * sxx - sx * sx;
    if (fabsf(denom) < 1e-6f) {
        _trend_slope = 0;
    } else {
        float slope = (fn * sxy - sx * sy) / denom;
        _trend_slope = (slope > 0.30f) ? 1 : (slope < -0.30f) ? -1 : 0;
    }

    mean_y /= fn;
    float var = 0.0f;
    for (uint8_t i = 0; i < n; i++) {
        uint8_t idx = (uint8_t)((_trend_idx - n + i + win) % win);
        float d = (float)_trend_buf[idx] - mean_y;
        var += d * d;
    }
    _trend_std_dev = sqrtf(var / fn);
}

const char* Ld2420Radar::get_activity_dynamics() const {
    if (_trend_count < 2)                          return "constant";
    if (_trend_std_dev > 2.5f && _trend_slope == 0) return "variable";
    if (_trend_slope >  0)                          return "increasing";
    if (_trend_slope <  0)                          return "decreasing";
    return "constant";
}


/* ====================================================
   JSON
   ==================================================== */

const char* Ld2420Radar::get_state_json() {
    snprintf(_json_buf, sizeof(_json_buf),
        "{"
        "\"online\":%s,"
        "\"presence\":%s,"
        "\"activity_score\":%u,"
        "\"activity_score_current\":%u,"
        "\"activity_score_dynamics\":\"%s\","
        "\"distance_m\":%.2f"
        "}",
        _online   ? "true" : "false",
        _presence ? "true" : "false",
        (unsigned)_activity_score,
        (unsigned)_activity_score_current,
        get_activity_dynamics(),
        _distance_m
    );
    return _json_buf;
}