/*
 * encoder_odom.c — quadrature encoder reading and differential-drive
 * odometry for Issue #632.
 *
 * TIM2 (32-bit) = left encoder,  TIM3 (16-bit) = right encoder.
 * Both configured in encoder mode 3 (count on both A and B edges × 4).
 *
 * RPM formula:
 *   rpm = delta_ticks * 60 / (ticks_per_rev * dt_s)
 *
 * Odometry (Euler-forward differential drive):
 *   d_l  = delta_left  * meters_per_tick
 *   d_r  = delta_right * meters_per_tick
 *   d_c  = (d_l + d_r) / 2
 *   dθ   = (d_r - d_l) / wheel_base_m
 *   x   += d_c * cos(θ)
 *   y   += d_c * sin(θ)
 *   θ   += dθ
 */

#include "encoder_odom.h"
#include "jlink.h"
#include <math.h>
#include <string.h>

/* ---- Platform abstraction (stubbed in TEST_HOST builds) ---- */

#ifndef TEST_HOST

#include "stm32f7xx_hal.h"
#include "config.h"

/* Read 32-bit left encoder counter (TIM2) */
static inline uint32_t enc_read_left(void)
{
    return (uint32_t)TIM2->CNT;
}

/* Read 16-bit right encoder counter (TIM3) */
static inline uint16_t enc_read_right(void)
{
    return (uint16_t)TIM3->CNT;
}

/* Configure TIM2 in encoder mode (left wheel) */
static void enc_tim2_init(void)
{
    /* Enable peripheral clock */
    __HAL_RCC_TIM2_CLK_ENABLE();
    __HAL_RCC_GPIOA_CLK_ENABLE();
    __HAL_RCC_GPIOB_CLK_ENABLE();

    /* PA15 = TIM2_CH1 (AF1), PB3 = TIM2_CH2 (AF1) — floating input */
    GPIO_InitTypeDef g = {0};
    g.Mode  = GPIO_MODE_AF_PP;
    g.Pull  = GPIO_PULLUP;
    g.Speed = GPIO_SPEED_FREQ_LOW;
    g.Alternate = GPIO_AF1_TIM2;

    g.Pin = GPIO_PIN_15;
    HAL_GPIO_Init(GPIOA, &g);

    g.Pin = GPIO_PIN_3;
    HAL_GPIO_Init(GPIOB, &g);

    /* Encoder mode 3: count on both TI1 and TI2 edges */
    TIM2->CR1   = 0;
    TIM2->SMCR  = TIM_SMCR_SMS_0 | TIM_SMCR_SMS_1;  /* SMS = 0x3 */
    TIM2->CCMR1 = (TIM_CCMR1_CC1S_0 | TIM_CCMR1_CC2S_0);  /* IC1→TI1, IC2→TI2 */
    TIM2->CCER  = 0;   /* no polarity inversion (change CC1P/CC2P to reverse) */
    TIM2->ARR   = 0xFFFFFFFFUL;
    TIM2->CNT   = 0;
    TIM2->CR1  |= TIM_CR1_CEN;
}

/* Configure TIM3 in encoder mode (right wheel) */
static void enc_tim3_init(void)
{
    __HAL_RCC_TIM3_CLK_ENABLE();
    __HAL_RCC_GPIOC_CLK_ENABLE();

    /* PC6 = TIM3_CH1 (AF2), PC7 = TIM3_CH2 (AF2) */
    GPIO_InitTypeDef g = {0};
    g.Mode  = GPIO_MODE_AF_PP;
    g.Pull  = GPIO_PULLUP;
    g.Speed = GPIO_SPEED_FREQ_LOW;
    g.Alternate = GPIO_AF2_TIM3;

    g.Pin = GPIO_PIN_6 | GPIO_PIN_7;
    HAL_GPIO_Init(GPIOC, &g);

    TIM3->CR1   = 0;
    TIM3->SMCR  = TIM_SMCR_SMS_0 | TIM_SMCR_SMS_1;
    TIM3->CCMR1 = (TIM_CCMR1_CC1S_0 | TIM_CCMR1_CC2S_0);
    TIM3->CCER  = 0;
    TIM3->ARR   = 0xFFFF;
    TIM3->CNT   = 0;
    TIM3->CR1  |= TIM_CR1_CEN;
}

/* Flash helpers */
static bool flash_write_words(uint32_t addr, const void *data, uint32_t len)
{
    const uint32_t *src = (const uint32_t *)data;
    uint32_t words = (len + 3u) / 4u;
    for (uint32_t i = 0; i < words; i++) {
        if (HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD,
                              addr + i * 4u, src[i]) != HAL_OK) {
            return false;
        }
    }
    return true;
}

static bool flash_erase_sector7(void)
{
    FLASH_EraseInitTypeDef erase = {
        .TypeErase    = FLASH_TYPEERASE_SECTORS,
        .Sector       = FLASH_SECTOR_7,
        .NbSectors    = 1,
        .VoltageRange = FLASH_VOLTAGE_RANGE_3,
    };
    uint32_t err = 0;
    return (HAL_FLASHEx_Erase(&erase, &err) == HAL_OK) &&
           (err == 0xFFFFFFFFUL);
}

