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

TestMode::TestMode(MoveControl *moveControl,  Navigation* navigation) {
    this->moveControl = moveControl;
    this->navigation = navigation;
}

TestMode::~TestMode() {
    this->moveControl->setDrivingStatus(DrivingStatus::stop);
}

void TestMode::loop() {
    if (millis() - this->lastMillis < this->delay)
        return;

    if (this->maneuver == Maneuver::Turn) {
        uint16_t delta = abs(this->azimuth - this->navigation->getAzimuth());
        if (delta > this->degree)
            this->abort = true;
    }

    if (millis() - this->actionStart > this->maneuverTime || this->abort) {
        this->busy = false;
        this->abort = false;
        this->moveControl->setDrivingStatus(DrivingStatus::stop);
        this->maneuver = Maneuver::None;
    }

    this->lastMillis = millis();
}

void TestMode::setSpeed(int16_t speed) {
    this->speed = (double) speed / 100.0;
}

void TestMode::setRotationSpeed(int16_t speed) {
    this->rotationSpeed = (double) speed / 100.0;
}

bool TestMode::drive(int16_t cm, int16_t degree) {
    if (this->busy)
        return false;

    this->actionStart = millis();
    this->moveControl->setDrivingStatus(DrivingStatus::drive);

    if (cm == 0) {
        //Only left or right
        this->moveControl->setSpeed(0);

        if (degree < 0)
            this->moveControl->setRotationSpeed(-this->rotationSpeed);
        else if (degree > 0)
            this->moveControl->setRotationSpeed(this->rotationSpeed);
        
        this->azimuth = this->navigation->getAzimuth();
        this->degree = degree;

        this->maneuverTime = 5 * 1000;
        this->busy = true;
        return true;

    } else if (degree == 0) {
        //Only forward or backward
        this->moveControl->setRotationSpeed(0);

        if (cm < 0)
            this->moveControl->setSpeed(-this->speed);
        else if (cm > 0)
            this->moveControl->setSpeed(this->speed);
        
        this->maneuverTime = (uint32_t) (((double) cm / 100.0) / this->speed) * 1000;
        this->busy = true;
        this->maneuver = Maneuver::Drive;
        return true;
    } else {
        //forward or backward and left or right
        // TODO: Calculate roationspeed
        if (cm < 0)
            this->moveControl->setSpeed(-this->speed);
        else if (cm > 0)
            this->moveControl->setSpeed(this->speed);
        
        this->maneuverTime = (uint32_t) (((double) cm / 100.0) / this->speed) * 1000;
        this->busy = true;
        this->maneuver = Maneuver::Drive;
        return true;
    }

    this->moveControl->setDrivingStatus(DrivingStatus::stop);    
    return false;
}

bool TestMode::leftEngine(int16_t powerPercentage, int16_t seconds) {
    if (this->engineInit(powerPercentage, seconds)) {
        this->moveControl->setRawPowerLeft(powerPercentage);
        this->maneuver = Maneuver::LeftEngine;
        return true;
    }
    return false;
}

bool TestMode::rightEngine(int16_t powerPercentage, int16_t seconds) {
    if (this->engineInit(powerPercentage, seconds)) {
        this->moveControl->setRawPowerRight(powerPercentage);
        this->maneuver = Maneuver::RightEngine;
        return true;
    }
    return false;
}

bool TestMode::bothEngine(int16_t powerPercentage, int16_t seconds) {
    if (this->engineInit(powerPercentage, seconds)) {
        this->moveControl->setRawPowerLeft(powerPercentage);
        this->moveControl->setRawPowerRight(powerPercentage);
        this->maneuver = Maneuver::BothEngine;
        return true;
    }
    return false;
}

void TestMode::abortManeuver() {
    this->abort = true;
}

uint8_t TestMode::getRemainingManeuverTime() {
    if (busy)
        return (uint8_t) ((this->maneuverTime - (millis() - this->actionStart)) / 1000);
    return 0;
}

bool TestMode::engineInit(int16_t powerPercentage, int16_t seconds) {
    if (this->busy)
        return false;
    if (powerPercentage >= 100 || powerPercentage <= -100)
        return false;
    if (seconds < 0)
        return false;

    this->actionStart = millis();
    this->moveControl->setDrivingStatus(DrivingStatus::raw);
    this->maneuverTime = seconds * 1000;
    this->busy = true;
    return true;
}