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

ManualControl::ManualControl(MoveControl *moveControl, const ControlPadInput *input) { 
    this->moveControl = moveControl;
    this->input = input;
    this->moveControl->setDrivingStatus(DrivingStatus::drive);
}

ManualControl::~ManualControl() {
    this->moveControl->setSpeed(0);
    this->moveControl->setRotationSpeed(0);
    this->moveControl->setDrivingStatus(DrivingStatus::stop);
}

void ManualControl::loop() {
    if (millis() - this->lastMillis < delay) {
        return;
    }
    this->runManualControl();
    this->lastMillis = millis();
}

void ManualControl::runManualControl() {
    int8_t x = this->input->x;
    int8_t y = this->input->y;

    double value_per_step = this->max_speed * 2 / 256;
    this->moveControl->setSpeed((y * -1) * value_per_step);

    value_per_step = this->max_rotation * 2 / 256;
    this->moveControl->setRotationSpeed(-x * value_per_step);
}