#else  /* TEST_HOST stubs */

/* Test-controlled counter values */
static uint32_t g_enc_left_cnt  = 0;
static uint16_t g_enc_right_cnt = 0;

static inline uint32_t enc_read_left(void)  { return g_enc_left_cnt;  }
static inline uint16_t enc_read_right(void) { return g_enc_right_cnt; }

static void enc_tim2_init(void) {}
static void enc_tim3_init(void) {}

static bool g_flash_erase_ok  = true;
static bool g_flash_write_ok  = true;
static enc_flash_config_t g_flash_store;
static bool g_flash_has_data  = false;

static bool flash_erase_sector7(void) { return g_flash_erase_ok; }
static bool flash_write_words(uint32_t addr, const void *data, uint32_t len)
{
    (void)addr;
    if (!g_flash_write_ok) return false;
    memcpy(&g_flash_store, data, len < sizeof(g_flash_store) ? len : sizeof(g_flash_store));
    g_flash_has_data = true;
    return true;
}

#endif /* TEST_HOST */

/* ---- Config defaults ---- */
static void cfg_set_defaults(enc_config_t *cfg)
{
    cfg->ticks_per_rev = ENC_TICKS_PER_REV_DEFAULT;
    cfg->wheel_diam_mm = ENC_WHEEL_DIAM_MM_DEFAULT;
    cfg->wheel_base_mm = ENC_WHEEL_BASE_MM_DEFAULT;
}

/* ---- Pre-compute derived constants from config ---- */
static void enc_update_precomputed(encoder_odom_t *eo)
{
    eo->meters_per_tick = (3.14159265358979f * (float)eo->cfg.wheel_diam_mm * 0.001f)
                          / (float)eo->cfg.ticks_per_rev;
    eo->wheel_base_m    = (float)eo->cfg.wheel_base_mm * 0.001f;
}

/* ---- encoder_odom_load_config() ---- */
bool encoder_odom_load_config(enc_config_t *cfg)
{
#ifndef TEST_HOST
    const enc_flash_config_t *p = (const enc_flash_config_t *)ENC_FLASH_ADDR;
    if (p->magic == ENC_FLASH_MAGIC &&
        p->ticks_per_rev > 0u &&
        p->wheel_diam_mm > 0u &&
        p->wheel_base_mm > 0u)
    {
        cfg->ticks_per_rev = p->ticks_per_rev;
        cfg->wheel_diam_mm = p->wheel_diam_mm;
        cfg->wheel_base_mm = p->wheel_base_mm;
        return true;
    }
#else
    if (g_flash_has_data && g_flash_store.magic == ENC_FLASH_MAGIC &&
        g_flash_store.ticks_per_rev > 0u &&
        g_flash_store.wheel_diam_mm > 0u &&
        g_flash_store.wheel_base_mm > 0u)
    {
        cfg->ticks_per_rev = g_flash_store.ticks_per_rev;
        cfg->wheel_diam_mm = g_flash_store.wheel_diam_mm;
        cfg->wheel_base_mm = g_flash_store.wheel_base_mm;
        return true;
    }
#endif
    cfg_set_defaults(cfg);
    return false;
}

/* ---- encoder_odom_save_config() ---- */
bool encoder_odom_save_config(const enc_config_t *cfg)
{
    enc_flash_config_t rec;
    memset(&rec, 0xFF, sizeof(rec));
    rec.magic         = ENC_FLASH_MAGIC;
    rec.ticks_per_rev = cfg->ticks_per_rev;
    rec.wheel_diam_mm = cfg->wheel_diam_mm;
    rec.wheel_base_mm = cfg->wheel_base_mm;

#ifndef TEST_HOST
    HAL_FLASH_Unlock();
    bool ok = flash_erase_sector7() &&
              flash_write_words(ENC_FLASH_ADDR, &rec, sizeof(rec));
    HAL_FLASH_Lock();
    return ok;
#else
    return flash_write_words(ENC_FLASH_ADDR, &rec, sizeof(rec));
#endif
}

/* ---- encoder_odom_init() ---- */
void encoder_odom_init(encoder_odom_t *eo)
{
    memset(eo, 0, sizeof(*eo));

    /* Load or default config */
    encoder_odom_load_config(&eo->cfg);
    enc_update_precomputed(eo);

    /* Initialize timers */
    enc_tim2_init();
    enc_tim3_init();

    /* Snapshot initial counter values (treat as zero reference) */
    eo->cnt_left  = enc_read_left();
    eo->cnt_right = enc_read_right();

    /* Initialize TLM timer so first call fires immediately */
    eo->last_tlm_ms = (uint32_t)(-(uint32_t)(1000u / (ENC_TLM_HZ > 0u ? ENC_TLM_HZ : 1u)));
}

