#include "rotary_encoder_driver.h"
#include <avr/interrupt.h>

RotaryEncoderDriver* driver = nullptr;

RotaryEncoderDriver::RotaryEncoderDriver()
    : cycle(0)
    , count(0)
    , prev_count(0)
    , speed_ready(false)
    , led(5)
    , eA(2)
    , eB(5) {}


void RotaryEncoderDriver::begin() {
    driver = this;

    led.init();
    eA.init();
    eB.init();

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

    timer.init(5000);
}

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

int RotaryEncoderDriver::position() {

    uint8_t oldSREG = SREG;

    cli();

    int16_t local_count = count;

    SREG = oldSREG;

    led.set_lo();

    return local_count;
} 

void RotaryEncoderDriver::update_delta() {
    
        delta = count - prev_count;

        prev_count = count;

        speed_ready = true;
    
    led.set_lo();
}

float RotaryEncoderDriver::count_per_sec() {

    uint8_t oldSREG = SREG;

    cli();

    int16_t local_delta = delta;

    SREG = oldSREG;

    return local_delta / 0.005f;
}


float RotaryEncoderDriver::rpm()
{
    const float COUNTS_PER_REV = 1050.0f;

    return count_per_sec() * 60.0f / COUNTS_PER_REV;
}

bool RotaryEncoderDriver::new_speed_available()
{
    uint8_t oldSREG = SREG;

    cli();

    bool ready = speed_ready;

    if (ready)
    {
        speed_ready = false;
    }

    SREG = oldSREG;

    return ready;
}


void RotaryEncoderDriver::reset_speed_measurement()
{
    uint8_t oldSREG = SREG;

    cli();

    cycle = 0;
    prev_count = count;
    delta = 0;
    speed_ready = false;

    SREG = oldSREG;
}

ISR(INT0_vect) {
    driver->update();
}
ISR(TIMER1_COMPA_vect) {
    driver->update_delta();    
}