/**
 * @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)
    : pulseCounter{new Counter(pin)}, diameter{diameter}, steps{steps}, buf{}
{
    this->pulseCounter->setFilterValue(Speedometer::maxFilterValue); // ignore pulses less than 1000 x 2.5ns

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

    Component::loopDelay = Speedometer::loopDelay;

    this->clearAvgBuf();
}

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

void Speedometer::run()
{
    static constexpr float minimalSpeed = 0.1;

    if (this->calibrationRunning)
    {
        return;
    }

    const uint32_t time = millis();

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

    const double pulse = this->pulseCounter->getValue();
    this->pulseCounter->clear();
    this->pulseCounter->resume();

    const double wheelRevolutionsAbsolute = pulse / this->steps;
    const double wheelRevolutionsRelativ = wheelRevolutionsAbsolute / (elapsedTime / 1000.0);

    double meterPerSecond = wheelRevolutionsRelativ * (diameter * PI);
    double radPerSecond = wheelRevolutionsRelativ * 2 * PI;

    if (meterPerSecond < minimalSpeed)
    {
        meterPerSecond = 0;
        radPerSecond = 0;
    }

    switch (this->currentDirection)
    {
    case Direction::Forward:
        this->speed = meterPerSecond;
        this->rad = radPerSecond;
        break;

    case Direction::Backward:
        this->speed = -meterPerSecond;
        this->rad = -radPerSecond;
        break;

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

    this->addValToBuf(static_cast<int16_t>(this->speed * Speedometer::conversionFactor));
}

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

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

double Speedometer::getAvgSpeed() const
{
    const double avg = this->calcAverage();
    return avg / Speedometer::conversionFactor;
}

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;
    const uint16_t res = abs(this->pulseCounter->getValue());
    this->pulseCounter->clear();
    this->pulseCounter->resume();
    std::cout << "Result: " << res << std::endl;
    return res;
}

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;
    }
}

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