#include "motor.h"

void Motor_Init(Motor *motor, TIM_HandleTypeDef *htim, uint32_t channel,
                GPIO_TypeDef *port1, uint16_t pin1,
                GPIO_TypeDef *port2, uint16_t pin2)
{
    motor->htim_pwm = htim;
    motor->channel_pwm = channel;
    motor->port_in1 = port1;
    motor->pin_in1 = pin1;
    motor->port_in2 = port2;
    motor->pin_in2 = pin2;
    motor->dir = MOTOR_STOP;
    motor->pwm_value = 0;

    /* Start PWM */
    HAL_TIM_PWM_Start(htim, channel);
}

void Motor_SetSpeed(Motor *motor, int16_t speed)
{
    /* Clamp */
    if (speed > 0) {
        motor->dir = MOTOR_FORWARD;
        HAL_GPIO_WritePin(motor->port_in1, motor->pin_in1, GPIO_PIN_SET);
        HAL_GPIO_WritePin(motor->port_in2, motor->pin_in2, GPIO_PIN_RESET);
    } else if (speed < 0) {
        motor->dir = MOTOR_BACKWARD;
        speed = -speed;
        HAL_GPIO_WritePin(motor->port_in1, motor->pin_in1, GPIO_PIN_RESET);
        HAL_GPIO_WritePin(motor->port_in2, motor->pin_in2, GPIO_PIN_SET);
    } else {
        motor->dir = MOTOR_STOP;
        HAL_GPIO_WritePin(motor->port_in1, motor->pin_in1, GPIO_PIN_RESET);
        HAL_GPIO_WritePin(motor->port_in2, motor->pin_in2, GPIO_PIN_RESET);
    }

    __HAL_TIM_SET_COMPARE(motor->htim_pwm, motor->channel_pwm, speed);
    motor->pwm_value = speed;
}

void Motor_SetSpeedScaled(Motor *motor, int16_t speed)
{
    if (speed > 1000)  speed = 1000;
    if (speed < -1000) speed = -1000;

    /* Map from -1000..1000 to hardware PWM period (e.g. 0..1000 for 1kHz) */
    int16_t pwm = (speed > 0) ? speed : -speed;

    Motor_SetSpeed(motor, (speed >= 0) ? pwm : -pwm);
}

void Motor_Stop(Motor *motor)
{
    Motor_SetSpeed(motor, 0);
}