/**
 * @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"

/**
 * @brief The default size of numbers to be taken in account for the average.
 * 
 */
#define BUFSIZE 10

/**
 * @brief Default value for min Millisseconds between each loop
 * @see setDelay(uint8_t val)
 */
#define DELAY_SPEEDOMETER 30
#define PI 3.1415926535897932384626433832795

/**
 * @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:
        /**
         * @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 Dorection 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.
         * @param numOfValForAvg Number of last values ​​to be taken into account for the average.
         */
        Speedometer(uint8_t pin, double diameter, uint16_t steps, uint8_t numOfValForAvg);
        Speedometer(uint8_t pin, double diameter, uint16_t steps);

        ~Speedometer();

        /**
         * @brief Calls runSpeedometer() to update all Values.
         * 
         * This function should be called every mainloop. If the delayLoop is not reached, than the
         * functions returns immediately.
         * @see runSpeedometer()
         * @see DELAY_SPEEDOMETER
         * @return time since the last call in Milliseconds
         */
        uint16_t loop();

        /**
         * @brief Normally called repeatedly by loop() to calculate new values.
         * 
         * Add a new Value to the average and update the speed.
         */
        void runSpeedometer();

        /**
         * @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 Set the min delayLoop between each loop
         * 
         * @param delayLoop time in Milliseconds
         */
        void setDelay(uint8_t delayLoop) { this->delayLoop = delayLoop; }

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

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

        /**
         * @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 init(uint8_t pin, double diameter, uint16_t steps);
        void initAvgBuf();
        void clearAvgBuf();
        void addValToBuf(int16_t val);
        void updateAvgBufSize();
        int16_t calcAverage();

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

        bool calibrationRunning = false;

        double speed = 0;
        double diameter;

        uint8_t printCounter = 0;
        uint8_t bufSize = BUFSIZE;
        uint8_t delayLoop = DELAY_SPEEDOMETER;
        uint8_t bufPos = 0;
        uint16_t steps;

        int16_t *buf = nullptr;

        uint32_t lastMillisLoop = 0;
        uint32_t lastMillisCalc = 0;
};

#endif // SPEEDOMETER_H