Bachelorarbeit-Rover/lib/Navigation/navigation.h

/**
 * @file navigation.h
 * @author Alexander Klein (alex@kleiax.de)
 * @brief Contains a class which navigate an object by the given route
 * @version 0.1
 * @date 2022-01-10
 *
 * @copyright Copyright (c) 2022
 *
 */

#ifndef NAVIGATION_H
#define NAVIGATION_H

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

#include "route.h"
#include "sensorData.h"
#include "component.h"

/**
 * @brief This struct inherits the result of the navigation
 *
 * The drive get objects of this struct and should
 * correct the direction in dependency on this.
 *
 */
struct CourseCorrection
{
    int16_t correction;
    double distance;
};

/**
 * @brief This class navigate an object
 *
 * The class use the given Route and the gnss device
 * to tell the driver in which direction he have to
 * be drive and the distance to the next checkpoint.
 *
 */
class Navigation : public Component
{
public:
    enum Status
    {
        InsufficientAccuracy,
        Unchanged,
        Updated,
        Complete
    };

    /**
     * @brief Construct a new Navigation object
     *
     * @param sensorData
     * @param route
     */
    Navigation(const SensorData *sensorData, Route *route = nullptr);

    /**
     * @brief Destroy the Navigation object
     *
     */
    ~Navigation();

    /**
     * @brief creates a new empty route
     *
     */
    void newRoute();

    /**
     * @brief Tries to start the route
     *
     * For example the route can not be started
     * if there are no Points or wrong Points.
     *
     * @return true route is started
     * @return false route can not be started
     */
    bool startNavigation();
    void freezeTargetPoint(bool val = true) { this->preventNextPoint = val; };

    double increaseMinDistanceToReachPoint() { return this->minDistanceToReachPoint += 0.1; }
    double decreaseMinDistanceToReachPoint() { return this->minDistanceToReachPoint -= 0.1; }

    // TODO: Dokumentation korrigieren.
    /**
     * @brief Get the Course Correction object
     *
     * @param correction
     * @param forceUpdate
     * @return Status
     */
    Status getCourseCorrection(CourseCorrection &correction, bool forceUpdate = false);

    // TODO: Dokumentation korrigieren.
    /**
     * @brief Tries to add the current Position to the route
     *
     * This can be go wrong if there is no valid GPS signal
     *
     * @return true successful added point
     * @return false no point added to route
     */
    Status addCurrentPosToRoute();

    /**
     * @brief Get the Route Info object
     *
     * This object contains information about the route.
     * For example the stored points.
     *
     * @return RouteInfo
     */
    RouteInfo getRouteInfo() const { return this->route->getRouteInfo(); }
    Route *getRoute() const { return this->route; }
    Point getCurrentPosition() const { return this->currentPosition; }

    Point::Accuracy getMinAccuracy() const { return this->minAccuracy; }
    void setMinAccuracy(Point::Accuracy accuracy) { this->minAccuracy = accuracy; }

    // map input in range from -180 to 180 degree
    static int16_t fixDegree(int16_t degree);

private:
    void run() override;
    void init(Route *route);
    bool nextPoint();
    bool setTargetPoint(Point target);
    int16_t calculateCourseCorrection(Point &point);

    static constexpr uint8_t loopDelay = 20;
    static constexpr uint8_t maxDisBetweenPoints = 10;
    static constexpr float minDisBetweenPoints = 0.3;

    const SensorData *sensorData;
    Route *route = nullptr;

    Point lastPointRouteInsert;
    Point lastPointCalcCorrection;
    Point lastPointDrivingDirectionChange;
    Point targetPoint;
    Point currentPosition;
    Point::Accuracy minAccuracy = Point::Accuracy::twoDigOfCM;

    bool navigationStarted = false;
    bool navigationFinished = false;
    bool isNtripInit = false;
    bool preventNextPoint = false;
    bool directionChangeMode = false;
    bool lastUsedCalcAzimuth = false;

    uint8_t timeToWait = 200;
    uint32_t lastMillis = 0;
    uint32_t ubxUpdateTime = 0;

    double minDistanceToReachPoint = 0.5;
};

#endif // NAVIGATION_H