/**
 * @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(Autopilot *pilot)
    : pilot {pilot}
{
    this->action();
}

DirectionChangeSignal::~DirectionChangeSignal()
{
    CalcAzimuth *calcAzimuth = pilot->getSensorData()->getCalcCompass();
    if (static_cast<bool>(calcAzimuth))
    {
        calcAzimuth->disableCalcAzimuth();
    }
}

void DirectionChangeSignal::action()
{
    CalcAzimuth *calcAzimuth = pilot->getSensorData()->getCalcCompass();
    if (static_cast<bool>(calcAzimuth))
    {
        calcAzimuth->drivingDirectionChange(pilot->getSensorData()->getCurrentPos());
    }
}

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

void Autopilot::run()
{
    this->routeInfo = this->navigation->getRouteInfo();

    switch (this->state)
    {
    case State::InsufficientAccuracy:
        this->askNavigationForOrder();
        return;

    case State::NoRoute:
        return;

    case State::None:
        return;

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

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

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

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

    case State::SelfDrivingRotate:
        this->checkButtonInput();
        this->rotate();
        break;

    case State::TargetReached:
        if (this->loopMode){
            this->restartLoop();
        }
        break;

    default:
        break;
    }
}

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

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

void Autopilot::testRotate(int16_t degree)
{
    if (!static_cast<bool>(degree))
    {
        return;
    }

    this->courseCorrection.correction = degree;
    this->beginRotate();
}

void Autopilot::init()
{
    if (this->navigation->startNavigation())
    {
        this->state = State::NavigationStarted;
    }
    else
    {
        this->state = State::NoRoute;
    }
    this->routeInfo = this->navigation->getRouteInfo();
    this->sensorData->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()
{
    DrivingSpeeds speeds = {0, 0};
    if (this->courseCorrection.distance >= this->minRemainingDistance)
    {
        speeds.x = this->maxSpeeds.x;
    }
    this->moveControl->setSpeeds(speeds);
}

void Autopilot::beginRotate()
{
    if (this->state != State::SelfDrivingRotate)
    {
        this->lastState = this->state;
        this->state = State::SelfDrivingRotate;
        this->rotationAimAzimuth = this->getSensorData()->getRealAzimuth() + this->courseCorrection.correction;
        this->rotationAimAzimuth = Navigation::fixDegree(this->rotationAimAzimuth);

        DrivingSpeeds speeds = {0, 0};
        if (this->courseCorrection.correction > 0)
        {
            speeds.rot = -this->maxSpeeds.rot;
        }
        else
        {
            speeds.rot = this->maxSpeeds.rot;
        }

        this->moveControl->setSpeeds(speeds);
    }
}

void Autopilot::rotate()
{
    if (abs(this->getSensorData()->getRealAzimuth() - this->rotationAimAzimuth) < this->maxCourseDeviationBeforeAct)
    {
        this->endRotate();
        return;
    }

    if ((abs(this->getSensorData()->getRealAzimuth() - this->rotationAimAzimuth) < this->maxCourseDeviationBeforeAct * 3) &&
        ((this->courseCorrection.correction > 0 && this->rotationAimAzimuth < this->getSensorData()->getRealAzimuth()) || (this->courseCorrection.correction < 0 && this->rotationAimAzimuth > this->getSensorData()->getRealAzimuth())))
    {
        this->endRotate();
        std::cout << "Autopilot::rotate: Rover rotated too far" << std::endl;
    }
}

void Autopilot::endRotate()
{
    if (this->state != State::SelfDrivingRotate)
    {
        return;
    }

    this->state = this->lastState;
    this->sensorData->getCalcCompass()->drivingDirectionChange(this->sensorData->getCurrentPos());
    this->moveControl->setRotationSpeed(0);
    this->moveControl->emergencyStop();
    this->moveControl->setDrivingStatus(MoveControl::Status::Drive);
}

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::SelfDrivingRotate)
        {
            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:
        if (this->state == State::InsufficientAccuracy){
            break;
        }
        this->lastState = this->state;
        this->state = State::InsufficientAccuracy;
        this->moveControl->setSpeed(0);
        this->moveControl->setRotationSpeed(0);
        break;

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

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

    default:
        break;
    }
}

void Autopilot::selfDriving()
{
    if (this->state != State::SelfDriving)
    {
        return;
    }

    if (abs(this->courseCorrection.correction) >= this->maxCourseDeviationBeforeAct)
    {
        this->beginRotate();
    }
    else
    {
        this->drive();
    }
}

void Autopilot::restartLoop()
{
    this->navigation->startNavigation();
    this->state = State::SelfDriving;
    this->routeInfo = this->navigation->getRouteInfo();
    this->lastOrderStatus = this->navigation->getCourseCorrection(this->courseCorrection, true);
    this->updateDisplay = true;
}

void Autopilot::afterActivate()
{
    this->setInputMode(ManualControl::InputMode::Digital);
    this->directionChangeSignal = new DirectionChangeSignal(this);
    this->setDirectionChangeCallback(this->directionChangeSignal);
    this->navigation = new Navigation(this->sensorData);
    this->init();
}