// =============================================================================
//  rudder_actuator_hydraulic.h -- reversible hydraulic pump actuator (Sprint 4)
// =============================================================================
//
//  Drives a reversible hydraulic pump (Hynautic, Hypro, Octopus, Vetus,
//  Lecomble & Schmitt) via:
//    DO1 (GPIO)  -- port direction solenoid
//    DO2 (GPIO)  -- starboard direction solenoid
//    DO3 (GPIO)  -- master power relay
//    PWM channel -- pump speed proportional to |angle_error|
//
//  The software angle limit is loaded from NVS at init() and clamped to
//  [15, 45] degrees.  It can be updated live via set_angle_limit_deg()
//  which also persists the new value to NVS.
// =============================================================================

#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"

namespace arautopilot::hal {

class HydraulicRudderActuator final : public IRudderActuator {
public:
    // NVS namespace + key for the angle limit
    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 {
        // Load angle limit from NVS (falls back to firmware_config.h default)
        _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();

        // Configure direction + power GPIOs
        pinMode(PIN_DO1, OUTPUT); digitalWrite(PIN_DO1, LOW);  // port
        pinMode(PIN_DO2, OUTPUT); digitalWrite(PIN_DO2, LOW);  // starboard
        pinMode(PIN_DO3, OUTPUT); digitalWrite(PIN_DO3, LOW);  // master power

        // Configure LEDC PWM for pump speed
        ledc_timer_config_t timer{};
        timer.speed_mode       = LEDC_LOW_SPEED_MODE;
        timer.duty_resolution  = LEDC_TIMER_8_BIT;
        timer.timer_num        = LEDC_TIMER_0;
        timer.freq_hz          = 5000;
        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_0;
        ch.timer_sel  = LEDC_TIMER_0;
        ch.gpio_num   = PIN_ACTUATOR_PWM;
        ch.duty       = 0;
        ledc_channel_config(&ch);

        AR_LOGI("HydraulicActuator", "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)               return false;
        if (_at_standby())           return false;

        // Software angle limit
        const float clamped = std::clamp(angle_deg, -_limit_deg, _limit_deg);

        // Hard-stop if limit switch asserted in the commanded direction
        if (clamped > 0.0f && _limit_stbd()) { _stop_outputs(); return false; }
        if (clamped < 0.0f && _limit_port())  { _stop_outputs(); return false; }

        const float duty_pct = std::fabsf(clamped) / _limit_deg * 100.0f;
        _set_pwm(static_cast<uint8_t>(duty_pct * 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::HYDRAULIC_REVERSIBLE; }

    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("HydraulicActuator", "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();  // defined in modes/standby.cpp

    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_0, duty);
        ledc_update_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_0);
    }
};

}  // namespace arautopilot::hal
