/**
 * @file speedometer.h
 * @author Alexander Klein (alex@kleiax.de)
 * @brief A complete 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 <cstdint>
#include <ESP32Encoder.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 calclutaed speed is the average of an amount of last measurments.
 * 
 */
class Speedometer {
    public:
        Speedometer();

        /**
         * @brief Initalize the speedometer
         * 
         * @param pinA Pin on the Esp from the encoder.
         * @param pinB 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.
         */
        void init(uint8_t pinA, uint8_t pinB, double diameter, uint16_t steps, uint8_t numOfValForAvg);
        void init(uint8_t pinA, uint8_t pinB, double diameter, uint16_t steps);

        /**
         * @brief Calls runSpeedometer() to update all Values.
         * 
         * This function should be called every mainloop. If the delay is not reached, than the
         * functions returns immediately.
         * @see runSpeedometer()
         * @see DELAY_SPEEDOMETER
         */
        void loop();

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

        /**
         * @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
         * 
         * @param val default = 250, max = 1023
         */
        void setEncFilter(uint16_t val);

        /**
         * @brief Set the min delay between each loop
         * 
         * @param val time in Milliseconds
         */
        void setDelay(uint8_t delay);

        /**
         * @brief Get the calculated speed of the Wheel
         * 
         * @return double speed in m/s
         */
        double getSpeed();


    private:
        void initAvgBuf();
        void addValToBuf(int16_t val);
        void updateAvgBufSize();
        int16_t calcAverage();

        ESP32Encoder encoder;

        bool isInit = false;

        double speed = 0;
        double diameter;

        uint8_t bufSize = BUFSIZE;
        uint16_t steps;
        uint8_t delay = DELAY_SPEEDOMETER;

        int16_t *buf;
};

#endif // SPEEDOMETER_H