/**
 * @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 <SparkFun_u-blox_GNSS_Arduino_Library.h>
#include <QMC5883LCompass.h>
#include <Arduino.h>
#include <iostream>
#include <SPI.h>

#include "route.h"
#include "NTRIPClient.h"
#include "calibrateCompass.h"

/**
 * @brief The minimal distance between Points
 * 
 * This is a very critical option. It have to be
 * around the half accuracy of the positioning system
 * 
 */
#define MIN_DISTANCE_BETWEEN_POINTS 0.3
#define MIN_DISTANCE_TO_REACH_POINT 0.5
#define AZIMUTH_UPDATE_DELAY 20

/**
 * @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 gps device
 * to tell the driver in which direction he have to
 * be drive and the distance to the next checkpoint.
 * 
 */
class Navigation {
    public:
        enum Status {
            InsufficientAccuracy,
            Unchanged,
            Updated,
            Complete
        };


        /**
         * @brief Construct a new Navigation object and using I2C
         * 
         * @param route with which to navigate
         */
        Navigation(Route* route = nullptr);

        /**
         * @brief Construct a new Navigation object and using SPI
         * 
         * @param route with which to navigate
         */
        Navigation(SPIClass* spiPort, uint8_t csPin, Route* route = nullptr);

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

        /**
         * @brief 
         * 
         * @param host 
         * @param port 
         * @param mountPoint 
         * @param user 
         * @param password 
         */
        void initNtrip(String host, uint16_t port, String mountPoint, String user, String password);

        /**
         * @brief Decodes new GPS information
         * 
         * Should be called ever main loop.
         */
        void loop();

        /**
         * @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
         * 
         * This should be called by the driver to get new instructions.
         * 
         * @param correction passed as refernce to get the data
         * @return true if new correction data provided
         * @return false if route is finished 
         */
        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();

    	// TODO: can be deleted?
        // bool setPointBeforeTurn();
        
        /**
         * @brief Get the Ubx Data object
         * 
         * This struct includes the most Data from the GNSS-Module.
         * 
         * @return UBX_NAV_PVT_data_t* 
         */
        UBX_NAV_PVT_data_t* getUbxData() { return this->ubxData; }
        
        /**
         * @brief Returns the NTRIPClient object
         * 
         * @return NTRIPClient* 
         */
        NTRIPClient* getNTRIPClient() { return this->ntripClient; }

        /**
         * @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; }

        /**
         * @brief Get azimuth
         * 
         * This value represents the angle between north and 
         * the line of sight. Clockwise.
         * 
         * @return uint16_t degree
         */
        uint16_t getAzimuth() const { return this->azimuth; }
        QMC5883LCompass* getCompass() const { return this->compass; }

        /**
         * @brief Set the output status for PVTdata.
         * 
         * If this is true, a lot of information from the gnss module will be printed in
         * the interval of navigation frequency.
         * 
         * @param status 
         */
        static void setOutputStatusPrintPVTdata(bool status);

    private:
        void updateCurrentLocation();
        int16_t calculateCourseCorrection(Point& point);

        /**
         * @brief Set the next point as target
         * 
         * @return true
         * @return false 
         */
        bool nextPoint();

        /**
         * @brief Set the target point
         * 
         * @param target 
         * @return true 
         * @return false 
         */
        bool setTargetPoint(Point target);

        void init(Route* route);

        SFE_UBLOX_GNSS* gps;
        UBX_NAV_PVT_data_t* ubxData = nullptr;
        Route* route = nullptr;
        NTRIPClient* ntripClient = nullptr;
        QMC5883LCompass* compass;

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


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

        char* host;
        char* mountPoint;
        char* user;
        char* password;

        uint8_t timeToWait = 200;
        uint16_t port;
        uint16_t azimuth = UINT16_MAX;
        uint32_t lastMillis = 0;
        uint32_t ubxUpdateTime = 0;

        double minDistanceToReachPoint = 0.5;

        static void printPVTdata(UBX_NAV_PVT_data_t *ubxDataStruct);
        static void savePVTdata(UBX_NAV_PVT_data_t *ubxDataStruct);

        static UBX_NAV_PVT_data_t* ubxDataStatic;

        static uint32_t ubxUpdateTimeStatic;

        static bool outputStatusPrintPVTdata;
        static bool newData;
};

#endif // NAVIGATION_H