#ifndef ServoController_h
#define ServoController_h

#include <peripherals/drivers/pca9685.h>
#include <eventbus.hpp>
#include <utils/math_utils.h>
#include <platform_shared/api.pb.h>
#include <platform_shared/message.pb.h>

#ifndef FACTORY_SERVO_PWM_FREQUENCY
#define FACTORY_SERVO_PWM_FREQUENCY 50
#endif

#ifndef FACTORY_SERVO_OSCILLATOR_FREQUENCY
#define FACTORY_SERVO_OSCILLATOR_FREQUENCY 27000000
#endif

enum class SERVO_CONTROL_STATE { DEACTIVATED, PWM, ANGLE };

using ServoSettings = api_ServoSettings;

inline ServoSettings ServoSettings_defaults() {
    ServoSettings settings = {};
    settings.servos_count = 12;
    const api_Servo defaults[12] = {
        {306, -1, 0, 2.0f, "Servo1"}, {306, 1, -45, 2.0f, "Servo2"},  {306, 1, 90, 2.0f, "Servo3"},
        {306, -1, 0, 2.0f, "Servo4"}, {306, -1, 45, 2.0f, "Servo5"},  {306, -1, -90, 2.0f, "Servo6"},
        {306, 1, 0, 2.0f, "Servo7"},  {306, 1, -45, 2.0f, "Servo8"},  {306, 1, 90, 2.0f, "Servo9"},
        {306, 1, 0, 2.0f, "Servo10"}, {306, -1, 45, 2.0f, "Servo11"}, {306, -1, -90, 2.0f, "Servo12"}};
    for (int i = 0; i < 12; i++) {
        settings.servos[i] = defaults[i];
    }
    return settings;
}

class ServoController {
  public:
    void begin() {
        _settings = EventBus::instance().peek<ServoSettings>();
        initializePCA();

        _settingsHandle =
            EventBus::instance().subscribe<ServoSettings>([this](const ServoSettings& s) { _settings = s; });
        servoPwmHandle_ = EventBus::instance().subscribe<socket_message_ServoPWMData>(
            [this](const socket_message_ServoPWMData& data) { setServoPWM(data.servo_id, data.servo_pwm); });
        servoStateHandle_ = EventBus::instance().subscribe<socket_message_ServoStateData>(
            [this](const socket_message_ServoStateData& data) { data.active ? activate() : deactivate(); });
    }

    void pcaWrite(int index, int value) {
        if (value < 0 || value > 4096) {
            ESP_LOGE("Peripherals", "Invalid PWM value %d for %d :: Valid range 0-4096", value, index);
            return;
        }
        _pca.setPWM(index, 0, value);
    }

    void activate() {
        if (is_active) return;
        control_state = SERVO_CONTROL_STATE::ANGLE;
        is_active = true;
        _pca.wakeup();
    }

    void deactivate() {
        if (!is_active) return;
        is_active = false;
        control_state = SERVO_CONTROL_STATE::DEACTIVATED;
        _pca.sleep();
    }

    void setServoPWM(int32_t servo_id, uint32_t pwm) {
        control_state = SERVO_CONTROL_STATE::PWM;
        if (servo_id < 0) {
            uint16_t pwms[12];
            std::fill_n(pwms, 12, static_cast<uint16_t>(pwm));
            _pca.setMultiplePWM(pwms, 12);
        } else {
            _pca.setPWM(servo_id, 0, pwm);
        }
        ESP_LOGI("SERVO_CONTROLLER", "Setting servo %d to %d", servo_id, pwm);
    }

    void updateActiveState() { is_active ? activate() : deactivate(); }

    void setMode(SERVO_CONTROL_STATE newMode) { control_state = newMode; }

    void setAngles(float new_angles[12]) {
        for (int i = 0; i < 12; i++) {
            target_angles[i] = new_angles[i];
        }
    }

    void calculatePWM() {
        uint16_t pwms[12];
        for (int i = 0; i < 12; i++) {
            angles[i] = lerp(angles[i], target_angles[i], 0.1);
            auto& servo = _settings.servos[i];
            float angle = servo.direction * angles[i] + servo.center_angle;
            uint16_t pwm = angle * servo.conversion + servo.center_pwm;
            pwms[i] = pwm = std::clamp<uint16_t>(pwm, 125, 600);
        }
        _pca.setMultiplePWM(pwms, 12);
    }

    void update() {
        if (control_state == SERVO_CONTROL_STATE::ANGLE) calculatePWM();
    }

  private:
    void initializePCA() {
        _pca.begin();
        _pca.setOscillatorFrequency(FACTORY_SERVO_OSCILLATOR_FREQUENCY);
        _pca.setPWMFreq(FACTORY_SERVO_PWM_FREQUENCY);
        _pca.sleep();
    }

    ServoSettings _settings {};
    PCA9685Driver _pca;

    SERVO_CONTROL_STATE control_state = SERVO_CONTROL_STATE::DEACTIVATED;

    bool is_active {false};
    float angles[12] = {0, 90, -145, 0, 90, -145, 0, 90, -145, 0, 90, -145};
    float target_angles[12] = {0, 90, -145, 0, 90, -145, 0, 90, -145, 0, 90, -145};

    SubscriptionHandle _settingsHandle;
    SubscriptionHandle servoPwmHandle_;
    SubscriptionHandle servoStateHandle_;
};

#endif