/**
 * @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::printMenu() {
    if (this->firstPrint)
        this->init();

    char buf1[17];
    char buf2[17];
    char unit[3];

    bool navigationStarted = this->autopilot->getNavigationStarted();
    bool navigationEnded = this->autopilot->getNavigationEnded();
    bool selfDriving = this->autopilot->getSelfDriving();
    bool selfDrivingAvailable = this->autopilot->getSelfDrivingAvailable();

    // 999m -360°
    CourseCorrection correction = this->autopilot->getCourseCorrection();
    uint16_t distance = 0;

    if (correction.distance < 1) {
        distance = (uint16_t) (correction.distance * 100);
        sprintf(unit, "cm");
    } else if (correction.distance < 1000) {
        distance = (uint16_t) correction.distance;
        sprintf(unit, "m");
    } else {
        distance = (uint16_t) (correction.distance / 1000);
        sprintf(unit, "km");
    }

    if (navigationStarted) {
        sprintf(buf1 ,"Points %u/%u",this->routeInfo.currentPoint ,this->routeInfo.totalPoints);
        if (selfDriving) {
            sprintf(buf2, "Auto: %u%s %d", distance, unit, correction.correction);
        } else {
            if (selfDrivingAvailable) {
                sprintf(buf2, "Self -> Auto");
            } else {
                sprintf(buf2, "Self: %u%s %d", distance, unit, correction.correction);
            }
        }
    } else {
        if (navigationEnded) {
            sprintf(buf1, "Am Ziel");
            sprintf(buf2, "angekommen.");
        } else {
            sprintf(buf1, "Keine Route");
            sprintf(buf2, "vorhanden.");
        }
    } 

        if (this->lcd) {
            this->lcd->clear();
            this->lcd->setCursor(0, 0);
            this->lcd->print(buf1);
            this->lcd->setCursor(0, 1);
            this->lcd->print(buf2);

        }
        std::cout << buf1 << " " << buf2 << std::endl;
}

void MenuAutopilot::update() {
    if (millis() - this->lastMillis < this->minDelay)
        return;
    this->lastMillis = millis();

    if (!this->autopilot->shouldUpdate())
        return;

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

void MenuAutopilot::init() {
    this->firstPrint = false;
    this->driveManager->changeModus(Modi::Autopilot);
    this->autopilot = (Autopilot*) this->driveManager->getDriveModiPtr();
    this->routeInfo = this->autopilot->getRouteInfo();
}