#include "speedometer.h"

Speedometer::Speedometer() {}

void Speedometer::init(uint8_t pinA, uint8_t pinB, double diameter, uint16_t steps, uint8_t numOfValForAvg) {
    this->init(pinA, pinB, diameter, steps);
    this->bufSize = numOfValForAvg;
}

void Speedometer::init(uint8_t pinA, uint8_t pinB, double diameter, uint16_t steps) {
    ESP32Encoder::useInternalWeakPullResistors=DOWN;
    this->encoder.attachFullQuad(pinA, pinB);
    this->diameter = diameter;
    this->steps = steps;

    initAvgBuf();

    this->isInit = true;
}

void Speedometer::loop() {
    static uint32_t last_millis = 0;
    uint32_t time = millis();

    //Cancel if delay is not reached
    if (time - last_millis < delay) {
        return;
    }
    runSpeedometer();
    last_millis = time;
}

void Speedometer::runSpeedometer() {
    static uint32_t last_millis = 0;
    uint32_t time = millis();

    uint16_t elapsed_time = time - last_millis;
    last_millis = time;

    int16_t count = encoder.getCount();
    this->addValToBuf(count);
    encoder.clearCount();

    uint16_t count_abs = abs(this->calcAverage());          // Absolute time in milliseconds
    double n = (double)count_abs / steps;                   // Wheel revolutions in absolute time
    double u = (double)n / ((double)elapsed_time / 1000);   // Wheel revolutions per second
    double ms = u * (diameter * PI);                        // Speed in m/s

    if (count > 0) {
        this->speed = ms;
    } else if (count < 0) {
        this->speed = ms * (-1);
    } else {
        this->speed = 0;
    }
}

void Speedometer::setNumOfValForAvg(uint8_t val) {
    this->bufSize = val;
    if (this->isInit) 
        updateAvgBufSize();
}

void Speedometer::setEncFilter(uint16_t val) {
    if (val > 1023) val = 1023;
    this->encoder.setFilter(val);
}

void Speedometer::setDelay(uint8_t delay) {
    this->delay = delay;
}

double Speedometer::getSpeed() {
    return this->speed;
}

void Speedometer::initAvgBuf() {
    this->buf = new int16_t[bufSize];
    for (uint8_t i = 0; i < bufSize; i++) 
        this->buf[i] = 0;
}

void Speedometer::addValToBuf(int16_t val) {
    static uint8_t bufPos = 0;
    this->buf[bufPos] = val;
    bufPos++;

    if (bufPos == bufSize) {
        bufPos = 0;
    }
}

void Speedometer::updateAvgBufSize() {
    delete[] this->buf;
    initAvgBuf();
}

int16_t Speedometer::calcAverage() {
    int16_t sum = 0;
    for (int i = 0; i < bufSize; i++)
        sum += this->buf[i];
    return sum / bufSize;
}