/**
 * @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"
#include "autopilot.h"

DirectionChangeSignal::DirectionChangeSignal(Navigation* navigation) {
    this->navigation = navigation;
    this->action();
}

DirectionChangeSignal::~DirectionChangeSignal() {
    navigation->dissableCalcAzimuth();
}

void DirectionChangeSignal::action() {
    this->navigation->drivingDirectionChange();
}


Autopilot::Autopilot(MoveControl* moveControl, const ControlPadInput *input, Navigation* navigation)
    : ManualControl(moveControl, input) {
        this->setInputMode(ManualControl::InputMode::Digital);
        this->directionChangeSignal = new DirectionChangeSignal(navigation);
        this->setDirectionChangeCallback(this->directionChangeSignal);
        this->navigation = navigation;
        this->init();
}

Autopilot::~Autopilot() {
    delete this->directionChangeSignal;
    // this->navigation->getNTRIPClient()->setActivated(false);
}

void Autopilot::loop() {
    if (this->state < State::SelfDriving)
        ManualControl::loop();

    if (millis() - this->displayUpdateLastMillis > this->displayUpdateDelayMillis) {
        this->updateDisplay = true;
        this->displayUpdateLastMillis = millis();
    }

    if (millis() - this->loopLastMillis < loopDelayMillis)
        return;

    this->routeInfo = this->navigation->getRouteInfo();
    this->runAutopilot();
    this->loopLastMillis = millis();
}

void Autopilot::runAutopilot() {
    switch (this->state) {
        case State::InsufficientAccuarcy:
            this->askNavigationForOrder();
            return;

        case State::NoRoute:
            return;

        case State::None:
            return;

        case State::NavigationStarted:
            this->askNavigationForOrder();
            this->checkButtonInput();
            break;

        case State::GetToStartPoint:
            this->askNavigationForOrder();
            if (this->routeInfo.currentPoint >= 2)
                this->state = State::SelfDrivingAvailable;
            break;

        case State::SelfDrivingAvailable:
            this->askNavigationForOrder();
            this->checkButtonInput();
            break;

        case State::SelfDriving:
            this->askNavigationForOrder();
            this->checkButtonInput();
            this->selfDriving();
            break;

        case State::TargetReached:
            break;
        
        default:
            break;
    }
}

void Autopilot::restart() {
    this->init();
}

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

void Autopilot::init() {
    if (this->navigation->startNavigation())
            this->state = State::NavigationStarted;
        else
            this->state = State::NoRoute;
        this->routeInfo = this->navigation->getRouteInfo();
        this->navigation->getNTRIPClient()->setAutoReconnect(true);

        this->lastOrderStatus = this->navigation->getCourseCorrection(this->courseCorrection, true);

        this->courseCorrection.correction = 0;
        this->courseCorrection.distance = 0;
        this->updateDisplay = true;
}

void Autopilot::drive() {
    this->moveControl->setRotationSpeed(0);
    if (this->courseCorrection.distance >= this->minRemainingDistance) 
        this->moveControl->setSpeed(this->drivingSpeed);
    else
        this,moveControl->setSpeed(0);
}

void Autopilot::rotate() {
    this->moveControl->setSpeed(0);
    if (this->courseCorrection.correction > 0)
        this->moveControl->setRotationSpeed(-this->rotationSpeed);
    else
        this->moveControl->setRotationSpeed(this->rotationSpeed);
    this->navigation->drivingDirectionChange();
}

void Autopilot::checkButtonInput() {
    if (ControlPadButton::isControlPadButtonPressed(this->input, ControlPadButton::PadButton::Action)
        && millis() - this->lastAutopilotChangeMillis > this->autopilotChangeDelayMillis) {
        
        if (this->state  == State::SelfDrivingAvailable)
            this->state = State::SelfDriving;
        else if (this->state == State::SelfDriving)
            this->state = State::SelfDrivingAvailable;
        else if (this->state == State::NavigationStarted)
            this->state = State::GetToStartPoint;
        this->updateDisplay = true;
        this->lastAutopilotChangeMillis = millis();
    }
}

void Autopilot::askNavigationForOrder() {
    this->lastOrderStatus = this->navigation->getCourseCorrection(this->courseCorrection);
    switch (this->lastOrderStatus) {
        case Navigation::Status::Complete:
            this->state = State::TargetReached;
            this->moveControl->setSpeed(0);
            this->moveControl->setRotationSpeed(0);
            break;
        
        case Navigation::Status::InsufficientAccuracy:
            this->lastState = this->state;
            this->state = State::InsufficientAccuarcy;
            this->moveControl->setSpeed(0);
            this->moveControl->setRotationSpeed(0);
            break;

        case Navigation::Status::Unchanged:
            if (this->state == State::InsufficientAccuarcy)
                this->state = this->lastState;
            break;

        case Navigation::Status::Updated:
            if (this->state == State::InsufficientAccuarcy)
                this->state = this->lastState;
            break;
        
        default:
            break;
    }
}

void Autopilot::selfDriving() {
    if (abs(this->courseCorrection.correction) >= this->maxCourseDeviationBerforeAct)
        this->rotate();
    else
        this->drive();
}