*
 *  Created on: 2021/5/17 9:48
 *      Author: ChenWei
 *
 */

#include "stm32f1xx_hal.h"
#include "MotorDriver.h"

namespace motorDriver {

    // motor driver for the 40mm linear actuator, left and right movement
    MotorDriver::MotorDriver() :
        m_left{
            .pwmPin = GPIO_PIN_12,
            .motorPwdnPin = GPIO_PIN_13,
            .directionPin = GPIO_PIN_14,
            .speedLimitHz = 5000, // max speed limit
            .dutyRange = 1000 // duty cycle range: 0~100%
        },
        m_right{
            .pwmPin = GPIO_PIN_9,
            .motorPwdnPin = GPIO_PIN_10,
            .directionPin = GPIO_PIN_8,
            .speedLimitHz = 5000, // max speed limit
            .dutyRange = 1000 // duty cycle range: 0~100%
        } {

    }

    void MotorDriver::move(float32_t leftSpeed, float32_t rightSpeed) {
        m_left.setSpeed(leftSpeed);
        m_right.setSpeed(rightSpeed);
        // move left motor
        if (m_left.isMoving()) {
            HAL_GPIO_WritePin(m_left.pwmPin, GPIO_HIGH);
        } else {
            HAL_GPIO_WritePin(m_left.pwmPin, GPIO_LOW);
        }
        // move right motor
        if (m_right.isMoving()) {
            HAL_GPIO_WritePin(m_right.pwmPin, GPIO_HIGH);
        } else {
            HAL_GPIO_WritePin(m_right.pwmPin, GPIO_LOW);
        }
    }

    void MotorDriver::stop() {
        m_left.stop();
        m_right.stop();
        // move left motor
        if (m_left.isMoving()) {
            HAL_GPIO_WritePin(m_left.pwmPin, GPIO_HIGH);
        } else {
            HAL_GPIO_WritePin(m_left.pwmPin, GPIO_LOW);
        }
        // move right motor
        if (m_right.isMoving()) {
            HAL_GPIO_WritePin(m_right.pwmPin, GPIO_HIGH);
        } else {
            HAL_GPIO_WritePin(m_right.pwmPin, GPIO_LOW);
        }
    }

    void MotorDriver::setDirection(bool32_t direction) {
        if (direction == DIR_LEFT) {
            m_left.setDirection(DIRECTION_FORWARD);
            m_right.setDirection(DIRECTION_BACKWARD);
        } else if (direction == DIR_RIGHT) {
            m_left.setDirection(DIRECTION_BACKWARD);
            m_right.setDirection(DIRECTION_FORWARD);
        } else {
            // stop motors
            stop();
        }
    }

    bool32_t MotorDriver::isMoving() const {
        return (m_left.isMoving() || m_right.isMoving());
    }

}