// =============================================================================
//  rudder_actuator_electric.h -- reversible DC motor actuator (Sprint 4)
// =============================================================================
//
//  Drives a reversible DC motor with mechanical end-stops (Lewmar, Simpson
//  Lawrence) via H-bridge (DRV8833 or equivalent):
//    DO1 (GPIO)  -- IN1 (port direction)
//    DO2 (GPIO)  -- IN2 (starboard direction)
//    DO3 (GPIO)  -- master power relay (ENA)
//    PWM channel -- motor speed (shared LEDC channel 1)
//
//  Architecture identical to HydraulicRudderActuator; only the PWM mapping
//  differs (electric motors typically need a minimum useful duty to overcome
//  static friction — see AR_ACTUATOR_DEADBAND_PCT in firmware_config.h).
// =============================================================================

#pragma once

#include <algorithm>
#include <cmath>
#include <cstdint>

#include <Preferences.h>
#include <driver/ledc.h>

#include "hal/di_do.h"
#include "hal/pinout.h"
#include "hal/rudder_actuator_iface.h"
#include "hal/rudder_sensor.h"
#include "system/ar_log.h"

#ifndef AR_ACTUATOR_DEADBAND_PCT
#  define AR_ACTUATOR_DEADBAND_PCT 5.0f
#endif

namespace arautopilot::hal {

class ElectricDcRudderActuator final : public IRudderActuator {
public:
    static constexpr const char* NVS_NS  = "ar_act";
    static constexpr const char* NVS_KEY = "angle_limit";
    static constexpr float DEFAULT_LIMIT_DEG = 35.0f;
    static constexpr float MIN_LIMIT_DEG     = 15.0f;
    static constexpr float MAX_LIMIT_DEG     = 45.0f;

    void init() override {
        _prefs.begin(NVS_NS, false);
        _limit_deg = _prefs.getFloat(NVS_KEY, DEFAULT_LIMIT_DEG);
        _limit_deg = std::clamp(_limit_deg, MIN_LIMIT_DEG, MAX_LIMIT_DEG);
        _prefs.end();

        pinMode(PIN_DO1, OUTPUT); digitalWrite(PIN_DO1, LOW);
        pinMode(PIN_DO2, OUTPUT); digitalWrite(PIN_DO2, LOW);
        pinMode(PIN_DO3, OUTPUT); digitalWrite(PIN_DO3, LOW);

        ledc_timer_config_t timer{};
        timer.speed_mode      = LEDC_LOW_SPEED_MODE;
        timer.duty_resolution = LEDC_TIMER_8_BIT;
        timer.timer_num       = LEDC_TIMER_1;
        timer.freq_hz         = 20000;  // 20 kHz — inaudible for DC motor
        timer.clk_cfg         = LEDC_AUTO_CLK;
        ledc_timer_config(&timer);

        ledc_channel_config_t ch{};
        ch.speed_mode = LEDC_LOW_SPEED_MODE;
        ch.channel    = LEDC_CHANNEL_1;
        ch.timer_sel  = LEDC_TIMER_1;
        ch.gpio_num   = PIN_ACTUATOR_PWM;
        ch.duty       = 0;
        ledc_channel_config(&ch);

        AR_LOGI("ElectricActuator", "init OK, limit=%.1f deg", _limit_deg);
    }

    void power_on()  override { digitalWrite(PIN_DO3, HIGH); _powered = true; }
    void power_off() override { _stop_outputs(); digitalWrite(PIN_DO3, LOW); _powered = false; }

    bool command(float angle_deg) override {
        if (!_powered || _at_standby()) return false;

        const float clamped = std::clamp(angle_deg, -_limit_deg, _limit_deg);
        if (clamped > 0.0f && _limit_stbd()) { _stop_outputs(); return false; }
        if (clamped < 0.0f && _limit_port())  { _stop_outputs(); return false; }

        const float raw_pct  = std::fabsf(clamped) / _limit_deg * 100.0f;
        // Apply deadband: below deadband -> 0, otherwise scale to [deadband, 100]
        const float db       = AR_ACTUATOR_DEADBAND_PCT;
        const float duty_pct = (raw_pct < 0.5f) ? 0.0f
                             : db + raw_pct * (100.0f - db) / 100.0f;

        _set_pwm(static_cast<uint8_t>(std::min(duty_pct, 100.0f) * 255.0f / 100.0f));

        if (clamped > 0.01f) {
            digitalWrite(PIN_DO1, LOW); digitalWrite(PIN_DO2, HIGH);
        } else if (clamped < -0.01f) {
            digitalWrite(PIN_DO2, LOW); digitalWrite(PIN_DO1, HIGH);
        } else {
            _stop_outputs();
        }
        return true;
    }

    float actual_angle_deg() const override { return rudder_sensor_get_angle_deg(); }
    bool  is_powered()       const override { return _powered; }
    bool  at_limit()         const override { return _limit_port() || _limit_stbd(); }
    float angle_limit_deg()  const override { return _limit_deg; }
    ActuatorType type()      const override { return ActuatorType::ELECTRIC_DC; }

    void set_angle_limit_deg(float limit_deg) override {
        _limit_deg = std::clamp(limit_deg, MIN_LIMIT_DEG, MAX_LIMIT_DEG);
        _prefs.begin(NVS_NS, false);
        _prefs.putFloat(NVS_KEY, _limit_deg);
        _prefs.end();
        AR_LOGI("ElectricActuator", "angle_limit updated -> %.1f deg", _limit_deg);
    }

private:
    Preferences _prefs;
    float _limit_deg{DEFAULT_LIMIT_DEG};
    bool  _powered{false};

    static bool _limit_port()  { return digitalRead(PIN_DI2) == HIGH; }
    static bool _limit_stbd()  { return digitalRead(PIN_DI3) == HIGH; }
    static bool _at_standby();

    void _stop_outputs() {
        _set_pwm(0);
        digitalWrite(PIN_DO1, LOW);
        digitalWrite(PIN_DO2, LOW);
    }

    static void _set_pwm(uint8_t duty) {
        ledc_set_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_1, duty);
        ledc_update_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_1);
    }
};

}  // namespace arautopilot::hal
