/**
 * @file Motion.cpp
 * @author Jonathan Schulze, Nick Hübenthal
 * @brief Implementation of the Motion class.
 * @version 0.1
 * @date 2023-12-13
 * 
 * @copyright Copyright (c) 2023
 * 
 */

#include "Motion.h"

TaskHandle_t xMoveTaskHandle = NULL;
TaskHandle_t xClockwiseTaskHandle = NULL;
TaskHandle_t xAntiClockwiseTaskHandle = NULL;

// Constructor
Motion::Motion() {
    
}

// Initialize the movement component.
void Motion::begin(void) {
    
}
void moveTask(void * args) {
    analogWrite(MOTOR_LEFT_PIN, 128);
    analogWrite(MOTOR_RIGHT_PIN, 128);
    vTaskDelay((uint32_t) args / portTICK_PERIOD_MS);
    analogWrite(MOTOR_LEFT_PIN, 0);
    analogWrite(MOTOR_RIGHT_PIN, 0);
    vTaskDelete(xMoveTaskHandle);
}

// Move forward for a certain amount of time.
void Motion::move(uint32_t moveForMs) {
    xTaskCreate(moveTask, "Move", 4096, (void*)moveForMs, 10, &xMoveTaskHandle);
}

void leftMotorTask(void * args) {
    analogWrite(MOTOR_LEFT_PIN, 128);
    vTaskDelay((uint32_t) args / portTICK_PERIOD_MS);
    analogWrite(MOTOR_LEFT_PIN, 0);
    vTaskDelete(xClockwiseTaskHandle);
}

// Rotate clockwise for a certain amount of time.
void Motion::rotateClockwise(uint32_t rotateForMs) {
    xTaskCreate(leftMotorTask, "LeftMotor", 4096, (void*)rotateForMs, 10, &xClockwiseTaskHandle);
}

void rightMotorTask(void * args) {
    analogWrite(MOTOR_RIGHT_PIN, 128);
    vTaskDelay((uint32_t) args / portTICK_PERIOD_MS);
    analogWrite(MOTOR_RIGHT_PIN, 0);
    vTaskDelete(xAntiClockwiseTaskHandle);
}

// Rotate anticlockwise for a certain amount of time.
void Motion::rotateAnticlockwise(uint32_t rotateForMs) {
    xTaskCreate(rightMotorTask, "RightMotor", 4096, (void*)rotateForMs, 10, &xAntiClockwiseTaskHandle);
}