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

#include "menuAutopilot.h"

void MenuAutopilot::printPage() const {
    CourseCorrection correction = this->autopilot->getCourseCorrection();
    UBX_NAV_PVT_data_t* gpsData = this->driveManager->getNavigation()->getUbxData();
    bool navigationStarted = this->autopilot->getState() >= Autopilot::State::NavigationStarted;

    String distanceString = "";
    if (navigationStarted) {
        uint16_t distance = 0;
        if (correction.distance < 1) {
            distance = (uint16_t) (correction.distance * 100);
            distanceString.concat(distance);
            distanceString.concat("cm");
        } else if (correction.distance < 1000) {
            distance = (uint16_t) correction.distance;
            distanceString.concat(distance);
            distanceString.concat("m");
        } else {
            distance = (uint16_t) (correction.distance / 1000);
            distanceString.concat(distance);
            distanceString.concat("km");
        }
    }

    String lineOne = "No information";
    String lineTwo = "available";

    switch (this->getCurrentPage()) {
        case 0: {
            lineOne = "Status: ";
            lineTwo = "";
            switch (this->autopilot->getState()) {
                case Autopilot::State::InsufficientAccuarcy :
                    lineTwo = "Err: LowAccuracy";
                    break;

                case Autopilot::State::NoRoute :
                    lineTwo = "Err: No route";
                    break;

                case Autopilot::State::NavigationStarted :
                    lineTwo = "Nav started";
                    break;

                case Autopilot::State::GetToStartPoint :
                    lineTwo = "Drive to start";
                    break;

                case Autopilot::State::SelfDrivingAvailable :
                    lineTwo = "Autopilot ready";
                    break;

                case Autopilot::State::SelfDriving :
                    lineTwo = "Autopilot active";
                    break;

                case Autopilot::State::TargetReached :
                    lineTwo = "Target reached";
                    break;
                
                default:
                    lineTwo = "UNKOWN";
                    break;
            }
            break;
        }

        case 1:
            if (!navigationStarted)
                break;
            lineOne = "Distance: ";
            lineOne.concat(distanceString);
            lineTwo = "Turn: ";
            lineTwo.concat(correction.correction);
            break;

        case 2:
            lineOne = "Target waypoint";
            lineTwo = "";
            lineTwo.concat(this->autopilot->getRouteInfo().currentPoint);
            lineTwo.concat(" from ");
            lineTwo.concat(this->autopilot->getRouteInfo().totalPoints);
            break;

        case 3: {
            NTRIPClientStates status = this->driveManager->getNavigation()->getNTRIPClient()->getClientState();
            lineOne = "NTRIP Client is";

            if (status ==  NTRIPClientStates::pushData)
                lineTwo = "enabled";
            else if (status == NTRIPClientStates::notAvailable)
                lineTwo = "not available";
            else
                lineTwo = "disabled";
            break;
        }

        case 4: {
            lineOne = "Carrier Solution";
            uint8_t carrSoln = gpsData->flags.bits.carrSoln;
            if (carrSoln == 0)
                lineTwo = "None";
            else if (carrSoln == 1)
                lineTwo = "Floating";
            else if (carrSoln == 2)
                lineTwo = "Fixed";
            else
                lineTwo = "UNKNOWN";
            break;
        }

        case 5: 
            lineOne = "hAccuracy: ";
            lineTwo = "Azimuth: ";
            lineTwo.concat(this->driveManager->getNavigation()->getAzimuth());
            if (gpsData->fixType)
                lineOne.concat(gpsData->hAcc);
            else
                lineOne.concat("0");
            break;

        case 6:
            lineOne = "Restart the";
            lineTwo = "Autopilot";
            break;

        case 7:
            lineOne = "Freeze target is";
            if (this->targetFreezed)
                lineTwo = "activated";
            else
                lineTwo = "deactivated";
            break;

        case 8:
            lineOne = "MinDisToPoint:";
            lineTwo = "";
            lineTwo.concat(this->minDistance);
            lineTwo.concat(" - Increase");
            break;

        case 9:
            lineOne = "MinDisToPoint:";
            lineTwo = "";
            lineTwo.concat(this->minDistance);
            lineTwo.concat(" - Decrease");
            break;

        case 10:
            lineOne = "Current minimal";
            if (this->driveManager->getNavigation()->getMinAccuracy() >= Point::Accuracy::twoDigOfCM)
                lineTwo = "accuracy is high";
            else 
                lineTwo = "accuracy is low";
            break;

        default:
            this->printDefault();
            return;
    }

    this->print(lineOne, lineTwo);
}

void MenuAutopilot::runCommand() const {
    switch (this->getCurrentPage()) {
        case 6:
            this->autopilot->restart();
            break;

        case 7:
            if (this->targetFreezed) {
                this->targetFreezed = false;
                this->driveManager->getNavigation()->freezeTargetPoint(false);
            } else {
                this->targetFreezed = true;
                this->driveManager->getNavigation()->freezeTargetPoint();
            }
            break;

        case 8:
            this->minDistance = this->driveManager->getNavigation()->increaseMinDistanceToReachPoint();
            break;

        case 9:
            this->minDistance = this->driveManager->getNavigation()->decreaseMinDistanceToReachPoint();
            break;

        case 10:
            if (this->driveManager->getNavigation()->getMinAccuracy() >= Point::Accuracy::twoDigOfCM)
                this->driveManager->getNavigation()->setMinAccuracy(Point::Accuracy::none);
            else 
                this->driveManager->getNavigation()->setMinAccuracy(Point::Accuracy::twoDigOfCM);
            break;
        
        default:
            break;
    }
}

void MenuAutopilot::update() {
    if (!this->autopilot->shouldUpdate())
        return;

    this->routeInfo = this->autopilot->getRouteInfo();
    this->printMenu();
}

void MenuAutopilot::init() {
    this->setCountPages(11);
    this->driveManager->changeModus(Modi::Autopilot);
    this->autopilot = (Autopilot*) this->driveManager->getDriveModiPtr();
    this->routeInfo = this->autopilot->getRouteInfo();
    this->driveManager->getNavigation()->increaseMinDistanceToReachPoint();
    this->minDistance = this->driveManager->getNavigation()->decreaseMinDistanceToReachPoint();
}