/**
 * @file calcAzimuth.h
 * @author Alexander Klein (alex@kleiax.de)
 * @brief Contains a class that calculates the azimuth from a last and a current position
 * @version 0.1
 * @date 2023-09-03
 *
 * @copyright Copyright (c) 2023
 *
 */

#ifndef CALC_AZIMUTH_H
#define CALC_AZIMUTH_H

#include "component.h"
#include "point.h"

/**
 * @brief A class to calculate an azimuth
 *
 * This class calculates the current Azimuth with the last position
 * where the rover has been rotated and the current position
 */
class CalcAzimuth : public Component
{
public:
    /**
     * @brief States which represent the quality of the current calculated azimuth
     */
    enum State
    {
        Invalid,
        Bad,
        Ok,
        Good,
        Super
    };

    /**
     * @brief Construct a new Calc Azimuth object
     *
     * @param point current position
     */
    CalcAzimuth(Point point);

    /**
     * @brief Have to be called if the rover rotates
     *
     * @param point current position
     */
    void drivingDirectionChange(Point point);

    /**
     * @brief update the current position
     *
     * This function should be called if the rover has moved in
     * a straight direction, to calculated the current Azimuth.
     * More distance to the point given to drivingDirectionChange()
     * increase the accuracy of the calculation.
     *
     * @param point current position
     */
    void updateCurrentPosition(Point point);
    void disableCalcAzimuth() { this->directionChangeMode = false; }

    int16_t getAzimuth() const { return this->calcAzimuth; }

    /**
     * @brief Get the State struct
     *
     * @return State current quality of the calculation
     */
    State getState() const { return this->state; }

    static String stateToString(State state);

private:
    void run() override;
    void updateAzimuth();

    State state = State::Invalid;
    Point lastChangePoint;
    Point currentPosition;

    bool positionChanged = false;
    bool directionChangeMode = false;
    int16_t calcAzimuth = INT16_MAX;
    double minDistanceForBetterQuality = 2;

    static constexpr uint8_t loopDelay = 50;
};

#endif // CALC_AZIMUTH_H