/**
 * @file menuManualDrive.cpp
 * @author Alexander Klein (alex@kleiax.de)
 * @brief Contains an implementation of the class MenuManualDrive
 * @version 0.1
 * @date 2022-01-20
 * 
 * @copyright Copyright (c) 2022
 * 
 */
#include "menuManualDrive.h"

MenuManualControl::MenuManualControl(DriveManager* driveManager) : MenuDriveMode(driveManager) {
    this->caliCompass = new CalibrateCompass(this->driveManager->getNavigation()->getCompass());
}

MenuManualControl::~MenuManualControl() {
    delete this->caliCompass;
    this->manualControl->setCalibrateCompass();
}

void MenuManualControl::printPage() const {
    String lineOne = "";
    String lineTwo = "";

    switch (this->getCurrentPage()) {
        case 0:
            lineOne = "-Ready to drive-";
            break;

        case 1:
            lineOne = "Speed: ";
            lineOne.concat(this->manualControl->getMaxSpeed());
            lineTwo = "Increase by 0.1";
            break;

        case 2:
            lineOne = "Speed: ";
            lineOne.concat(this->manualControl->getMaxSpeed());
            lineTwo = "Decrease by 0.1";
            break;

        case 3:
            lineOne = "RotSpeed: ";
            lineOne.concat(this->manualControl->getMaxRotation());
            lineTwo = "Increase by 0.1";
            break;

        case 4:
            lineOne = "RotSpeed: ";
            lineOne.concat(this->manualControl->getMaxRotation());
            lineTwo = "Decrease by 0.1";
            break;

        case 5:
            lineOne = "Dutycycle Left:";
            lineTwo.concat(this->manualControl->getDutycycleLeft());
            break;

        case 6:
            lineOne = "Dutycycle Right:";
            lineTwo.concat(this->manualControl->getDutycycleRight());
            break;
        
        case 7:
            switch (this->caliCompass->getState()) {
                case CalibrateCompass::State::Ready :
                    lineOne = "Start compass";
                    lineTwo = "calibration";
                    break;

                case CalibrateCompass::State::Calibrating :
                    lineOne = "Calibrating...";
                    lineTwo = "Move around";
                    break;

                case CalibrateCompass::State::Finished:
                    lineOne = "Calibration";
                    lineTwo = "finished";
                    break;
                
                default:
                    break;
            }
            break;

        case 8:
            lineOne = "Save compass";
            lineTwo = "data in flash";
            break;

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

    this->print(lineOne, lineTwo);
}

void MenuManualControl::init() {
    this->firstPrint = false;
    this->driveManager->changeModus(Modi::ManualControl);
    this->manualControl = (ManualControl*) this->driveManager->getDriveModiPtr();
    this->setCountPages(9);
    this->updateDelay = 500; 
}

void MenuManualControl::runCommand() const {
    switch (this->getCurrentPage()) {

        case 1:
            this->manualControl->increaseMaxSpeed();
            break;

        case 2:
            this->manualControl->decreaseMaxSpeed();
            break;

        case 3:
            this->manualControl->increaseMaxRotation();
            break;

        case 4:
            this->manualControl->increaseMaxRotation();
            break;

        case 7:
            switch (this->caliCompass->getState()) {
                case CalibrateCompass::State::Ready :
                    this->manualControl->setCalibrateCompass(this->caliCompass);
                    this->caliCompass->start();
                    break;

                case CalibrateCompass::State::Finished:
                    this->manualControl->setCalibrateCompass();
                    this->caliCompass->useData();
                    this->caliCompass->reset();
                    break;
                
                default:
                    break;
            }
            break;

        case 8:
            this->caliCompass->saveData();
        
        default:
            break;
    }
}