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

RotaryEncoderDriver* driver = nullptr;

RotaryEncoderDriver::RotaryEncoderDriver()
    : cycle(0)
    , count(0)
    , prev_count(0)
    , speed_ready(false)
    , hist_idx(0)
    , eA(2)
    , eB(5)
    , delta(0)
{
    for (uint8_t i = 0; i < HIST_LEN; i++) 
    {
        history[i] = 0;
    }
}


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

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

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

    timer.init(4000);
}

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

    int16_t oldest = history[hist_idx];

    history[hist_idx] = count;

    hist_idx = (hist_idx + 1) % HIST_LEN;

    delta = count - oldest;   // breyting yfir HIST_LEN * Ts (16 ms) glugga

    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 / (HIST_LEN * 0.004f);
}


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;

    for (uint8_t i = 0; i < HIST_LEN; i++) 
    {
        history[i] = count;
    }

    hist_idx = 0;

    SREG = oldSREG;
}

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