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

#include "Motion.h"

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

// Initialize the movement component.
void Motion::begin(void) {
    ledc_timer_config_t motor_timer = {
        .speed_mode       = LEDC_MODE,
        .duty_resolution  = DUTY_RES,
        .timer_num        = TIMER,
        .freq_hz          = FREQUENCY,  
        .clk_cfg          = LEDC_AUTO_CLK
    }; 
    ledc_timer_config(&motor_timer);
    Motion::left.begin();
    Motion::right.begin();
};
void Motion::moveTask(void * args) {
    Motion::left.setSpeed(LEFT_MOTOR_DUTY);
    Motion::right.setSpeed(RIGHT_MOTOR_DUTY);
    vTaskDelay((uint32_t) args / portTICK_PERIOD_MS);
    Motion::left.setSpeed(0);
    Motion::right.setSpeed(0);
    vTaskDelete(xMoveTaskHandle);
};

// Move forward for a certain amount of time.
void Motion::move(uint32_t moveForMs) {
    if (moveForMs > 0){
    xTaskCreate(moveTask, "Move", 4096, (void*)moveForMs, 10, &xMoveTaskHandle);
    } else{
        Motion::left.setSpeed(LEFT_MOTOR_DUTY);
        Motion::right.setSpeed(RIGHT_MOTOR_DUTY);
    }
};

void Motion::leftMotorTask(void * args) {
    Motion::left.setSpeed(LEFT_MOTOR_DUTY);
    Motion::right.setSpeed(0);
    vTaskDelay((uint32_t) args / portTICK_PERIOD_MS);
    Motion::left.setSpeed(0);
    vTaskDelete(xClockwiseTaskHandle);
};

// Rotate clockwise for a certain amount of time.
void Motion::rotateClockwise(uint32_t rotateForMs) {
    if (rotateForMs > 0){
        xTaskCreate(leftMotorTask, "LeftMotor", 4096, (void*)rotateForMs, 10, &xClockwiseTaskHandle);
    } else {
        Motion::left.setSpeed(LEFT_MOTOR_DUTY);
        Motion::right.setSpeed(0);
    }
};

void Motion::rightMotorTask(void * args) {
    Motion::right.setSpeed(RIGHT_MOTOR_DUTY);
    Motion::left.setSpeed(0);
    vTaskDelay((uint32_t) args / portTICK_PERIOD_MS);
    Motion::right.setSpeed(0);
    vTaskDelete(xAntiClockwiseTaskHandle);
};

// Rotate anticlockwise for a certain amount of time.
void Motion::rotateAntiClockwise(uint32_t rotateForMs) {
    if(rotateForMs > 0){
        xTaskCreate(rightMotorTask, "RightMotor", 4096, (void*)rotateForMs, 10, &xAntiClockwiseTaskHandle);
    } else {
        Motion::right.setSpeed(RIGHT_MOTOR_DUTY);
        Motion::left.setSpeed(0);
    }
};

void Motion::stop(void){
    Motion::left.setSpeed(0);
    Motion::right.setSpeed(0);
}
 
