/**
 * @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"

void TestMode::run()
{
    if (this->maneuver == Maneuver::Turn)
    {
        uint16_t delta = abs(this->azimuth - this->getSensorData()->getRealAzimuth());
        if (delta > this->degree)
        {
            this->abort = true;
        }
    }

    if (this->busy && millis() - this->actionStart > this->maneuverTime || this->abort)
    {
        this->busy = false;
        this->abort = false;
        this->moveControl->setDrivingStatus(MoveControl::Status::Stop);
        this->maneuver = Maneuver::None;
    }
}

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

    this->actionStart = millis();
    this->moveControl->setDrivingStatus(MoveControl::Status::Drive);

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

        if (degree < 0)
        {
            this->moveControl->setRotationSpeed(-this->maxSpeeds.rot);
        }
        else if (degree > 0)
        {
            this->moveControl->setRotationSpeed(this->maxSpeeds.rot);
        }

        this->azimuth = this->getSensorData()->getRealAzimuth();
        this->degree = degree;

        this->maneuverTime = 5 * 1000;
        this->busy = true;
        return true;
    }
    if (degree == 0)
    {
        // Only forward or backward
        this->moveControl->setRotationSpeed(0);

        if (cmDistance < 0)
        {
            this->moveControl->setSpeed(-this->maxSpeeds.x);
        }
        else if (cmDistance > 0)
        {
            this->moveControl->setSpeed(this->maxSpeeds.x);
        }

        this->maneuverTime = static_cast<uint32_t>(((abs(cmDistance) / 100.0) / this->maxSpeeds.x) * 1000);
        this->busy = true;
        this->maneuver = Maneuver::Drive;
        return true;
    }
    else
    {
        if (cmDistance < 0)
        {
            this->moveControl->setSpeed(-this->maxSpeeds.x);
        }
        else if (cmDistance > 0)
        {
            this->moveControl->setSpeed(this->maxSpeeds.x);
        }

        this->maneuverTime = static_cast<uint32_t>(((cmDistance / 100.0) / this->maxSpeeds.x) * 1000);
        this->busy = true;
        this->maneuver = Maneuver::Drive;
        return true;
    }

    this->moveControl->setDrivingStatus(MoveControl::Status::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;
        std::cout << "TestMode::leftEngine" << std::endl;
        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() const
{
    if (this->busy)
    {
        return static_cast<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 > TestMode::maxPercentage || powerPercentage < -TestMode::maxPercentage)
    {
        return false;
    }
    if (seconds < 0)
    {
        return false;
    }

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