#include "motor.h"
#include "Clock.h"
#include <stdio.h>



void motor_init(void) {
    P2->SEL0 &= ~0xc0;
    P2->SEL1 &= ~0xc0;
    P2->DIR |= 0xc0; // output
    P2->OUT &= ~0xc0;// clear

    P3->SEL0 &= ~0xc0;
    P3->SEL1 &= ~0xc0;
    P3->DIR |= 0xc0; // output
    P3->OUT &= ~0xc0;// clear

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

    pwm_init_for_motors(MOTOR_PERIOD);
}

void motor_move(int32_t left, int32_t right) {
    if (left == 0 && right == 0) {
        pwm_set_duty_left(0);
        pwm_set_duty_right(0);
    } else if (left >= 0 && right >= 0) {
        // fwd fwd
        P3->OUT |= 0xc0;
        P5->OUT &= ~0x30;
        pwm_set_duty_left((uint16_t) left);
        pwm_set_duty_right((uint16_t) right);
    } else if (left < 0 && right < 0) {
        // back back
        P3->OUT |= 0xc0;
        P5->OUT = 0x30;
        pwm_set_duty_left((uint16_t) (-left));
        pwm_set_duty_right((uint16_t) (-right));
    } else if (left >= 0 && right < 0) {
        // fwd back
        P3->OUT |= 0xc0;
        P5->OUT |= (P5->OUT & (~0x30)) | 0x20;
        pwm_set_duty_left((uint16_t) left);
        pwm_set_duty_right((uint16_t) (-right));
    } else if (left < 0 && right >= 0) {
        // back fwd
        P3->OUT |= 0xc0;
        P5->OUT |= (P5->OUT & (~0x30)) | 0x10;
        pwm_set_duty_left((uint16_t) (-left));
        pwm_set_duty_right((uint16_t) right);
    }
}

void motor_stop(void) {
    pwm_set_duty_left(0);
    pwm_set_duty_right(0);
    P3->OUT &= ~0xc0;
}