#include "motor.h"
#include <ch.h>
#include <hal.h>

volatile fix16_t motor_speed;
volatile fix16_t servo_position;

#define MOTOR_PWM_MAX 600

const PWMConfig pwmconfig_tim4 = {
    12000000, // 12 MHz timer, 600 ticks per period => 20kHz PWM
    MOTOR_PWM_MAX,
    NULL,
    {{1, NULL}, {1, NULL}}
};

int32_t servo_center = 1600;
int32_t servo_delta = 400;

const PWMConfig pwmconfig_tim3 = {
    1000000, // 1MHz timer, 10000 ticks per period => 100 Hz servo pulse
    10000,
    NULL,
    {{1, NULL}}
};

void motor_init()
{
    pwmStart(&PWMD4, &pwmconfig_tim4);
    pwmEnableChannel(&PWMD4, 0, MOTOR_PWM_MAX);
    pwmEnableChannel(&PWMD4, 1, MOTOR_PWM_MAX);
    
    pwmStart(&PWMD3, &pwmconfig_tim3);
    pwmEnableChannel(&PWMD3, 0, servo_center);
    
    palSetPad(GPIOB, GPIOB_SERVO_PWR);
}

void set_motor_speed(fix16_t speed)
{
    if (speed > fix16_one)
        speed = fix16_one;
    
    if (speed < -fix16_one)
        speed = -fix16_one;
    
    if (speed > 0)
    {
        pwmEnableChannel(&PWMD4, 0, MOTOR_PWM_MAX);
        pwmEnableChannel(&PWMD4, 1, MOTOR_PWM_MAX - fix16_mul(MOTOR_PWM_MAX, speed));
    }
    else
    {
        pwmEnableChannel(&PWMD4, 0, MOTOR_PWM_MAX - fix16_mul(MOTOR_PWM_MAX, -speed));
        pwmEnableChannel(&PWMD4, 1, MOTOR_PWM_MAX);
    }
    
    chSysLock();
    motor_speed = speed;
    chSysUnlock();
}

void set_servo_position(fix16_t position)
{
    if (position > fix16_one)
        position = fix16_one;
    
    if (position < -fix16_one)
        position = -fix16_one;
    
    pwmEnableChannel(&PWMD3, 0, servo_center + fix16_mul(servo_delta, position));
    
    chSysLock();
    servo_position = position;
    chSysUnlock();
}

void set_servo_power(bool on)
{
    if (on)
        palSetPad(GPIOB, GPIOB_SERVO_PWR);
    else
        palClearPad(GPIOB, GPIOB_SERVO_PWR);
}