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

#define MOTOR_PWM_TICKS 600
#define MOTOR_PWM_MAX 400

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

#include <chprintf.h>
#include "usb_usart.h"

static int s_angle;

void motor_init()
{
    pwmStart(&PWMD4, &pwmconfig_tim4);
    pwmEnableChannel(&PWMD4, 0, 0);
    pwmEnableChannel(&PWMD4, 1, 0);
    pwmEnableChannel(&PWMD4, 2, 0);
}

void motor_set_phases(int p1, int p2, int p3)
{
    pwmEnableChannel(&PWMD4, 0, p1 * MOTOR_PWM_MAX / 100);
    pwmEnableChannel(&PWMD4, 1, p2 * MOTOR_PWM_MAX / 100);
    pwmEnableChannel(&PWMD4, 2, p3 * MOTOR_PWM_MAX / 100);
}

static int get_phase_pwm(int angle, int phase)
{
    int delta = (angle * 4 + 720 - phase * 120) % 360;
    
    if (delta > 180)
        delta = 360 - delta;
    
    if (delta >= 120)
        return 0;
    
    return (120 - delta) * 100 / 120;
}

void motor_goto(int angle)
{
    angle = angle % 360;
    
    int p1 = get_phase_pwm(angle, 0);
    int p2 = get_phase_pwm(angle, 1);
    int p3 = get_phase_pwm(angle, 2);
    
    motor_set_phases(p1, p2, p3);
    
    s_angle = angle;
}

int motor_getangle()
{
    return s_angle;
}

void motor_poweroff()
{
    motor_set_phases(0, 0, 0);
}