#include "motor.h"
#include "Clock.h"

// Period: 20ms
// MAX speed: 20

static void normalize_speed(int *speed) {
    if (*speed <= 0) *speed = 0;
    else if (*speed > MAX_SPEED) *speed = MAX_SPEED;
}

void motor_init(void) {
    P3->SEL0 &= ~0xc0;
    P3->SEL1 &= ~0xc0;
    P3->DIR  |=  0xc0;
    P3->OUT  &= ~0xc0;

    P5->SEL0 &= ~0x30;
    P5->SEL1 &= ~0x30;
    P5->DIR  |=  0x30;
    P5->OUT  &= ~0x30;

    P2->SEL0 &= ~0xc0;
    P2->SEL1 &= ~0xc0;
    P2->DIR  |=  0xc0;
    P2->OUT  &= ~0xc0;
}

void move_stop() {
    P2->OUT  &= ~0xc0;
}

static void move_forward() {
    P5->OUT  &= ~0b00110000;
    P3->OUT  |=  0b11000000;
    P2->OUT  |=  0b11000000;
}

static void move_backward() {
    P5->OUT  |=  0b00110000;
    P3->OUT  |=  0b11000000;
    P2->OUT  |=  0b11000000;
}

static void turn_left() {
    P3->OUT  |=  0b11000000;
    P2->OUT  &= ~0b10000000;
    P2->OUT  |=  0b01000000;
}

static void turn_right() {
    P3->OUT  |=  0b11000000;
    P2->OUT  &= ~0b01000000;
    P2->OUT  |=  0b10000000;
}

static void rotate_clockwise() {
    P5->OUT  &= ~0b00110000;
    P5->OUT  |=  0b00100000;
    P3->OUT  |=  0b11000000;
    P2->OUT  |=  0b11000000;
}

static void rotate_cclockwise() {
    P5->OUT  &= ~0b00110000;
    P5->OUT  |=  0b00010000;
    P3->OUT  |=  0b11000000;
    P2->OUT  |=  0b11000000;
}

void motor_move_forward(int speed) {
    normalize_speed(&speed);

    move_forward();
    Clock_Delay1ms(speed);

    move_stop();
    Clock_Delay1ms(PERIOD_ms - speed);
}

void motor_move_backward(int speed) {
    normalize_speed(&speed);

    move_backward();
    Clock_Delay1ms(speed);

    move_stop();
    Clock_Delay1ms(PERIOD_ms - speed);
}

void motor_turn_left(int speed) {
    normalize_speed(&speed);

    turn_left();
    Clock_Delay1ms(speed);

    move_stop();
    Clock_Delay1ms(PERIOD_ms - speed);
}

void motor_turn_right(int speed) {
    normalize_speed(&speed);

    turn_right();
    Clock_Delay1ms(speed);

    move_stop();
    Clock_Delay1ms(PERIOD_ms - speed);
}

void motor_rotate_clockwise(int speed) {
    normalize_speed(&speed);

    rotate_clockwise();
    Clock_Delay1ms(speed);

    move_stop();
    Clock_Delay1ms(PERIOD_ms - speed);

    move_forward();
    move_stop();
}

void motor_rotate_cclockwise(int speed) {
    normalize_speed(&speed);

    rotate_cclockwise();
    Clock_Delay1ms(speed);

    move_stop();
    Clock_Delay1ms(PERIOD_ms - speed);

    move_forward();
    move_stop();
}