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

#include "driveModi/driveManager.h"

Modi& operator++(Modi& m, int) {
    return m = (m == Modi::TestMode) ? Modi::Off : static_cast<Modi>(static_cast<int>(m)+1);
}

DriveManager::DriveManager(MoveControl *moveControl, SPIClass *spiPort, const ControlPadInput *input, bool wifi) {
    this->moveControl = moveControl;
    this->input = input;

    this->spiPort = spiPort;
    this->navigation = new Navigation(spiPort, UBLOX_GNSS_SPI_CS);
    if (wifi)
        this->navigation->initNtrip(NTRIP_HOST, NTRIP_PORT, NTRIP_MOUNT_POINT, NTRIP_USER, NTRIP_PASSWORD);
    // if (wifi)
    //     std::cout << "Wifi is true" << std::endl;
}

DriveManager::~DriveManager() {
    delete this->navigation;
    delete this->spiPort;
}

void DriveManager::loop() {
    DebugTimes moveControlTime;
    this->moveControl->loop();
    moveControlTime.stopConsol("MoveControlTime", 5);

    DebugTimes navigationTime;
    this->navigation->loop();
    navigationTime.stopConsol("NavigationTime", 20);

    if (currentModusPtr)
        this->currentModusPtr->loop();
}

void DriveManager::nextModus() {
    changeModus(this->currentModus++);
}

void DriveManager::changeModus(Modi modus) {
    this->currentModus = modus;

    delete this->currentModusPtr;
    //Reset Route to first point
    this->navigation->startNavigation();

    //Set moveControl to a safe state
    this->moveControl->setSpeed(0);
    this->moveControl->setRotationSpeed(0);
    this->moveControl->setDrivingStatus(MoveControl::Status::Stop);
    
    switch (modus) {
        case Modi::Off:
            this->currentModusPtr = nullptr;
            break;

        case Modi::ManualControl: {
            this->currentModusPtr = new ManualControl(this->moveControl, this->input);
            }
            break;

        case Modi::CaptureRoute: {
            this->currentModusPtr = new CaptureRoute(this->moveControl, this->input,  this->navigation);
            }
            break;

        case Modi::Autopilot: {
            this->currentModusPtr = new Autopilot(this->moveControl, this->input, this->navigation);
            }
            break;

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

        case Modi::TestMode: {
                this->currentModusPtr = new TestMode(this->moveControl,  this->navigation);
            }
            break;
        
        default:
            this->currentModusPtr = nullptr;
            break;
    }
}