// =============================================================================
//  rudder_actuator.cpp -- rudder actuator driver (Sprint 1 stub)
// =============================================================================

#include "rudder_actuator.h"

#include <Arduino.h>

#include "../modes/standby.h"
#include "../system/ar_log.h"
#include "di_do.h"
#include "pinout.h"

namespace arautopilot::hal {

namespace {
constexpr const char* TAG = "AR/HAL";

bool g_power_on = false;
int8_t g_last_command_pct = 0;
}  // namespace

void rudder_actuator_init() {
    // do_init() in di_do.cpp already configured DO1..DO10 as outputs LOW.
    // Just make sure our local state matches and assert the safe defaults.
    do_write(PIN_DO1_PUMP_PORT, false);
    do_write(PIN_DO2_PUMP_STBD, false);
    do_write(PIN_DO3_ACTUATOR_POWER, false);
    g_power_on = false;
    g_last_command_pct = 0;
    AR_LOGI(TAG, "rudder_actuator_init: power OFF, both directions LOW");
}

void rudder_actuator_power_on() {
    do_write(PIN_DO1_PUMP_PORT, false);  // ensure direction outputs LOW before
    do_write(PIN_DO2_PUMP_STBD, false);  // we energise the relay
    do_write(PIN_DO3_ACTUATOR_POWER, true);
    g_power_on = true;
    AR_LOGI(TAG, "rudder_actuator: master power ON");
}

void rudder_actuator_power_off() {
    do_write(PIN_DO1_PUMP_PORT, false);
    do_write(PIN_DO2_PUMP_STBD, false);
    do_write(PIN_DO3_ACTUATOR_POWER, false);
    g_power_on = false;
    g_last_command_pct = 0;
    AR_LOGW(TAG, "rudder_actuator: master power OFF (interlock or operator)");
}

bool rudder_actuator_is_powered() { return g_power_on; }

bool rudder_actuator_at_limit() {
    return di_read(PIN_DI2_LIMIT_SWITCH_PORT) ||
           di_read(PIN_DI3_LIMIT_SWITCH_STBD);
}

bool rudder_command(int8_t pwm_pct) {
    // ---- Safety interlocks -------------------------------------------------
    // Interlock #1: master power must be on. Without DO3, the H-bridge or
    // pump relays are isolated and the direction outputs do nothing -- but
    // we refuse anyway so the firmware state is consistent.
    if (!g_power_on) {
        if (pwm_pct != 0) {
            AR_LOGW(TAG, "rudder_command(%d): refused -- power OFF",
                    (int)pwm_pct);
        }
        do_write(PIN_DO1_PUMP_PORT, false);
        do_write(PIN_DO2_PUMP_STBD, false);
        g_last_command_pct = 0;
        return false;
    }

    // Interlock #2: STANDBY mode never drives the rudder.
    if (modes::is_standby()) {
        if (pwm_pct != 0) {
            AR_LOGW(TAG, "rudder_command(%d): refused -- STANDBY", (int)pwm_pct);
        }
        do_write(PIN_DO1_PUMP_PORT, false);
        do_write(PIN_DO2_PUMP_STBD, false);
        g_last_command_pct = 0;
        return false;
    }

    // Interlock #3: limit switches block further travel in that direction.
    if (pwm_pct < 0 && di_read(PIN_DI2_LIMIT_SWITCH_PORT)) {
        AR_LOGW(TAG, "rudder_command(%d): blocked -- port limit", (int)pwm_pct);
        do_write(PIN_DO1_PUMP_PORT, false);
        return false;
    }
    if (pwm_pct > 0 && di_read(PIN_DI3_LIMIT_SWITCH_STBD)) {
        AR_LOGW(TAG, "rudder_command(%d): blocked -- starboard limit",
                (int)pwm_pct);
        do_write(PIN_DO2_PUMP_STBD, false);
        return false;
    }

    // ---- Direction output --------------------------------------------------
    // Sprint 1 stub: just toggle the direction outputs. No PWM modulation;
    // the intensity will be wired in Sprint 2 via LEDC.
    if (pwm_pct > 0) {
        do_write(PIN_DO1_PUMP_PORT, false);
        do_write(PIN_DO2_PUMP_STBD, true);
    } else if (pwm_pct < 0) {
        do_write(PIN_DO2_PUMP_STBD, false);
        do_write(PIN_DO1_PUMP_PORT, true);
    } else {
        do_write(PIN_DO1_PUMP_PORT, false);
        do_write(PIN_DO2_PUMP_STBD, false);
    }
    g_last_command_pct = pwm_pct;
    return true;
}

}  // namespace arautopilot::hal