/* ---- encoder_odom_reset_pose() ---- */
void encoder_odom_reset_pose(encoder_odom_t *eo)
{
    eo->x_mm      = 0.0f;
    eo->y_mm      = 0.0f;
    eo->theta_rad = 0.0f;
}

/* ---- encoder_odom_tick() ---- */
void encoder_odom_tick(encoder_odom_t *eo, uint32_t now_ms)
{
    /* Compute dt */
    uint32_t elapsed_ms = now_ms - eo->last_tick_ms;
    if (elapsed_ms == 0u) return;  /* avoid divide-by-zero at high call rates */
    if (elapsed_ms > 500u) elapsed_ms = 500u;  /* clamp: stale data guard */
    float dt_s = (float)elapsed_ms * 0.001f;
    eo->last_tick_ms = now_ms;

    /* Read counters */
    uint32_t new_left  = enc_read_left();
    uint16_t new_right = enc_read_right();

    /* Signed delta — handles 32/16-bit wrap correctly */
    int32_t delta_left  = (int32_t)(new_left  - eo->cnt_left);
    int16_t delta_right = (int16_t)(new_right - eo->cnt_right);

    eo->cnt_left  = new_left;
    eo->cnt_right = (uint16_t)new_right;

    /* RPM */
    float rpm_scale = 60.0f / ((float)eo->cfg.ticks_per_rev * dt_s);
    float rpm_l = (float)delta_left  * rpm_scale;
    float rpm_r = (float)delta_right * rpm_scale;

    /* Clamp to int16 range */
    if (rpm_l >  32767.0f) rpm_l =  32767.0f;
    if (rpm_l < -32768.0f) rpm_l = -32768.0f;
    if (rpm_r >  32767.0f) rpm_r =  32767.0f;
    if (rpm_r < -32768.0f) rpm_r = -32768.0f;
    eo->rpm_left  = (int16_t)rpm_l;
    eo->rpm_right = (int16_t)rpm_r;

    /* Wheel displacements (metres) */
    float d_left  = (float)delta_left  * eo->meters_per_tick;
    float d_right = (float)delta_right * eo->meters_per_tick;

    /* Linear / angular increments */
    float d_center = (d_left + d_right) * 0.5f;
    float d_theta  = (d_right - d_left) / eo->wheel_base_m;

    /* Linear speed (mm/s), clamped to int16 */
    float speed_raw = (d_center / dt_s) * 1000.0f;  /* m/s → mm/s */
    if (speed_raw >  32767.0f) speed_raw =  32767.0f;
    if (speed_raw < -32768.0f) speed_raw = -32768.0f;
    eo->speed_mmps = (int16_t)speed_raw;

    /* Integrate pose (Euler-forward) */
    float cos_h = cosf(eo->theta_rad);
    float sin_h = sinf(eo->theta_rad);
    eo->x_mm      += d_center * cos_h * 1000.0f;
    eo->y_mm      += d_center * sin_h * 1000.0f;
    eo->theta_rad += d_theta;

    /* Wrap theta to (-π, π] */
    while (eo->theta_rad >  3.14159265358979f) eo->theta_rad -= 2.0f * 3.14159265358979f;
    while (eo->theta_rad < -3.14159265358979f) eo->theta_rad += 2.0f * 3.14159265358979f;
}

/* ---- encoder_odom_send_tlm() ---- */
void encoder_odom_send_tlm(const encoder_odom_t *eo, uint32_t now_ms)
{
    if (ENC_TLM_HZ == 0u) return;

    uint32_t interval_ms = 1000u / ENC_TLM_HZ;
    if ((now_ms - eo->last_tlm_ms) < interval_ms) return;
    /* Cast away const for timestamp — only mutable field */
    ((encoder_odom_t *)eo)->last_tlm_ms = now_ms;

    jlink_tlm_odom_t tlm;
    tlm.rpm_left   = eo->rpm_left;
    tlm.rpm_right  = eo->rpm_right;

    /* x/y: float mm → int32_t mm (saturate at ±2147 km — more than adequate) */
    float xf = eo->x_mm;
    float yf = eo->y_mm;
    if (xf >  2147483647.0f) xf =  2147483647.0f;
    if (xf < -2147483648.0f) xf = -2147483648.0f;
    if (yf >  2147483647.0f) yf =  2147483647.0f;
    if (yf < -2147483648.0f) yf = -2147483648.0f;
    tlm.x_mm = (int32_t)xf;
    tlm.y_mm = (int32_t)yf;

    /* theta: radians → centidegrees (0.01° steps) */
    float theta_cdeg = eo->theta_rad * (18000.0f / 3.14159265358979f);
    if (theta_cdeg >  32767.0f) theta_cdeg =  32767.0f;
    if (theta_cdeg < -32768.0f) theta_cdeg = -32768.0f;
    tlm.theta_cdeg = (int16_t)theta_cdeg;

    tlm.speed_mmps = eo->speed_mmps;

    jlink_send_odom_tlm(&tlm);
}