#include "car_move.h"
#include "tim.h"
#include "variables.h"

#define BTN_LEFT_FWD   0x10U
#define BTN_LEFT_REV   0x08U
#define BTN_RIGHT_FWD  0x04U
#define BTN_RIGHT_REV  0x02U

typedef enum
{
    MOTOR_MODE_BRAKE = 0,
    MOTOR_MODE_FORWARD,
    MOTOR_MODE_REVERSE
} MotorMode_t;

static uint32_t motor_pwm_max(void)
{
    return __HAL_TIM_GET_AUTORELOAD(&htim1);
}

static uint16_t decode_speed_level(uint8_t level)
{
    const uint32_t max_pwm = motor_pwm_max();

    switch (level)
    {
        case 1U: return (uint16_t)(max_pwm / 4U);
        case 2U: return (uint16_t)(max_pwm / 2U);
        case 3U: return (uint16_t)((max_pwm * 3U) / 4U);
        case 4U: return (uint16_t)(max_pwm);
        default: return 0U;
    }
}

static MotorMode_t decode_motor_mode(uint8_t ctrl, uint8_t forward_mask, uint8_t reverse_mask)
{
    const uint8_t forward = (ctrl & forward_mask) ? 1U : 0U;
    const uint8_t reverse = (ctrl & reverse_mask) ? 1U : 0U;

    if ((forward == 1U) && (reverse == 0U))
    {
        return MOTOR_MODE_FORWARD;
    }

    if ((forward == 0U) && (reverse == 1U))
    {
        return MOTOR_MODE_REVERSE;
    }

    return MOTOR_MODE_BRAKE;
}

static void set_left_motor(MotorMode_t mode, uint16_t pwm, uint32_t brake_pwm)
{
    switch (mode)
    {
        case MOTOR_MODE_FORWARD:
            __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, 0U);
            __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, pwm);
            break;

        case MOTOR_MODE_REVERSE:
            __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, pwm);
            __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, 0U);
            break;

        case MOTOR_MODE_BRAKE:
        default:
            __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, brake_pwm);
            __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, brake_pwm);
            break;
    }
}

static void set_right_motor(MotorMode_t mode, uint16_t pwm, uint32_t brake_pwm)
{
    switch (mode)
    {
        case MOTOR_MODE_FORWARD:
            __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, pwm);
            __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4, 0U);
            break;

        case MOTOR_MODE_REVERSE:
            __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, 0U);
            __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4, pwm);
            break;

        case MOTOR_MODE_BRAKE:
        default:
            __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, brake_pwm);
            __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4, brake_pwm);
            break;
    }
}

void car_move(uint8_t ctrl, uint16_t speed[2])
{
    const uint32_t max_pwm = motor_pwm_max();
    const uint8_t left_level = motor_ctrl_buffer[0];
    const uint8_t right_level = motor_ctrl_buffer[1];

    speed[0] = decode_speed_level(left_level);
    speed[1] = decode_speed_level(right_level);

    set_left_motor(decode_motor_mode(ctrl, BTN_LEFT_FWD, BTN_LEFT_REV), speed[0], max_pwm);
    set_right_motor(decode_motor_mode(ctrl, BTN_RIGHT_FWD, BTN_RIGHT_REV), speed[1], max_pwm);
}

/* ========================== Network movement command ========================== */
#define NET_MOVE_STOP      0x00U
#define NET_MOVE_FORWARD   0x11U
#define NET_MOVE_BACKWARD  0x22U
#define NET_MOVE_LEFT      0x33U
#define NET_MOVE_RIGHT     0x44U

void car_move_cmd(uint8_t move_cmd)
{
    const uint32_t max_pwm = motor_pwm_max();
    const uint16_t pwm = (uint16_t)max_pwm;

    switch (move_cmd)
    {
        case NET_MOVE_FORWARD:
            set_left_motor(MOTOR_MODE_FORWARD, pwm, max_pwm);
            set_right_motor(MOTOR_MODE_FORWARD, pwm, max_pwm);
            break;

        case NET_MOVE_BACKWARD:
            set_left_motor(MOTOR_MODE_REVERSE, pwm, max_pwm);
            set_right_motor(MOTOR_MODE_REVERSE, pwm, max_pwm);
            break;

        case NET_MOVE_LEFT:
            /* differential steering: rotate left in place */
            set_left_motor(MOTOR_MODE_REVERSE, pwm, max_pwm);
            set_right_motor(MOTOR_MODE_FORWARD, pwm, max_pwm);
            break;

        case NET_MOVE_RIGHT:
            /* differential steering: rotate right in place */
            set_left_motor(MOTOR_MODE_FORWARD, pwm, max_pwm);
            set_right_motor(MOTOR_MODE_REVERSE, pwm, max_pwm);
            break;

        case NET_MOVE_STOP:
        default:
            set_left_motor(MOTOR_MODE_BRAKE, 0U, max_pwm);
            set_right_motor(MOTOR_MODE_BRAKE, 0U, max_pwm);
            break;
    }
}
