#include "encoder.h"

void Encoder_Init(Encoder *enc, TIM_HandleTypeDef *htim)
{
    enc->htim_enc = htim;
    enc->count = 0;
    enc->last_count = 0;
    enc->speed = 0.0f;
    enc->speed_filtered = 0.0f;

    /* Start encoder in quadrature mode */
    HAL_TIM_Encoder_Start(htim, TIM_CHANNEL_ALL);
}

void Encoder_Update(Encoder *enc, float dt)
{
    int32_t raw = (int16_t)__HAL_TIM_GET_COUNTER(enc->htim_enc);

    /* Accumulate (handle overflow if needed) */
    enc->count += raw - enc->last_count;

    if (dt > 0.0f) {
        enc->speed = (float)(raw - enc->last_count) / dt;
        /* Simple low-pass filter */
        enc->speed_filtered = 0.8f * enc->speed_filtered + 0.2f * enc->speed;
    }

    enc->last_count = raw;
}

int32_t Encoder_GetCount(Encoder *enc)
{
    return enc->count;
}

float Encoder_GetSpeed(Encoder *enc)
{
    return enc->speed_filtered;
}

void Encoder_Reset(Encoder *enc)
{
    enc->count = 0;
    enc->last_count = 0;
    enc->speed = 0.0f;
    enc->speed_filtered = 0.0f;
}