/**
 * @file menuTestMode.cpp
 * @author Alexander Klein (alex@kleiax.de)
 * @brief Contains the implementation of the class MenuTestMode.
 * @version 0.1
 * @date 2022-09-08
 * 
 * @copyright Copyright (c) 2022
 * 
 */
#include "menuTestMode.h"

MenuTestMode::MenuTestMode(DriveManager* driveManager) {
    this->driveManager = driveManager;    
}

MenuTestMode::~MenuTestMode() {
    if (isInit) {
        delete this->mainMenu;
    }
}

void MenuTestMode::printMenu() {
    if (!this->isInit) {
        this->isInit = true;
        this->init();
    }

    this->mainMenu->printMenu();
}

void MenuTestMode::down() {
    if (this->checkInput())
        this->mainMenu->down();
}

void MenuTestMode::up() {
    if (this->checkInput())
        this->mainMenu->up();
}

void MenuTestMode::right() {
    if (this->checkInput())
        this->mainMenu->right();
}

void MenuTestMode::left() {
    if (this->mainMenu->isInSubmenu())
        if (this->checkInput())
            this->mainMenu->left();
        else {
            this->testMode->abortManeuver();
            this->mainMenu->left();
        }
    else
        this->parentMenu->printMenu();
}

void MenuTestMode::yes() {
    if (this->checkInput())
        this->mainMenu->yes();
}

void MenuTestMode::no() {
    if (this->mainMenu->isInSubmenu())
        if (this->checkInput())
            this->mainMenu->no();
        else {
            this->testMode->abortManeuver();
            this->mainMenu->no();
        }
    else
        this->left();
}

void MenuTestMode::init() {
    this->testMode = new TestMode();
    this->driveManager->changeModus(this->testMode);

    this->testMode->setSpeeds(DrivingSpeeds{1, 8});

    auto dummy = []() {
        std::cout << "Dummy in Action" <<std::endl;
    };

    // Create menu
    this->mainMenu = new Menu;
    auto engineMenu = new Menu;
    auto lightMenu = new Menu;
    auto encoderMenu = new Menu;
    // Sound
    // Ping google

    auto drivingMenu = new MenuIntInput(2, new MenuTestModeWrapper(this, &TestMode::drive));
    auto engineLeftMenu = new MenuIntInput(2, new MenuTestModeWrapper(this, &TestMode::leftEngine));
    auto engineRightMenu = new MenuIntInput(2, new MenuTestModeWrapper(this, &TestMode::rightEngine));
    auto engineBothMenu = new MenuIntInput(2, new MenuTestModeWrapper(this, &TestMode::bothEngine));
    auto encoderLeftMenu = new SpeedometerTest(this->testMode->getSpeedometerLeft());
    auto encoderRightMenu = new SpeedometerTest(this->testMode->getSpeedometerRight());


    // Set menus on LCD
    this->mainMenu->setLcd(this->lcd);
    engineMenu->setLcd(this->lcd);
    engineLeftMenu->setLcd(this->lcd);
    engineRightMenu->setLcd(this->lcd);
    engineBothMenu->setLcd(this->lcd);
    drivingMenu->setLcd(this->lcd);
    lightMenu->setLcd(this->lcd);
    encoderMenu->setLcd(this->lcd);
    encoderLeftMenu->setLcd(this->lcd);
    encoderRightMenu->setLcd(this->lcd);

    // Other menu config
    drivingMenu->setEntry(0, "Centimeter", 100);
    drivingMenu->setMinMaxSteps(0, -1000, 1000, 25);
    drivingMenu->setEntry(1, "Degree");
    drivingMenu->setMinMaxSteps(1, -360, 360, 15);
    drivingMenu->setPrintParentMenu(false);
    engineLeftMenu->setEntry(0, "Percentage");
    engineLeftMenu->setMinMaxSteps(0, -100, 100, 5);
    engineLeftMenu->setEntry(1, "Seconds", 5);
    engineLeftMenu->setMinMaxSteps(1, 0, 120, 1);
    engineLeftMenu->setPrintParentMenu(false);
    engineRightMenu->setEntry(0, "Percentage");
    engineRightMenu->setMinMaxSteps(0, -100, 100, 5);
    engineRightMenu->setEntry(1, "Seconds", 5);
    engineRightMenu->setMinMaxSteps(1, 0, 120, 1);
    engineRightMenu->setPrintParentMenu(false);
    engineBothMenu->setEntry(0, "Percentage");
    engineBothMenu->setMinMaxSteps(0, -100, 100, 5);
    engineBothMenu->setEntry(1, "Seconds", 5);
    engineBothMenu->setMinMaxSteps(1, 0, 120, 1);
    engineBothMenu->setPrintParentMenu(false);


    // Entrys for the menus
    MenuAction* engineAction = new MenuAction("Engine", engineMenu);
    MenuAction* engineLeftAction = new MenuAction("Left", engineLeftMenu);
    MenuAction* engineRightAction = new MenuAction("Right", engineRightMenu);
    MenuAction* engineBothAction = new MenuAction("Both", engineBothMenu);

    MenuAction* drivingAction = new MenuAction("Driving", drivingMenu);

    MenuAction* encoderAction = new MenuAction("Encoder", encoderMenu);
    MenuAction* encoderLeftAction = new MenuAction("Left", encoderLeftMenu);
    MenuAction* encoderRightAction = new MenuAction("Right", encoderRightMenu);

    MenuAction* lightAction = new MenuAction("Light", lightMenu);
    MenuAction* lightFlashAction = new MenuAction("Flash", dummy);
    MenuAction* lightFadeAction = new MenuAction("Fade", dummy);
    MenuAction* lightRedAction = new MenuAction("Red", dummy);
    MenuAction* lightGreenAction = new MenuAction("Green", dummy);
    MenuAction* lightBlueAction = new MenuAction("Blue", dummy);
    MenuAction* lightOffAction = new MenuAction("Off", dummy);    

    // Add entrys to the menus
    this->mainMenu->addEntry(engineAction);
    this->mainMenu->addEntry(drivingAction);
    this->mainMenu->addEntry(encoderAction);
    this->mainMenu->addEntry(lightAction);

    engineMenu->addEntry(engineLeftAction);
    engineMenu->addEntry(engineRightAction);
    engineMenu->addEntry(engineBothAction);

    encoderMenu->addEntry(encoderLeftAction);
    encoderMenu->addEntry(encoderRightAction);

    lightMenu->addEntry(lightFlashAction);
    lightMenu->addEntry(lightFadeAction);
    lightMenu->addEntry(lightRedAction);
    lightMenu->addEntry(lightGreenAction);
    lightMenu->addEntry(lightBlueAction);
    lightMenu->addEntry(lightOffAction);  
}

