#pragma once

#include "AHEncoder.hpp"

BEGIN_CS_NAMESPACE

inline int32_t AHEncoder::read() {
    if (interrupts_in_use < 2)
        update();
    return position.get();
}

inline int32_t AHEncoder::readAndReset(int32_t newpos) {
    if (interrupts_in_use < 2)
        update();
    return position.xchg(newpos);
}

inline void AHEncoder::write(int32_t p) {
    if (interrupts_in_use < 2)
        update();
    position.set(p);
}

//        new      new      old      old
//        pin2     pin1     pin2     pin1     Result
//        0        0        0        1        +1
//        0        0        1        0        -1
//        0        0        1        1        +2
//        0        1        0        0        -1
//        0        1        1        0        -2
//        0        1        1        1        +1
//        1        0        0        0        +1
//        1        0        0        1        -2
//        1        0        1        1        -1
//        1        1        0        0        +2
//        1        1        0        1        -1
//        1        1        1        0        +1

inline void AHEncoder::update() {
    uint8_t s = state & 0b11;
    if (direct_pins[0].read())
        s |= 4;
    if (direct_pins[1].read())
        s |= 8;
    state = (s >> 2);
    switch (s) {
        case 1:
        case 7:
        case 8:
        case 14: position.add_isr(1); return;
        case 2:
        case 4:
        case 11:
        case 13: position.add_isr(-1); return;
        case 3:
        case 12: position.add_isr(2); return;
        case 6:
        case 9: position.add_isr(-2); return;
    }
}

END_CS_NAMESPACE