#include "servo.h"
#include "config.h"
#include "stm32f7xx_hal.h"
#include <string.h>

/* ================================================================
 * Servo PWM Protocol
 * ================================================================
 * TIM4 at 50 Hz (20 ms period)
 * APB1 clock: 54 MHz
 * Prescaler: 53 (54 MHz / 54 = 1 MHz)
 * ARR: 19999 (1 MHz / 20000 = 50 Hz)
 * CCR: 500-2500 (0.5-2.5 ms out of 20 ms)
 *
 * Servo pulse mapping:
 *   500 µs  → 0°   (full left/down)
 *   1500 µs → 90°  (center)
 *   2500 µs → 180° (full right/up)
 */

#define SERVO_PWM_FREQ      50u            /* 50 Hz */
#define SERVO_PERIOD_MS     20u            /* 20 ms = 1/50 Hz */
#define SERVO_CLOCK_HZ      1000000u       /* 1 MHz timer clock */
#define SERVO_PRESCALER     53u            /* APB1 54 MHz / 54 = 1 MHz */
#define SERVO_ARR           19999u         /* 1 MHz / 20000 = 50 Hz */

typedef struct {
    uint16_t current_angle_deg[SERVO_COUNT];
    uint16_t target_angle_deg[SERVO_COUNT];
    uint16_t pulse_us[SERVO_COUNT];

    /* Sweep state */
    uint32_t sweep_start_ms[SERVO_COUNT];
    uint32_t sweep_duration_ms[SERVO_COUNT];
    uint16_t sweep_start_deg[SERVO_COUNT];
    uint16_t sweep_end_deg[SERVO_COUNT];
    bool     is_sweeping[SERVO_COUNT];
} ServoState;

static ServoState s_servo = {0};
static TIM_HandleTypeDef s_tim_handle = {0};

/* ================================================================
 * Helper functions
 * ================================================================
 */

static uint16_t angle_to_pulse_us(uint16_t degrees)
{
    /* Linear interpolation: 0° → 500µs, 180° → 2500µs */
    if (degrees > 180) degrees = 180;

    uint32_t pulse = SERVO_MIN_US + (uint32_t)degrees * (SERVO_MAX_US - SERVO_MIN_US) / 180;
    return (uint16_t)pulse;
}

static uint16_t pulse_us_to_angle(uint16_t pulse_us)
{
    /* Inverse mapping: 500µs → 0°, 2500µs → 180° */
    if (pulse_us < SERVO_MIN_US) pulse_us = SERVO_MIN_US;
    if (pulse_us > SERVO_MAX_US) pulse_us = SERVO_MAX_US;

    uint32_t angle = (uint32_t)(pulse_us - SERVO_MIN_US) * 180 / (SERVO_MAX_US - SERVO_MIN_US);
    return (uint16_t)angle;
}

static void update_pwm(ServoChannel channel)
{
    /* Convert pulse width (500-2500 µs) to CCR value */
    /* At 1 MHz timer clock: 1 µs = 1 count */
    uint32_t ccr_value = s_servo.pulse_us[channel];

    if (channel == SERVO_PAN) {
        __HAL_TIM_SET_COMPARE(&s_tim_handle, SERVO_PAN_CHANNEL, ccr_value);
    } else {
        __HAL_TIM_SET_COMPARE(&s_tim_handle, SERVO_TILT_CHANNEL, ccr_value);
    }
}

/* ================================================================
 * Public API
 * ================================================================
 */

void servo_init(void)
{
    /* Initialize state */
    memset(&s_servo, 0, sizeof(s_servo));

    /* Center both servos at 90° */
    for (int i = 0; i < SERVO_COUNT; i++) {
        s_servo.current_angle_deg[i] = 90;
        s_servo.target_angle_deg[i] = 90;
        s_servo.pulse_us[i] = SERVO_CENTER_US;
    }

    /* Configure GPIO PB6 (CH1) and PB7 (CH2) as TIM4 PWM */
    __HAL_RCC_GPIOB_CLK_ENABLE();

    GPIO_InitTypeDef gpio_init = {0};
    gpio_init.Mode      = GPIO_MODE_AF_PP;
    gpio_init.Pull      = GPIO_NOPULL;
    gpio_init.Speed     = GPIO_SPEED_FREQ_HIGH;
    gpio_init.Alternate = SERVO_AF;

    /* Configure PB6 (pan) */
    gpio_init.Pin = SERVO_PAN_PIN;
    HAL_GPIO_Init(SERVO_PAN_PORT, &gpio_init);

    /* Configure PB7 (tilt) */
    gpio_init.Pin = SERVO_TILT_PIN;
    HAL_GPIO_Init(SERVO_TILT_PORT, &gpio_init);

    /* Configure TIM4: 50 Hz PWM */
    __HAL_RCC_TIM4_CLK_ENABLE();

    s_tim_handle.Instance           = SERVO_TIM;
    s_tim_handle.Init.Prescaler     = SERVO_PRESCALER;
    s_tim_handle.Init.CounterMode   = TIM_COUNTERMODE_UP;
    s_tim_handle.Init.Period        = SERVO_ARR;
    s_tim_handle.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
    s_tim_handle.Init.RepetitionCounter = 0;

    HAL_TIM_PWM_Init(&s_tim_handle);

    /* Configure TIM4_CH1 (pan) for PWM */
    TIM_OC_InitTypeDef oc_init = {0};
    oc_init.OCMode     = TIM_OCMODE_PWM1;
    oc_init.Pulse      = SERVO_CENTER_US;
    oc_init.OCPolarity = TIM_OCPOLARITY_HIGH;
    oc_init.OCFastMode = TIM_OCFAST_DISABLE;

    HAL_TIM_PWM_ConfigChannel(&s_tim_handle, &oc_init, SERVO_PAN_CHANNEL);
    HAL_TIM_PWM_Start(&s_tim_handle, SERVO_PAN_CHANNEL);

    /* Configure TIM4_CH2 (tilt) for PWM */
    oc_init.Pulse = SERVO_CENTER_US;
    HAL_TIM_PWM_ConfigChannel(&s_tim_handle, &oc_init, SERVO_TILT_CHANNEL);
    HAL_TIM_PWM_Start(&s_tim_handle, SERVO_TILT_CHANNEL);
}

