/**
 * @file manualControl.cpp
 * @author Alexander Klein (alex@kleiax.de)
 * @brief Implementation of the class manualControl.h
 * @version 0.1
 * @date 2021-12-13
 *
 * @copyright Copyright (c) 2021
 *
 */
#include "manualControl.h"

void ManualControl::run()
{
    switch (this->inputMode)
    {
    case InputMode::Analog:
        this->analogControl();
        break;

    case InputMode::Digital:
        this->digitalControl();
        break;

    default:
        break;
    }
}

void ManualControl::switchInputMode()
{
    this->inputMode = (this->inputMode == InputMode::Analog) ? InputMode::Digital : InputMode::Analog;
}

void ManualControl::analogControl()
{
    // Have to be int16_t to avoid overflow (int8_t = 255 - 127 = -128)
    const int16_t xAxis = this->input->x - 127;
    const int16_t yAxis = this->input->y - 127;

    double value_per_step = this->maxSpeeds.x * 2 / UINT8_MAX;
    this->moveControl->setSpeed(-xAxis * value_per_step);

    value_per_step = this->maxSpeeds.rot * 2 / UINT8_MAX;
    this->moveControl->setRotationSpeed(yAxis * value_per_step);
}

void ManualControl::digitalControl()
{
    static constexpr uint8_t deadzone = 120;
    const int16_t yAxis = this->input->x - 127;
    const int16_t xAxis = this->input->y - 127;

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

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

    if (static_cast<bool>(this->directionChangeWrapper) && (xAxis > deadzone || xAxis < -deadzone))
    {
        this->lastLoopTurned = true;
    }
    else if (this->lastLoopTurned)
    {
        if (static_cast<bool>(this->directionChangeWrapper))
        {
            this->directionChangeWrapper->action();
        }
        this->lastLoopTurned = false;
    }
}