/**
 * @file driveManager.cpp
 * @author Alexander Klein (alex@kleiax.de)
 * @brief Implemention of the class driveManager.h.
 * @version 0.1
 * @date 2021-12-14
 * 
 * @copyright Copyright (c) 2021
 * 
 */

#include "driveModi/driveManager.h"

DriveManager::DriveManager(MoveControl *moveControl) {
    this->moveControl = moveControl;
}

void DriveManager::loop() {
    this->moveControl->loop();
    if (currentModus)
        this->currentModus->loop();
}

void DriveManager::runDriveManager() {
    
}

void DriveManager::nextModus() {
    
}

void DriveManager::changeModus(Modi modus) {

    //Set moveControl to a safe state
    delete this->currentModus;

    this->moveControl->setSpeed(0);
    this->moveControl->setRotationspeed(0);
    this->moveControl->setDrivingStatus(DrivingStatus::stop);
    
    switch (modus) {
        case Modi::Off:
            this->currentModus = nullptr;
            break;

        case Modi::ManualControl: {
            ManualControl *ptr = new ManualControl;
            ptr->init(this->moveControl);
            this->currentModus = ptr;
            }
            break;

        case Modi::CaptureRoute:
            this->currentModus = nullptr;
            break;

        case Modi::Autopilot:
            this->currentModus = nullptr;
            break;

        case Modi::ConsolControl:
            this->currentModus = nullptr;
            break;

        case Modi::TestMode:
            this->currentModus = nullptr;
            break;
        
        default:
            this->currentModus = nullptr;
            break;
    }
}