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

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

#include "inc/msp432p401r.h"

#undef BIT0
#undef BIT1
#undef BIT2
#undef BIT3
#undef BIT4
#undef BIT5
#undef BIT6
#undef BIT7

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

// duty := H/(H+L) = H/10000
// H+L is always 10 000 microsecond

// 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) {
    // 1. Configure all motor control pins as outputs
    // P5.4 (Left DIR), P5.5 (Right DIR)
    P5->SEL0 &= ~(LEFT_DIR_PIN | RIGHT_DIR_PIN);         // GPIO function
    P5->SEL1 &= ~(LEFT_DIR_PIN | RIGHT_DIR_PIN);
    P5->DIR  |=  (LEFT_DIR_PIN | RIGHT_DIR_PIN);           // Output direction

    // P3.6 (Right SLEEP), P3.7 (Left SLEEP)
    P3->SEL0 &= ~(LEFT_SLEEP_PIN | RIGHT_SLEEP_PIN);         // GPIO function
    P3->SEL1 &= ~(LEFT_SLEEP_PIN | RIGHT_SLEEP_PIN);
    P3->DIR  |=  (LEFT_SLEEP_PIN | RIGHT_SLEEP_PIN);           // Output direction
    
    // P2.6 (Right EN), P2.7 (Left EN)
    P2->SEL0 &= ~(LEFT_PWM_PIN | RIGHT_PWM_PIN);         // GPIO function
    P2->SEL1 &= ~(LEFT_PWM_PIN | RIGHT_PWM_PIN);
    P2->DIR  |=  (LEFT_PWM_PIN | RIGHT_PWM_PIN);           // Output direction

    // 2. Initial state: disable motors
    P3->OUT  &= ~(LEFT_SLEEP_PIN | RIGHT_SLEEP_PIN);  // SLEEP pins low - DRV8838 disabled
    P2->OUT  &= ~(LEFT_PWM_PIN | RIGHT_PWM_PIN);  // EN pins low

    SysTickWait1us(1000);
}

// Stops both motors, puts driver to sleep
void MotorStop(void) {
    P3->OUT &= ~(LEFT_SLEEP_PIN | RIGHT_SLEEP_PIN);  // SLEEP pins low
    P2->OUT &= ~(LEFT_SLEEP_PIN | RIGHT_SLEEP_PIN);  // EN pins low
}

// 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) {
    P5->OUT &= ~(LEFT_DIR_PIN | RIGHT_DIR_PIN);     // set direction - PH low
    P3->OUT |=  (LEFT_SLEEP_PIN | RIGHT_SLEEP_PIN); // wake up motors

    uint32_t i;
    for (i = 0; i < times_10ms; ++i) {
        P2->OUT |= (LEFT_PWM_PIN | RIGHT_PWM_PIN);  // Enable both motors (EN high)
        SysTickWait1us(duty);
        
        P2->OUT &= ~(LEFT_PWM_PIN | RIGHT_PWM_PIN); // Disable motors
        SysTickWait1us(10000 - duty);

        uint8_t bump = BumpRead();
        if ((~bump & BUMP_PINS)) break;
    }

    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) {
    P5->OUT |=  (LEFT_DIR_PIN | RIGHT_DIR_PIN);     // set direction - PH high
    P3->OUT |=  (LEFT_SLEEP_PIN | RIGHT_SLEEP_PIN); // wake up motors

    uint32_t i;
    for (i = 0; i < times_10ms; ++i) {
        P2->OUT |= (LEFT_PWM_PIN | RIGHT_PWM_PIN);  // Enable both motors (EN high)
        SysTickWait1us(duty);
        
        P2->OUT &= ~(LEFT_PWM_PIN | RIGHT_PWM_PIN); // Disable motors
        SysTickWait1us(10000 - duty);

        uint8_t bump = BumpRead();
        if ((~bump & BUMP_PINS)) break;
    }
    
    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) {
    P5->OUT &= ~(LEFT_DIR_PIN);   // set direction
    P3->OUT |=  (LEFT_SLEEP_PIN); // wake up motors

    uint32_t i;
    for (i = 0; i < times_10ms; ++i) {
        P2->OUT |= (LEFT_PWM_PIN);  // Enable both motors (EN high)
        SysTickWait1us(duty);
        
        P2->OUT &= ~(LEFT_PWM_PIN); // Disable motors
        SysTickWait1us(10000 - duty);

        uint8_t bump = BumpRead();
        if ((~bump & BUMP_PINS)) break;
    }
    
    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) {
    P5->OUT &= ~(RIGHT_DIR_PIN);   // set direction - PH high
    P3->OUT |=  (RIGHT_SLEEP_PIN); // wake up motors

    uint32_t i;
    for (i = 0; i < times_10ms; ++i) {
        P2->OUT |= (RIGHT_PWM_PIN);  // Enable both motors (EN high)
        SysTickWait1us(duty);
        
        P2->OUT &= ~(RIGHT_PWM_PIN); // Disable motors
        SysTickWait1us(10000 - duty);
        
        uint8_t bump = BumpRead();
        if ((~bump & BUMP_PINS)) break;
    }
    
    MotorStop();
}