#include "motor.h"
#include "pwm.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
}

// Stop the motors, power down the drivers, and
// set the PWM speed control to 0% duty cycle.
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

    SetPWMDutyLeftMotor(0);
    SetPWMDutyRightMotor(0);
}

// Left and right wheels forward with the given duty cycles. 
// Duty cycle is 0 to 14,998.
void MotorForward(uint16_t left_duty, uint16_t right_duty) { 
    P5->OUT &= ~(LEFT_DIR_PIN | RIGHT_DIR_PIN);     // set direction - PH low
    P3->OUT |=  (LEFT_SLEEP_PIN | RIGHT_SLEEP_PIN); // wake up motors
    P2->OUT |= (LEFT_PWM_PIN | RIGHT_PWM_PIN);

    SetPWMDutyRightMotor(right_duty);
    SetPWMDutyLeftMotor(left_duty);
}

// Left and right wheels backward with the given duty cycles.
// Duty cycle is 0 to 14,998.
void MotorBackward(uint16_t left_duty, uint16_t right_duty) { 
    P5->OUT |=  (LEFT_DIR_PIN | RIGHT_DIR_PIN);     // set direction - PH high
    P3->OUT |=  (LEFT_SLEEP_PIN | RIGHT_SLEEP_PIN); // wake up motors
    P2->OUT |= (LEFT_PWM_PIN | RIGHT_PWM_PIN);

    SetPWMDutyRightMotor(right_duty);
    SetPWMDutyLeftMotor(left_duty);

}

// Left wheel backward, right wheel forward with the given duty cycles.
// Duty cycle is 0 to 14,998.
void MotorLeft(uint16_t left_duty, uint16_t right_duty) { 
    P5->OUT &= ~(LEFT_DIR_PIN);   // set direction
    P3->OUT |=  (LEFT_SLEEP_PIN); // wake up motor
    P2->OUT |= (LEFT_PWM_PIN | RIGHT_PWM_PIN);

    SetPWMDutyRightMotor(right_duty);
    SetPWMDutyLeftMotor(left_duty);
}

// Left wheel forward, right wheel backward with the given duty cycles. 
// Duty cycle is 0 to 14,998.
void MotorRight(uint16_t left_duty, uint16_t right_duty) { 
    P5->OUT &= ~(RIGHT_DIR_PIN);   // set direction - PH high
    P3->OUT |=  (RIGHT_SLEEP_PIN); // wake up motors
    P2->OUT |= (LEFT_PWM_PIN | RIGHT_PWM_PIN);

    SetPWMDutyRightMotor(right_duty);
    SetPWMDutyLeftMotor(left_duty);
}