void servo_set_angle(ServoChannel channel, uint16_t degrees)
{
    if (channel >= SERVO_COUNT) return;
    if (degrees > 180) degrees = 180;

    s_servo.current_angle_deg[channel] = degrees;
    s_servo.target_angle_deg[channel] = degrees;
    s_servo.pulse_us[channel] = angle_to_pulse_us(degrees);

    /* Stop any sweep in progress */
    s_servo.is_sweeping[channel] = false;

    /* Update PWM immediately */
    update_pwm(channel);
}

uint16_t servo_get_angle(ServoChannel channel)
{
    if (channel >= SERVO_COUNT) return 0;
    return s_servo.current_angle_deg[channel];
}

void servo_set_pulse_us(ServoChannel channel, uint16_t pulse_us)
{
    if (channel >= SERVO_COUNT) return;
    if (pulse_us < SERVO_MIN_US) pulse_us = SERVO_MIN_US;
    if (pulse_us > SERVO_MAX_US) pulse_us = SERVO_MAX_US;

    s_servo.pulse_us[channel] = pulse_us;
    s_servo.current_angle_deg[channel] = pulse_us_to_angle(pulse_us);
    s_servo.target_angle_deg[channel] = s_servo.current_angle_deg[channel];

    /* Stop any sweep in progress */
    s_servo.is_sweeping[channel] = false;

    /* Update PWM immediately */
    update_pwm(channel);
}

void servo_sweep(ServoChannel channel, uint16_t start_deg, uint16_t end_deg, uint32_t duration_ms)
{
    if (channel >= SERVO_COUNT) return;
    if (duration_ms == 0) return;
    if (start_deg > 180) start_deg = 180;
    if (end_deg > 180) end_deg = 180;

    s_servo.sweep_start_deg[channel] = start_deg;
    s_servo.sweep_end_deg[channel] = end_deg;
    s_servo.sweep_duration_ms[channel] = duration_ms;
    s_servo.sweep_start_ms[channel] = 0;  /* Will be set on first tick */
    s_servo.is_sweeping[channel] = true;
}

void servo_tick(uint32_t now_ms)
{
    for (int ch = 0; ch < SERVO_COUNT; ch++) {
        if (!s_servo.is_sweeping[ch]) continue;

        /* Initialize start time on first call */
        if (s_servo.sweep_start_ms[ch] == 0) {
            s_servo.sweep_start_ms[ch] = now_ms;
        }

        uint32_t elapsed = now_ms - s_servo.sweep_start_ms[ch];
        uint32_t duration = s_servo.sweep_duration_ms[ch];

        if (elapsed >= duration) {
            /* Sweep complete */
            s_servo.is_sweeping[ch] = false;
            s_servo.current_angle_deg[ch] = s_servo.sweep_end_deg[ch];
            s_servo.pulse_us[ch] = angle_to_pulse_us(s_servo.sweep_end_deg[ch]);
        } else {
            /* Linear interpolation */
            int16_t start = (int16_t)s_servo.sweep_start_deg[ch];
            int16_t end = (int16_t)s_servo.sweep_end_deg[ch];
            int32_t delta = end - start;

            /* angle = start + (delta * elapsed / duration) */
            int32_t angle_i32 = start + (delta * (int32_t)elapsed / (int32_t)duration);
            s_servo.current_angle_deg[ch] = (uint16_t)angle_i32;
            s_servo.pulse_us[ch] = angle_to_pulse_us(s_servo.current_angle_deg[ch]);
        }

        /* Update PWM */
        update_pwm((ServoChannel)ch);
    }
}

bool servo_is_sweeping(void)
{
    for (int i = 0; i < SERVO_COUNT; i++) {
        if (s_servo.is_sweeping[i]) return true;
    }
    return false;
}

void servo_stop_sweep(ServoChannel channel)
{
    if (channel >= SERVO_COUNT) return;
    s_servo.is_sweeping[channel] = false;
}