//
// Created by Johnathon Slightham on 2025-07-15.
//

#include "control/Servo1Actuator.h"
#include "AngleControlMessageBuilder.h"
#include "SensorMessageBuilder.h"
#include "constants/module.h"
#include "driver/ledc.h"
#include "flatbuffers_generated/SensorMessage_generated.h"
#include "util/number_utils.h"
#include "esp_log.h"
#include <cmath>

#define LOW_DUTY 200
#define HIGH_DUTY 1000
#define PWM_FREQ 50 // 50 Hz for servo

// Task period in milliseconds
#define TASK_PERIOD_MS 20
// Max degrees to move per task tick
#define MAX_DEG_PER_TICK (SERVO_MAX_SPEED_DEG_PER_SEC * (TASK_PERIOD_MS / 1000.0f))

Servo1Actuator::Servo1Actuator() {
    ledc_timer_config_t ledc_timer = {
        .speed_mode = LEDC_LOW_SPEED_MODE,
        .duty_resolution = LEDC_TIMER_13_BIT,
        .timer_num = LEDC_TIMER_0,
        .freq_hz = PWM_FREQ,
        .clk_cfg = LEDC_AUTO_CLK,
    };
    ESP_ERROR_CHECK(ledc_timer_config(&ledc_timer));

    ledc_channel_config_t ledc_channel = {
        .gpio_num = SERVO_GPIO,
        .speed_mode = LEDC_LOW_SPEED_MODE,
        .channel = LEDC_CHANNEL_0,
        .intr_type = LEDC_INTR_DISABLE,
        .timer_sel = LEDC_TIMER_0,
        .duty = (HIGH_DUTY + LOW_DUTY) / 2, // move motor to midpoint initially
        .hpoint = 0,
    };
    ESP_ERROR_CHECK(ledc_channel_config(&ledc_channel));

    xTaskCreate(servo_task, "servo_task", 4096, this, 1, &m_servo_task);
}

Servo1Actuator::~Servo1Actuator() {
    m_running = false;
    if (m_servo_task != nullptr) {
        vTaskDelete(m_servo_task);
    }
}

void Servo1Actuator::write_pwm(float angle) {
    const auto duty = util::mapRange<float>(angle, 0.0f, 180.0f, LOW_DUTY, HIGH_DUTY);
    ESP_ERROR_CHECK(ledc_set_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_0, static_cast<uint32_t>(duty)));
    ESP_ERROR_CHECK(ledc_update_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_0));
}

void Servo1Actuator::servo_task(void *args) {
    auto *that = static_cast<Servo1Actuator *>(args);

    while (that->m_running) {
        const float target = that->m_target.load();
        const float current = that->m_current;
        const float diff = target - current;

        if (std::abs(diff) > 0.001f) {
            float step = diff;
            if (std::abs(step) > MAX_DEG_PER_TICK) {
                step = (diff > 0.0f ? 1.0f : -1.0f) * MAX_DEG_PER_TICK;
            }
            that->m_current += step;
            // ESP_LOGI("Servo1", "current=%.2f target=%.2f", that->m_current, target);
            write_pwm(that->m_current);
        }

        vTaskDelay(pdMS_TO_TICKS(TASK_PERIOD_MS));
    }

    vTaskDelete(nullptr);
}

void Servo1Actuator::actuate(uint8_t *cmd) {
    const auto *angleControlCmd =
        Flatbuffers::AngleControlMessageBuilder::parse_angle_control_message(cmd);
    m_target = static_cast<float>(angleControlCmd->angle());
    // ESP_LOGI("Servo1", "new target: %d", angleControlCmd->angle());
}

std::vector<Flatbuffers::sensor_value> Servo1Actuator::get_sensor_data() {
    return {Flatbuffers::target_angle{static_cast<int16_t>(m_current)}};
}