/**
 * @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)
    int16_t x = this->input->x - 127;
    int16_t y = this->input->y - 127;

    // static int counter = 0;
    // if (counter % 60 == 0) {
    //     std::cout << "Input: x: " << (int) this->input->x << " y: " << (int) this->input->y << std::endl;
    //     std::cout << "Output: x: " << (int) x << " y: " << (int) y << std::endl;
    // }
    // counter++;

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

    value_per_step = this->maxRotationSpeed * 2 / UINT8_MAX;
    this->moveControl->setRotationSpeed(y * value_per_step);
}

void ManualControl::digitalControl() {
    int16_t y = this->input->x - 127;
    int16_t x = this->input->y - 127;

    if (y > 120)
        this->moveControl->setSpeed(-this->maxForwardSpeed);
    else if (y < -120)
        this->moveControl->setSpeed(this->maxForwardSpeed);
    else
        this->moveControl->setSpeed(0);

    if (x > 120)
        this->moveControl->setRotationSpeed(this->maxRotationSpeed);
    else if (x < -120)
        this->moveControl->setRotationSpeed(-this->maxRotationSpeed);
    else
        this->moveControl->setRotationSpeed(0);

    if (this->directionChangeWrapper && (x > 120 || x < -120))
        this->lastLoopTurned = true;
    else if (this->lastLoopTurned) {
        if (this->directionChangeWrapper)
            this->directionChangeWrapper->action();
        this->lastLoopTurned = false;
    }      
}