/**
 * @file speedometer.cpp
 * @author Alexander Klein (alex@kleiax.de)
 * @brief Implementation of the class speedometer.h
 * @see speedometer.h
 * @version 0.1
 * @date 2021-12-13
 * 
 * @copyright Copyright (c) 2021
 * 
 */
#include "speedometer.h"

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

Speedometer::Speedometer(uint8_t pin, double diameter, uint16_t steps) {
    this->init(pin, diameter, steps);
}

Speedometer::~Speedometer() {
    delete[] this->buf;
    delete this->pulseCounter;
}

uint16_t Speedometer::loop() {
    if (this->calibrationRunning)
        return -1;

    uint32_t time = millis();
    uint16_t elapsedTime = time - this->lastMillisLoop;

    //Cancel if delayLoop is not reached
    if (elapsedTime < this->delayLoop)
        return elapsedTime;
    
    runSpeedometer();
    this->lastMillisLoop = time;
    return elapsedTime;
}

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

    uint16_t elapsedTime = time - this->lastMillisCalc;
    this->lastMillisCalc = time;

    this->addValToBuf(this->pulseCounter->getValue());
    this->pulseCounter->clear();
    this->pulseCounter->resume();

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

    switch (this->currentDirection) {
        case Direction::Forward :
            this->speed = ms;
            break;
        
        case Direction::Backward :
            this->speed = -ms;
            break;

        case Direction::None :
            this->speed = 0;
            break;
    }

    // if (this->speed)
    //     this->printCounter++;
    // if (this->printCounter > 10) {
    //     this->printCounter = 0;
    //     std::cout << "Speedometer::runSpeedometer speed: " << (int) this->speed << std::endl;
    // }
}

void Speedometer::setDirection(Direction dir) {
    if (this->currentDirection == dir)
        return;
    this->currentDirection = dir;
    this->clearAvgBuf();
}

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

void Speedometer::setEncFilter(uint16_t val) {
    if (val > 1023) 
        val = 1023;
    this->pulseCounter->setFilterValue(val);
}

void Speedometer::calibrationMeasurementStart() {
    std::cout << "Start" << std::endl;
    this->calibrationRunning = true;
    this->pulseCounter->clear();
    this->pulseCounter->resume();
}

uint16_t Speedometer::calibrationMeasurementStop() {
    std::cout << "Ende" << std::endl;
    this->calibrationRunning = false;
    uint16_t res = abs(this->pulseCounter->getValue());
    this->pulseCounter->clear();
    this->pulseCounter->resume();
    std::cout << "Result: " << res << std::endl;
    return res;
}

void Speedometer::init(uint8_t pin, double diameter, uint16_t steps) {
    this->diameter = diameter;
    this->steps = steps;

    this->pulseCounter = new Counter(pin);
    this->pulseCounter->setFilterValue(1000); // ignore pulses less than 1000 x 2.5ns

    this->pulseCounter->clear();
    this->pulseCounter->resume();

    initAvgBuf();
}

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

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

void Speedometer::addValToBuf(int16_t val) {
    this->buf[this->bufPos] = val;
    this->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 < this->bufSize; i++)
        sum += this->buf[i];
    return sum / this->bufSize;
}