/**
 * @file speedometer.h
 * @author Alexander Klein (alex@kleiax.de)
 * @brief A implementation to measure wheel speeds with an encoder.
 * @version 0.1
 * @date 2021-12-09
 * 
 * @copyright Copyright (c) 2021
 * 
 */

#ifndef SPEEDOMETER_H
#define SPEEDOMETER_H

#include <Arduino.h>
#include <cstdint>
#include <iostream>

#include "counter.h"
#include "component.h"

/**
 * @brief A class which use a encoder to calc the speed
 * 
 * This class use ESP32 pulse counter hardware peripheral.
 * The calculated speed is the average of an amount of last measurements.
 * 
 */
class Speedometer : public Component {
    public:
        /**
         * @brief Enum to control the direction.
         * 
         * If the Direction is Forward, the internal counter counts up and a positiv speed will be returned.
         * If the Direction is Backward, the internal counter counts down and a negativ speed will be returned.
         * If the Direction is None, no measurement will be taken.
         */
        enum Direction {
            None,
            Forward,
            Backward
        };

        /**
         * @brief Construct a new Speedometer object
         * 
         * @param pin Pin on the Esp from the encoder.
         * @param diameter Diameter of the wheel in meters.
         * @param steps Encodersteps for a complete wheel rotation.
         */
        Speedometer(uint8_t pin, double diameter, uint16_t steps);

        ~Speedometer();

        /**
         * @brief Set the direction
         * 
         * @param dir Direction
         */
        void setDirection(Direction dir);

        /**
         * @brief Set the number of last values ​​to be taken into account for the average.
         * 
         * @param val length of the array
         */
        void setNumOfValForAvg(uint8_t val);

        /**
         * @brief Set the Enc Filter to prevent bouncing
         * 
         *  ignore pulses less than val x 2.5ns
         * 
         * @param val default = 1000, max = 1023
         */
        void setEncFilter(uint16_t val);

        /**
         * @brief Get the Direction
         * 
         * @return Direction 
         */
        Direction getDirection() const { return this->currentDirection; }

        /**
         * @brief Get the calculated speed of the Wheel
         * 
         * @return double speed in m/s
         */
        double getSpeed() const { return this->speed; }
        double getAvgSpeed() const;

        /**
         * @brief Start calibration
         * 
         * This functions stops the loop. So that steps of one manual wheel turn
         * can measured. Call calibrationMeasurementStop to start the loop and get
         * the result.
         */
        void calibrationMeasurementStart();

        /**
         * @brief Stop calibration
         * 
         * Start the loop function and read the past steps.
         * 
         * @return uint16_t steps since calibrationMeasurementStart was called
         */
        uint16_t calibrationMeasurementStop();


    private:
        void run() override;
        void init(uint8_t pin, double diameter, uint16_t steps);
        void clearAvgBuf();
        void addValToBuf(int16_t val);
        int16_t calcAverage() const;

        static constexpr uint8_t loopDelay = 30;
        static constexpr uint8_t bufSize = 5;
        static constexpr uint8_t conversionFactor = 100;        

        Counter* pulseCounter;
        Direction currentDirection = Direction::None;

        bool calibrationRunning = false;

        double speed = 0;
        double diameter;

        uint8_t printCounter = 0;
        uint8_t bufPos = 0;
        uint16_t steps;

        int16_t buf[Speedometer::bufSize];

        uint32_t lastMillisCalc = 0;
};

#endif // SPEEDOMETER_H