// =============================================================================
//  di_do.cpp -- generic DI/DO implementation
// =============================================================================

#include "di_do.h"

#include <Arduino.h>

#include <array>

#include "../system/ar_log.h"
#include "pinout.h"

namespace arautopilot::hal {

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

// Tunable: how many consecutive identical samples constitute a confirmed
// state for a DI. At a 50 Hz poll rate, 2 -> ~40 ms debounce.
constexpr uint8_t DI_DEBOUNCE_SAMPLES = 2;

struct DiSlot {
    uint8_t pin;
    bool state;            // last confirmed (debounced) state
    bool prev_state;       // state on the *previous* poll, for edge detection
    uint8_t stable_count;  // consecutive identical raw reads
    bool raw_last;         // last raw read
};

constexpr std::size_t DI_COUNT = 5;
std::array<DiSlot, DI_COUNT> g_di_slots{
    DiSlot{PIN_DI1_DISENGAGE_BUTTON, false, false, 0, false},
    DiSlot{PIN_DI2_LIMIT_SWITCH_PORT, false, false, 0, false},
    DiSlot{PIN_DI3_LIMIT_SWITCH_STBD, false, false, 0, false},
    DiSlot{PIN_DI4_EXTERNAL_ALARM, false, false, 0, false},
    DiSlot{PIN_DI5_MANUAL_CONFIRM, false, false, 0, false},
};

struct DoSlot {
    uint8_t pin;
    bool state;
};

constexpr std::size_t DO_COUNT = 10;
std::array<DoSlot, DO_COUNT> g_do_slots{
    DoSlot{PIN_DO1_PUMP_PORT, false},      DoSlot{PIN_DO2_PUMP_STBD, false},
    DoSlot{PIN_DO3_ACTUATOR_POWER, false}, DoSlot{PIN_DO4_BUZZER, false},
    DoSlot{PIN_DO5_ENGAGED_LAMP, false},   DoSlot{PIN_DO6_RESERVED, false},
    DoSlot{PIN_DO7_RESERVED, false},       DoSlot{PIN_DO8_RESERVED, false},
    DoSlot{PIN_DO9_RESERVED, false},       DoSlot{PIN_DO10_RESERVED, false},
};

DiSlot* find_di(uint8_t pin) {
    for (auto& slot : g_di_slots) {
        if (slot.pin == pin) {
            return &slot;
        }
    }
    return nullptr;
}

DoSlot* find_do(uint8_t pin) {
    for (auto& slot : g_do_slots) {
        if (slot.pin == pin) {
            return &slot;
        }
    }
    return nullptr;
}
}  // namespace

void di_init() {
    for (auto& slot : g_di_slots) {
        pinMode(slot.pin, INPUT_PULLUP);
        // INPUT_PULLUP means a closed-to-ground switch reads LOW=pressed.
        // Our logical "true" = "pressed/active", so we invert in di_poll().
        slot.state = false;
        slot.prev_state = false;
        slot.stable_count = 0;
        slot.raw_last = false;
    }
    AR_LOGI(TAG, "di_init: %u digital inputs configured (INPUT_PULLUP)",
            (unsigned)g_di_slots.size());
}

void di_poll() {
    for (auto& slot : g_di_slots) {
        // Active-low (INPUT_PULLUP + switch-to-GND).
        bool raw = (digitalRead(slot.pin) == LOW);
        slot.prev_state = slot.state;
        if (raw == slot.raw_last) {
            if (slot.stable_count < DI_DEBOUNCE_SAMPLES) {
                ++slot.stable_count;
            }
            if (slot.stable_count >= DI_DEBOUNCE_SAMPLES) {
                slot.state = raw;
            }
        } else {
            slot.stable_count = 0;
        }
        slot.raw_last = raw;
    }
}

bool di_read(uint8_t pin) {
    auto* slot = find_di(pin);
    return slot != nullptr ? slot->state : false;
}

bool di_rising_edge(uint8_t pin) {
    auto* slot = find_di(pin);
    return slot != nullptr && slot->state && !slot->prev_state;
}

bool di_falling_edge(uint8_t pin) {
    auto* slot = find_di(pin);
    return slot != nullptr && !slot->state && slot->prev_state;
}

void do_init() {
    for (auto& slot : g_do_slots) {
        pinMode(slot.pin, OUTPUT);
        digitalWrite(slot.pin, LOW);
        slot.state = false;
    }
    AR_LOGI(TAG, "do_init: %u digital outputs configured (driven LOW)",
            (unsigned)g_do_slots.size());
}

bool do_write(uint8_t pin, bool high) {
    auto* slot = find_do(pin);
    if (slot == nullptr) {
        AR_LOGW(TAG, "do_write: unknown pin %d", (int)pin);
        return false;
    }
    if (slot->state == high) {
        return false;
    }
    digitalWrite(slot->pin, high ? HIGH : LOW);
    slot->state = high;
    return true;
}

bool do_state(uint8_t pin) {
    auto* slot = find_do(pin);
    return slot != nullptr ? slot->state : false;
}

void do_all_off() {
    for (auto& slot : g_do_slots) {
        digitalWrite(slot.pin, LOW);
        slot.state = false;
    }
    AR_LOGW(TAG, "do_all_off: all digital outputs driven LOW");
}

}  // namespace arautopilot::hal
