#include "gpio.hpp"
#include "log.hpp"
#include "zephyr.hpp"
#include <expected>

namespace rims {

K_FIFO_DEFINE(gpioIngress);
K_FIFO_DEFINE(gpioEggress);

fifo_queue<gpio_IngressMessage, 2> gpioIngressQueue{gpioIngress};
fifo_queue<gpio_EgressMessage, 2>  gpioEgressQueue{gpioEggress};

constexpr int                                channels = 2;
constexpr std::array<gpio_dt_spec, channels> pins     = {gpio_dt_spec GPIO_DT_SPEC_GET(DT_NODELABEL(gprelay_1_en), gpios), gpio_dt_spec GPIO_DT_SPEC_GET(DT_NODELABEL(gprelay_2_en), gpios)};

constexpr auto gpio_factory = handler_factory<gpio_IngressMessage, gpio_EgressMessage, gpio::error>{};

constexpr std::array<handler, 1> gpio_modeHandlers = {gpio_factory.make_handler_expected<GpioRequest, &GPIO::handle_gpioRequest>()};

GPIO::GPIO() {
    gpioIngressQueue.k_event_init(_events.at(0));
}
// thread main
void GPIO::loop() {
    while (1) {
        auto ret = zephyr::event_poll::k_poll_forever(_events);
        if (ret == 0) {
            zephyr::event_poll::k_poll_handle(_events[0], [&]() { event_gpioMessageArrived(); });
        }
    }
}

std::expected<void, gpio::error> GPIO::handle_gpioRequest(const GpioRequest &request, GpioResponse &response)  {
    ULOG_INFO("GpioRequest request handler");
    // if(request.gpio) // todo check channel

    response.gpio = request.gpio;

    auto togpioerr = [](auto) { return gpio::io_error{}; };

    if (request.has_state) {
        switch (request.gpio) {
        case gpio_GPIO_GPIO_pin_1:
            ULOG_INFO("PIN[%d] set to %d", 0, request.state);
            TRY_TRANSFORM_ERROR(zephyr::gpio::pin_set_dt(pins[0], request.state), togpioerr);
            break;
        case gpio_GPIO_GPIO_pin_2:
            ULOG_INFO("PIN[%d] set to %d", 1, request.state);
            TRY_TRANSFORM_ERROR(zephyr::gpio::pin_set_dt(pins[1], request.state), togpioerr);
            break;
        default:
            return std::unexpected{gpio::bad_id{}};
        }
    }
    switch (request.gpio) {
    case gpio_GPIO_GPIO_pin_1:
        response.state = TRY_TRANSFORM_ERROR(zephyr::gpio::pin_get_state(pins[0]), togpioerr); // try returns error if
        break;
    case gpio_GPIO_GPIO_pin_2:
        response.state = TRY_TRANSFORM_ERROR(zephyr::gpio::pin_get_state(pins[1]), togpioerr);
        break;
    default:
        return std::unexpected{gpio::bad_id{}};
    }
    return {};
}

void GPIO::event_gpioMessageArrived() {
    static gpio_IngressMessage req;
    static gpio_EgressMessage  resp;
    
    constexpr auto service = "gpio";
    
    ULOG_INFO("%s consume ingress", service);
    gpioIngressQueue.try_consume([&](const gpio_IngressMessage &_req) {
        req = _req;
        return true;
    });
    
    ULOG_INFO("%s execute handler", service);
    for (const auto &handler : gpio_modeHandlers) {
        if (handler.execute(this, req, resp)) break;
    }
    
    ULOG_INFO("%s produce egress", service);
    gpioEgressQueue.try_produce(resp);
}

void GpioThread::threadMain() {
    ULOG_INFO("GPIOThread start");
    /// runs in context of new thread, on new thread stack etc.
    GPIO thread{};
    thread.loop();
}

int GpioThread::do_hardwarenInit() {
    auto configure = [](const auto &dt_spec) { zephyr::gpio::pin_configure(dt_spec, GPIO_OUTPUT_INACTIVE); };
    std::for_each(pins.begin(), pins.end(), configure);
    return 0;
}

} // namespace rims