#include <avr/io.h>
#include <util/delay.h>
#include <Arduino.h>

#include "digital_in.h"
#include "digital_out.h"

class RotaryEncoderDriver {
    int count{0};
    bool last_lo{false};

    Digital_out led{5};

    Digital_in eA;
    Digital_in eB;

public:
    RotaryEncoderDriver(unsigned short pin_a = 4, unsigned short pin_b = 5) : eA(pin_a), eB(pin_b) {
        led.init();
        eA.init();
        eB.init();
        last_lo = eA.is_lo();
    }

    int position() {
        bool curr_hi = eA.is_hi();

        if (last_lo && curr_hi) {
            count += eB.is_hi() ? 1 : -1;
            led.set_hi();
        } else {
            led.set_lo();
        }

        last_lo = !curr_hi;
        return count;
    } 

};

int main() {
    uint8_t cycle{0};

    RotaryEncoderDriver encoder;
    Serial.begin(9600);

    int prev_pos = encoder.position();
    while (true) {
        _delay_us(100);
        int curr_pos = encoder.position();

        if (cycle++ == 255) {
            int delta = curr_pos - prev_pos;
            float rpm = delta * 60.0f / (700.0f * 0.0256f);

            Serial.print("Count: ");
            Serial.print(delta);
            Serial.print("\tRPM: ");
            Serial.println(rpm);

            prev_pos = curr_pos;
        }
    }
    return 0;
}