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

#include "driveModi/Modi/Autopilot/autopilot.h"

Autopilot::Autopilot(MoveControl* moveControl, Navigation* navigation)
    : ManualControl(moveControl) {
        this->navigation = navigation;
        this->navigationStarted = this->navigation->startNavigation();
        this->navigation->getRouteInfo();
        this->courseCorrection.correction = 0;
        this->courseCorrection.distance = 0;
        this->updateDisplay = true;
}

void Autopilot::loop() {
    if (!this->selfDriving && this->navigationStarted)
        ManualControl::loop();

    if (millis() - this->last_millis < delay)
        return;

    this->runAutopilot();
    this->last_millis = millis();
}

void Autopilot::runAutopilot() {
    if (!this->navigationStarted || this->navigationEnded)
        return;

    this->courseCorrection = this->navigation->getCourseCorrection();
    this->routeInfo = this->navigation->getRouteInfo();
    this->updateDisplay = true;

    // Check if start Point is near to current Location
    if (this->routeInfo.currentPoint >= 2)
        this->selfDrivingAvailable = true;

    if (!this->selfDriving && this->selfDrivingAvailable)
        this->setSelfDriving(Ps3.data.button.triangle);

    if (this->routeInfo.currentPoint == this->routeInfo.totalPoints) {
        this->navigationEnded = true;
        this->navigationStarted = false;
        this->selfDrivingAvailable = false;
        this->setSelfDriving(false);
    }

    if (selfDriving) {
        this->setSpeedInRelToDistance(this->courseCorrection.distance);
        this->setRotInRelToDistance(this->courseCorrection.correction);
    }   
}

bool Autopilot::shouldUpdate() {
    if (this->updateDisplay) {
        this->updateDisplay = false;
        return true;
    }
    return false;
}

void Autopilot::setSelfDriving(bool val) {
    if (!this->navigationStarted)
        return;

    this->updateDisplay = true;

    this->selfDriving = val;
    this->moveControl->setSpeed(0);
    this->moveControl->setRotationspeed(0);
}

void Autopilot::setSpeedInRelToDistance(double distance) {
    if (distance >= MIN_DISTANCE_TO_NEXT_POINT)
        this->moveControl->setSpeed(AUTOPILOT_MAX_SPEED);
    else if (distance < MIN_DISTANCE_TO_NEXT_POINT)
        this->moveControl->setSpeed(AUTOPILOT_SPEED_2);
    else {
        this->moveControl->setSpeed(0);
        std::cout << "Error in Autopilot::setSpeedInRelToDistance" << std::endl;
    }
}

void Autopilot::setRotInRelToDistance(int16_t course) {
    int8_t steps = course / COURSE_CORRECTION_FACTOR;

    if (steps == 0 && abs(course) >= MIN_COURSE_CORRECTION_TO_ACT)
        steps = 1;

    if (course == 0)
        this->moveControl->setRotationspeed(0);
    else
        this->moveControl->setRotationspeed(steps);
}