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

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

class RotaryEncoderDriver {
    volatile int count{0};

    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();

        EICRA |= (1<<ISC01) | (1<<ISC00);
        EIMSK |= (1<<INT0);
    }

    void update() {
        count += eB.is_hi() ? 1 : -1;
        led.set_hi();
    }

    int position() {
        led.set_lo();
        return count;
    } 

};

RotaryEncoderDriver encoder{2,5};

int main() {
    uint8_t cycle{0};

    Serial.begin(9600);

    int prev_pos = encoder.position();
    sei();
    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;
}

ISR(INT0_vect) {
    encoder.update();
}