#include <stdbool.h>
#include "motor_simple.h"

#include "bump.h"
#include "systick.h"

#include "inc/msp432p401r.h"


#define BIT0 (uint8_t)(0x01)
#define BIT1 (uint8_t)(0x02)
#define BIT2 (uint8_t)(0x04)
#define BIT3 (uint8_t)(0x08)
#define BIT4 (uint8_t)(0x10)
#define BIT5 (uint8_t)(0x20)
#define BIT6 (uint8_t)(0x40)
#define BIT7 (uint8_t)(0x80)


// Left motor (PH) direction connected to P5.4 (J3.29)
#define LEFT_DIR_PIN     BIT4  // P5.4
// Right motor (PH) direction connected to P5.5 (J3.30)
#define RIGHT_DIR_PIN    BIT5  // P5.5

// Left motor (EN) PWM connected to P2.7/TA0CCP4 (J4.40)
#define LEFT_PWM_PIN     BIT7  // P2.7
// Right motor (EN) PWM connected to P2.6/TA0CCP3 (J4.39)
#define RIGHT_PWM_PIN    BIT6  // P2.6

// Left motor (nSLEEP) enable connected to P3.7 (J4.31)
#define LEFT_SLEEP_PIN   BIT7  // P3.7
// Right motor (nSLEEP) enable connected to P3.6 (J2.11)
#define RIGHT_SLEEP_PIN  BIT6  // P3.6


// Initializes the 6 GPIO output lines and puts driver to sleep
void MotorInit(void) {
    P5->SEL0 &= ~(LEFT_DIR_PIN | RIGHT_DIR_PIN);    // GPIO enable
    P5->SEL1 &= ~(LEFT_DIR_PIN | RIGHT_DIR_PIN);
    P5->DIR |=   (LEFT_DIR_PIN | RIGHT_DIR_PIN);    // Output direction
    P5->OUT &=  ~(LEFT_DIR_PIN | RIGHT_DIR_PIN);    // Initialize low
    
    // motor enable pins - P2.6 P2.7
    P2->SEL0 |=  (LEFT_PWM_PIN | RIGHT_PWM_PIN);    // TimerA function
    P2->SEL1 &= ~(LEFT_PWM_PIN | RIGHT_PWM_PIN);
    P2->DIR |=   (LEFT_PWM_PIN | RIGHT_PWM_PIN);    // Output direction
    
    // motor sleep pins - P3.6 P3.7
    P3->SEL0 &= ~(LEFT_SLEEP_PIN | RIGHT_SLEEP_PIN);    // GPIO enable
    P3->SEL1 &= ~(LEFT_SLEEP_PIN | RIGHT_SLEEP_PIN);
    P3->DIR |=   (LEFT_SLEEP_PIN | RIGHT_SLEEP_PIN);    // Output direction
    P3->OUT &=  ~(LEFT_SLEEP_PIN | RIGHT_SLEEP_PIN);    // Initialize low (sleep mode)
}

// Stops both motors, puts driver to sleep
void MotorStop(void) {
    P2->OUT &= ~0xC0;   // off
    P3->OUT &= ~0xC0;   // low current sleep mode
}

// Drives both motors forward at duty (100 to 9900)
// Runs for time duration, and then stops
// Stop the motors and return if any bumper switch is active
void MotorForward(uint16_t duty, uint32_t times_10ms) {
    // set direction - PH low
    P5->OUT &= ~(LEFT_DIR_PIN | RIGHT_DIR_PIN);

    // wake up motors
    P3->OUT |= (LEFT_SLEEP_PIN | RIGHT_SLEEP_PIN);

    // Set PWM duty cycle
    TIMER_A0->CCR[3] = duty;  // Right motor (P2.6/TA0.3)
    TIMER_A0->CCR[4] = duty;  // Left motor  (P2.7/TA0.4)

    while(times_10ms--) {
        Delay10ms(1);
    }
    
    MotorStop();
}

// Drives both motors backward at duty (100 to 9900)
// Runs for time duration, and then stops
// Runs even if any bumper switch is active
void MotorBackward(uint16_t duty, uint32_t times_10ms) {
    // set direction - PH high
    P5->OUT |= (LEFT_DIR_PIN | RIGHT_DIR_PIN);

    // wake up motors
    P3->OUT |= (LEFT_SLEEP_PIN | RIGHT_SLEEP_PIN);

    // Set PWM duty cycle
    TIMER_A0->CCR[3] = duty;  // Right motor (P2.6/TA0.3)
    TIMER_A0->CCR[4] = duty;  // Left motor  (P2.7/TA0.4)

    while(times_10ms--) {
        Delay10ms(1);
    }
    
    MotorStop();
}

// Drives just the left motor forward at duty (100 to 9900)
// Right motor is stopped (sleeping)
// Runs for time duration, and then stops
// Stop the motor and return if any bumper switch is active
void MotorLeft(uint16_t duty, uint32_t times_10ms) {
    // set direction - PH low
    P5->OUT &= ~LEFT_DIR_PIN;

    // wake up motors
    P3->OUT = (P3->OUT & ~RIGHT_SLEEP_PIN) | LEFT_SLEEP_PIN;

    // Set PWM duty cycle
    TIMER_A0->CCR[3] = 0;      // Right motor off
    TIMER_A0->CCR[4] = duty;   // Left motor on

    while(times_10ms--) {
        Delay10ms(1);
    }
    
    MotorStop();
}

// Drives just the right motor forward at duty (100 to 9900)
// Left motor is stopped (sleeping)
// Runs for time duration, and then stops
// Stop the motor and return if any bumper switch is active
void MotorRight(uint16_t duty, uint32_t times_10ms) {
    // set direction - PH low
    P5->OUT &= ~RIGHT_DIR_PIN;

    // wake up motors
    P3->OUT = (P3->OUT & ~LEFT_SLEEP_PIN) | RIGHT_SLEEP_PIN;

    // Set PWM duty cycle
    TIMER_A0->CCR[3] = duty;    // Right motor off
    TIMER_A0->CCR[4] = 0;       // Left motor on

    while(times_10ms--) {
        Delay10ms(1);
    }
    
    MotorStop();
}