/**
 * @file menuCalibrateCompass.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 "menuCalibrateCompass.h"

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

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

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

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

        case 1:
            lineOne = "Azimuth:";
            lineTwo.concat(this->driveManager->getNavigation()->getAzimuth());
            break;        
        
        case 2:
            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 3:
            lineOne = "Save compass";
            lineTwo = "data in flash";
            break;

        case 4:
            lineOne = "Load data";
            lineTwo = "from flash";
            break;

        case 5:
            lineOne = "Remove";
            lineTwo = "calibration data";
            break;

        case 6:
            lineOne = "Use current";
            lineTwo = "calibration data";
            break;

        case 7:
            lineOne = "X min: ";
            lineOne.concat(this->caliCompass->getCallibrationData().data[0][0]);
            lineTwo = "X max: ";
            lineTwo.concat(this->caliCompass->getCallibrationData().data[0][1]);
            break;

        case 8:
            lineOne = "Y min: ";
            lineOne.concat(this->caliCompass->getCallibrationData().data[1][0]);
            lineTwo = "Y max: ";
            lineTwo.concat(this->caliCompass->getCallibrationData().data[1][1]);
            break;

        case 9:
            lineOne = "Z min: ";
            lineOne.concat(this->caliCompass->getCallibrationData().data[2][0]);
            lineTwo = "Z max: ";
            lineTwo.concat(this->caliCompass->getCallibrationData().data[2][1]);
            break;

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

    this->print(lineOne, lineTwo);
}

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

void MenuCalibrateCompass::runCommand() const {
    switch (this->getCurrentPage()) {
        case 2:
            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 3:
            this->caliCompass->saveData();
            break;

        case 4:
            this->caliCompass->loadData();
            break;

        case 5:
            this->caliCompass->removeCalibration();
            break;

        case 6:
            this->caliCompass->useData();
            break;        
        
        default:
            break;
    }
}