#include "../2025_include_user/Motor.h"

void Motor::init() {}

void Motor::setLeftSpeed(const int left_speed) {
    setLeftDirection(left_speed > 0);
    const int pwmA_abs = Abs(left_speed);
    DL_TimerA_setCaptureCompareValue(Motor_INST, pwmA_abs, GPIO_Motor_C0_IDX);
}

void Motor::setRightSpeed(const int right_speed) {
    setRightDirection(right_speed > 0);
    const int pwmB_abs = Abs(right_speed);
    DL_TimerA_setCaptureCompareValue(Motor_INST, pwmB_abs, GPIO_Motor_C1_IDX);
}

void Motor::setSpeed(const int left_speed, const int right_speed) {
    setLeftSpeed(left_speed);
    setRightSpeed(right_speed);
}

void Motor::setLeftDirection(const uint8_t direction) {
    if (direction) {
        DL_GPIO_setPins(MotorDirection_PORT, MotorDirection_AIN1_PIN);
        DL_GPIO_clearPins(MotorDirection_PORT, MotorDirection_AIN2_PIN);
    } else {
        DL_GPIO_setPins(MotorDirection_PORT, MotorDirection_AIN2_PIN);
        DL_GPIO_clearPins(MotorDirection_PORT, MotorDirection_AIN1_PIN);
    }
}

void Motor::setRightDirection(const uint8_t direction) {
    if (direction) {
        DL_GPIO_setPins(MotorDirection_PORT, MotorDirection_BIN1_PIN);
        DL_GPIO_clearPins(MotorDirection_PORT, MotorDirection_BIN2_PIN);
    } else {
        DL_GPIO_setPins(MotorDirection_PORT, MotorDirection_BIN2_PIN);
        DL_GPIO_clearPins(MotorDirection_PORT, MotorDirection_BIN1_PIN);
    }
}

void Motor::setDirection(const uint8_t left_direction, const uint8_t right_direction) {
    setLeftDirection(left_direction);
    setRightDirection(right_direction);
}

void Motor::stopLeft() {
    setLeftSpeed(0);
    DL_GPIO_setPins(MotorDirection_PORT, MotorDirection_AIN1_PIN);
    DL_GPIO_setPins(MotorDirection_PORT, MotorDirection_AIN2_PIN);
}

void Motor::stopRight() {
    setRightSpeed(0);
    DL_GPIO_setPins(MotorDirection_PORT, MotorDirection_BIN1_PIN);
    DL_GPIO_setPins(MotorDirection_PORT, MotorDirection_BIN2_PIN);
}

void Motor::stop() {
    stopLeft();
    stopRight();
}

int Motor::Abs(const int num) {
    return num < 0 ? -num : num; // 取绝对值
}