#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;
}

static void set_dir_l(char dir) { // f: forward, b: backward
    if      (dir == 'f') P5->OUT &= ~0b00010000;
    else if (dir == 'b') P5->OUT |=  0b00010000;
}

static void set_dir_r(char dir) { // f: forward, b: backward
    if      (dir == 'f') P5->OUT &= ~0b00100000;
    else if (dir == 'b') P5->OUT |=  0b00100000;
}

static void set_en_l(int enable) { // 1: on, 0: off
    if      (enable == 1) P2->OUT |=  0b10000000;
    else if (enable == 0) P2->OUT &= ~0b10000000;
}

static void set_en_r(int enable) { // 1: on, 0: off
    if      (enable == 1) P2->OUT |=  0b01000000;
    else if (enable == 0) P2->OUT &= ~0b01000000;
}

void motor_move_stop() {
    // P2->OUT  &= ~0xc0;
    set_en_l(0);
    set_en_r(0);
}

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);

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

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

    move_backward();
    Clock_Delay1ms(speed);

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

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

    turn_left();
    Clock_Delay1ms(speed);

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

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

    turn_right();
    Clock_Delay1ms(speed);

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

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

    rotate_clockwise();
    Clock_Delay1ms(speed);

    motor_move_stop();
    Clock_Delay1ms(PERIOD_ms - speed);

    set_dir_l('f');
    set_dir_r('f');
    // move_forward();
    // motor_move_stop();
}

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

    rotate_cclockwise();
    Clock_Delay1ms(speed);

    motor_move_stop();
    Clock_Delay1ms(PERIOD_ms - speed);

    set_dir_l('f');
    set_dir_r('f');
    // move_forward();
    // motor_move_stop();
}

void motor_move_custom(int left_speed, int right_speed) { // positive will be forward, negative will be backward
    int l_speed = left_speed > 0 ? left_speed : -left_speed;
    int r_speed = right_speed > 0 ? right_speed : -right_speed;
    normalize_speed(&l_speed);
    normalize_speed(&r_speed);

    if (left_speed >= 0) set_dir_l('f');
	else if (left_speed < 0) set_dir_l('b');

    if (right_speed >= 0) set_dir_r('f');
	else if (right_speed < 0) set_dir_r('b');


    if (l_speed >= r_speed) {
        set_en_l(1);
        set_en_r(1);
        Clock_Delay1ms(r_speed);

        set_en_r(0);
        Clock_Delay1ms(l_speed - r_speed);

        set_en_l(0);
        Clock_Delay1ms(PERIOD_ms - l_speed);

    } else if (l_speed < r_speed) {
        set_en_l(1);
        set_en_r(1);
        Clock_Delay1ms(l_speed);

        set_en_l(0);
        Clock_Delay1ms(r_speed - l_speed);

        set_en_r(0);
        Clock_Delay1ms(PERIOD_ms - r_speed);

    }
}