#include "AHEncoder.hpp"

#include <AH/STL/utility>
#include "hardware/gpio.h"

BEGIN_CS_NAMESPACE

AHEncoder::AHEncoder(uint8_t pinA, uint8_t pinB)
    : pins {pinA, pinB},
      direct_pins {direct_pin_read(pinA), direct_pin_read(pinB)} {}

AHEncoder::AHEncoder(AHEncoder &&other)
    : pins(other.pins), direct_pins(other.direct_pins) {
    if (other.interrupts_in_use)
        return; // can't move from initialized encoder
}

AHEncoder &AHEncoder::operator=(AHEncoder &&other) {
    swap(*this, other);
    return *this;
}

void swap(AHEncoder &a, AHEncoder &b) {
    std::swap(a.interrupts_in_use, b.interrupts_in_use);
    std::swap(a.pins, b.pins);
    noInterrupts();
    std::swap(a.state, b.state);
    std::swap(a.direct_pins, b.direct_pins);
    std::swap(a.position, b.position);
    if (a.interrupts_in_use > 0) {
        AHEncoder::instance_table[a.pins[0]] = &a;
        AHEncoder::instance_table[a.pins[1]] = &a;
    }
    if (b.interrupts_in_use > 0) {
        AHEncoder::instance_table[b.pins[0]] = &b;
        AHEncoder::instance_table[b.pins[1]] = &b;
    }
    interrupts();
}

AHEncoder::~AHEncoder() { end(); }

void AHEncoder::begin() {
    if (interrupts_in_use > 0)
        return;
    pinMode(pins[0], INPUT_PULLUP);
    pinMode(pins[1], INPUT_PULLUP);
    sleep_ms(2);

    uint8_t s = 0;
    if (direct_pins[0].read())
        s |= 1;
    if (direct_pins[1].read())
        s |= 2;
    state = s;

    attachInterruptCtx(pins[0]);
    attachInterruptCtx(pins[1]);
}

void AHEncoder::end() {
    if (interrupts_in_use > 0) {
        detachInterruptCtx(pins[0]);
        detachInterruptCtx(pins[1]);
    }
}

static bool gpio_callback_registered = false;

void encoder_gpio_callback(uint gpio, uint32_t) {
    if (gpio < CS_ENCODER_ARGLIST_SIZE) {
        if (auto *enc = AHEncoder::instance_table[gpio])
            enc->update();
    }
}

void AHEncoder::attachInterruptCtx(uint8_t pin) {
    if (instance_table[pin] != nullptr)
        return;
    instance_table[pin] = this;
    ++interrupts_in_use;

    if (!gpio_callback_registered) {
        gpio_set_irq_enabled_with_callback(
            pin, GPIO_IRQ_EDGE_FALL | GPIO_IRQ_EDGE_RISE, true,
            encoder_gpio_callback);
        gpio_callback_registered = true;
    } else {
        gpio_set_irq_enabled(pin, GPIO_IRQ_EDGE_FALL | GPIO_IRQ_EDGE_RISE,
                             true);
    }
}

void AHEncoder::detachInterruptCtx(uint8_t pin) {
    gpio_set_irq_enabled(pin, GPIO_IRQ_EDGE_FALL | GPIO_IRQ_EDGE_RISE, false);
    --interrupts_in_use;
    instance_table[pin] = nullptr;
}

AHEncoder *AHEncoder::instance_table[CS_ENCODER_ARGLIST_SIZE] {};

END_CS_NAMESPACE