/**
 * @file navigation.cpp
 * @author Alexander Klein (alex@kleiax.de)
 * @brief Contains the implementation of the class Navigation
 * @version 0.1
 * @date 2022-01-31
 *
 * @copyright Copyright (c) 2022
 *
 */

#include "navigation.h"

Navigation::Navigation(const SensorData *sensorData, Route *route)
    : sensorData{sensorData}
{
    this->init(route);
}

void Navigation::init(Route *route)
{
    if (static_cast<bool>(route))
    {
        this->route = route;
    }
    else
    {
        this->route = new Route();
    }

    Component::loopDelay = Navigation::loopDelay;
}

Navigation::~Navigation()
{
    delete this->route;
}

void Navigation::run() {}

void Navigation::newRoute()
{
    delete this->route;
    this->route = new Route();
}

bool Navigation::startNavigation()
{
    const Point newTargetPoint = this->route->startRoute();
    this->navigationStarted = this->setTargetPoint(newTargetPoint);
    if (this->navigationStarted)
    {
        this->navigationFinished = false;
    }
    return this->navigationStarted;
}

Navigation::Status Navigation::getCourseCorrection(CourseCorrection &correction, bool forceUpdate)
{
    if (this->navigationFinished)
    {
        return Status::Complete;
    }

    if (this->currentPosition.getAccuracy() <= this->minAccuracy)
    {
        return Status::InsufficientAccuracy;
    }

    // Check if the Rover has moved, if the Rover hasnt moved this function will be return.
    if (this->currentPosition.distanceTo(this->lastPointCalcCorrection) < (this->minDistanceToReachPoint) / 2 && !forceUpdate)
    {
        correction.correction = this->calculateCourseCorrection(this->lastPointCalcCorrection);
        correction.distance = this->lastPointCalcCorrection.distanceTo(this->targetPoint);
        return Status::Unchanged;
    }

    double distance = this->currentPosition.distanceTo(this->targetPoint);

    // Check if I need a new Point
    if (distance < this->minDistanceToReachPoint && this->preventNextPoint == false)
    {
        if (!this->nextPoint())
        {
            this->navigationFinished = true;
            this->navigationStarted = false;
            return Status::Complete; // End of navigation
        }
        distance = this->currentPosition.distanceTo(this->targetPoint);
    }

    correction.correction = this->calculateCourseCorrection(this->currentPosition);
    correction.distance = distance;
    this->lastPointCalcCorrection = this->currentPosition;
    return Status::Updated;
}

Navigation::Status Navigation::addCurrentPosToRoute()
{
    if (this->currentPosition.getAccuracy() <= this->minAccuracy)
    {
        return Status::InsufficientAccuracy;
    }

    // First Point
    if (this->route->getRouteInfo().totalPoints == 0)
    {
        this->route->addPointToRoute(this->currentPosition);
        this->lastPointRouteInsert = this->currentPosition;
        return Status::Updated;
    }

    // Every Point after the first
    const double distance = this->currentPosition.distanceTo(this->lastPointRouteInsert);
    if (Navigation::minDisBetweenPoints <= distance && Navigation::maxDisBetweenPoints >= distance)
    {
        this->route->addPointToRoute(this->currentPosition);
        this->lastPointRouteInsert = this->currentPosition;
        return Status::Updated;
    }
    return Status::Unchanged;
}

int16_t Navigation::calculateCourseCorrection(Point &point)
{
    const int16_t targetCourse = point.courseTo(this->targetPoint);
    int16_t correctionCourse = 0;

    if (this->sensorData->getCalcAzimuthState() == CalcAzimuth::State::Good || this->sensorData->getCalcAzimuthState() == CalcAzimuth::State::Super)
    {
        correctionCourse = targetCourse - this->sensorData->getCalcAzimuth();
        this->lastUsedCalcAzimuth = true;
    }
    else
    {
        correctionCourse = targetCourse - this->sensorData->getRealAzimuth();
        this->lastUsedCalcAzimuth = false;
    }

    return Navigation::fixDegree(correctionCourse);
}

bool Navigation::nextPoint()
{
    if (!this->navigationStarted)
    {
        return false;
    }
    return this->setTargetPoint(this->route->getNextPoint());
}

bool Navigation::setTargetPoint(Point target)
{
    if (target.isInit())
    {
        this->targetPoint = target;
        return true;
    }
    return false;
}

int16_t Navigation::fixDegree(int16_t degree)
{
    static constexpr uint16_t fullCircle = 360;

    while (degree < -fullCircle / 2 || degree > fullCircle / 2)
    {
        if (degree > fullCircle / 2)
        {
            degree -= fullCircle;
        }
        else if (degree < -fullCircle / 2)
        {
            degree += fullCircle;
        }
    }
    return degree;
}