void MenuTestMode::update() {
    if (this->maneuverInAction)
        if (millis() - this->lastUpdateTime > updateDelay) {
            this->printManeuverTime();
            this->lastUpdateTime = millis();
        }
}

bool MenuTestMode::checkInput() {
    if (this->testMode->getBusy()) {
        this->printManeuverTime();
        this->maneuverInAction = true;
        return false;
    }

    if (this->maneuverInAction)
        this->maneuverInAction = false;
    
    return true; 
}

void MenuTestMode::printManeuverTime() {
    uint8_t maneuverTime = this->testMode->getRemainingManeuverTime();
    if (maneuverTime) {
        String lineTwo = "";
        lineTwo.concat(maneuverTime);
        lineTwo.concat(" secs");
        this->print("Maneuver time:", lineTwo);
    } else {
        this->maneuverInAction = false;
        this->printMenu();
    }
}

MenuTestModeWrapper::MenuTestModeWrapper(MenuTestMode* menu, TestModeFunctionSingle testModeFunction) {
    this->menu = menu;
    this->testMode = this->menu->getTestMode();
    this->testModeFunctionSingle = testModeFunction;
}

MenuTestModeWrapper::MenuTestModeWrapper(MenuTestMode* menu, TestModeFunctionDouble testModeFunction) {
    this->menu = menu;
    this->testMode = this->menu->getTestMode();
    this->testModeFunctionDouble = testModeFunction;
}

void MenuTestModeWrapper::action(int16_t* values, uint8_t length) {
    // This function calls a member function with a pointer
    // Very helpful site:
    // https://isocpp.org/wiki/faq/pointers-to-members#fnptr-vs-memfnptr-types

    bool res = false;

    if (length == 2 && this->testModeFunctionDouble)
        res = (this->testMode->*this->testModeFunctionDouble)(values[0], values[1]);
    else if (length == 1 && this->testModeFunctionSingle)
        res = (this->testMode->*this->testModeFunctionSingle)(values[0]);

    if (res) {
        this->menu->printManeuverTime();
        this->menu->maneuverStarted();
    }
}