/****************************************************************************
 Module
   MotorCommands.c

 Revision
   1.0.1

 Description
   Defines functions for controlling drive motors

 Notes

 History
 When           Who     What/Why
 -------------- ---     --------
 01/20/18 09:46 edzc    first pass
 02/03/18 07:09 edzc    updated with functionality to drive a 2 wheeled robot

****************************************************************************/
// the common headers for C99 types
#include <stdint.h>
#include <stdbool.h>
#include <stdio.h>

// the headers to access the GPIO subsystem
#include "inc/hw_memmap.h"
#include "inc/hw_types.h"
#include "inc/hw_gpio.h"
#include "inc/hw_PWM.h"
#include "inc/hw_timer.h"
#include "inc/hw_nvic.h"
#include "inc/hw_sysctl.h"

// the headers to access the TivaWare Library
#include "driverlib/sysctl.h"
#include "driverlib/pin_map.h"
#include "driverlib/gpio.h"
#include "driverlib/timer.h"
#include "driverlib/interrupt.h"

#include "BITDEFS.H"

#include "PWM.h"
#include "MotorCommands.h"

// **********************************************************************
// PROTOTYPES
static void LeftMotorReverse(void);
static void RightMotorReverse(void);
static void LeftMotorForward(void);
static void RightMotorForward(void);

// READABILITY DEFINES
#define PORT_D BIT3HI

#define RightMotor 1
#define LeftMotor 0
#define Servo 7

#define FlywheelPin    BIT5HI
#define FlywheelISRPin BIT6HI //PC6
#define IR_EmitterPin  BIT4HI
#define ServoPin       BIT0HI

#define LeftMotorDirHI   BIT3HI
#define RightMotorDirHI  BIT2HI

#define LeftMotorDirLO   BIT3LO
#define RightMotorDirLO  BIT2LO

#define RightOffset 0
#define LeftOffset 0

#define FullSpeed 90
#define HalfSpeed 70
#define VeerSpeed 50
#define SlowSpeed 40 
#define StopSpeed 0
#define TurnSpeed 60 
#define TurnSlowSpeed 50
#define VeerDifference 35

#define FlywheelSpeed 65
#define ServoSpeed 50
#define IR_EmitterSpeed 50

// PeriodInMS * PWMTicksPerMS
#define PWMTicksPerMS 1250 // 1 tick = 1 us 

float ServoRestPulseWidth = 1.8;
float ServoDropBallPulseWidth = 1;

// **********************************************************************
// PUBLIC FUNCTIONS //
// **********************************************************************
void IRReloadSetPeriod(uint32_t Period)
{
 HWREG(PWM0_BASE + PWM_O_2_LOAD) = (((Period*PWMTicksPerMS)/(2))/(1000))>>1; //Setting the period value 
 PWM_SetDuty(IR_EmitterSpeed, IR_EmitterPin); //setting 50% duty cycle
  
return; 
}

void IRReloadOff (void) 
{
	//Use the PWM_SetDuty function to set the duty cycle of the IR_EmitterPin to 0
  PWM_SetDuty(StopSpeed, IR_EmitterPin);
}

void ServoRest (void)
{
	 //Use the PWM_SetPulseWidth function to set the pulse width of the servo pin to 0
   PWM_SetPulseWidth(ServoRestPulseWidth);
}

void ServoDropBall (void)
{
	 //Use the PWM_SetPulseWidth function to set the pulse width of the servo pin to move to the drop ball position
   PWM_SetPulseWidth(ServoDropBallPulseWidth);
}

void ServoOff(void)
{
	//Use the PWM_SetDuty function to set the duty cycle of the ServoPin to 0
  PWM_SetDuty(StopSpeed, ServoPin);
}

void FlywheelOn(void)
{
	//Use the PWM_SetDuty function to set the duty cycle of the FlywheelPin to the FlywheelSpeed
  PWM_SetDuty(FlywheelSpeed, FlywheelPin);
}

void FlywheelOff(void)
{
	//Use the PWM_SetDuty function to set the duty cycle of the FlywheelPin to 0
  PWM_SetDuty(StopSpeed, FlywheelPin);
}

// Move drive motors forward at MAX speed
void ForwardFull(void)
{
  PWM_SetDuty(FullSpeed + RightOffset, LeftMotor);
  LeftMotorForward();
  PWM_SetDuty(FullSpeed + LeftOffset, RightMotor);
  RightMotorForward();
}

// Moves drive wheels forward at Half speed
void ForwardHalf(void)
{
  PWM_SetDuty(HalfSpeed + RightOffset, LeftMotor);
  LeftMotorForward();
  PWM_SetDuty(HalfSpeed + LeftOffset, RightMotor); ///changed for testing
  RightMotorForward();
}

// Moves drive wheels forward at slow speed
void ForwardSlow(void)
{
  PWM_SetDuty(SlowSpeed + RightOffset, LeftMotor);
  LeftMotorForward();
  PWM_SetDuty(SlowSpeed + LeftOffset, RightMotor);
  RightMotorForward();
}

//Moves drive wheel backward at full speed
void ReverseFull(void)
{
  PWM_SetDuty((100-FullSpeed)-RightOffset, LeftMotor);
  LeftMotorReverse();
  PWM_SetDuty((100-FullSpeed)-LeftOffset, RightMotor);
  RightMotorReverse();
}

// Moves drive wheels backward at half speed
void ReverseHalf(void)
{
  PWM_SetDuty((100-HalfSpeed)-RightOffset, LeftMotor);
  LeftMotorReverse();
  PWM_SetDuty((100-HalfSpeed)-LeftOffset, RightMotor);
  RightMotorReverse();
}

// Moves drive wheels backward at slow speed
void ReverseSlow(void)
{
  PWM_SetDuty((100-SlowSpeed)-RightOffset, LeftMotor);
  LeftMotorReverse();
  PWM_SetDuty((100-SlowSpeed)-LeftOffset, RightMotor);
  RightMotorReverse();
}

// Rotates Clockwise
void RotateCCW(void)
{
  // Set the LeftMotor to move forwards and the RightMotor to move backwards
  PWM_SetDuty(TurnSpeed, LeftMotor);
  LeftMotorForward();
  PWM_SetDuty((100-TurnSpeed), RightMotor);
  RightMotorReverse();
}

// RotateCCWSlowly
void RotateCCWSlow(void)
{
  // Set the LeftMotor to move forwards and the RightMotor to move backwards
  PWM_SetDuty(TurnSlowSpeed, LeftMotor);
  LeftMotorForward();
  PWM_SetDuty((100-TurnSlowSpeed), RightMotor);
  RightMotorReverse();
}

// Rotates Clockwise
void RotateCW(void)
{
  // Set the LeftMotor to move backwards and the RightMotor to move forwards
  PWM_SetDuty((100-TurnSpeed), LeftMotor);
  LeftMotorReverse();
  PWM_SetDuty(TurnSpeed, RightMotor);
  RightMotorForward();
}

// RotateCWSlowly
void RotateCWSlow(void)
{
  // Set the LeftMotor to move backwards and the RightMotor to move forwards
  PWM_SetDuty((100-TurnSlowSpeed), LeftMotor);
  LeftMotorReverse();
  PWM_SetDuty(TurnSlowSpeed, RightMotor);
  RightMotorForward();
}

// VeerLeft
void VeerLeftHalf(void)
{
  // Set the LeftMotor to move faster than the right motor in the forward direction
  PWM_SetDuty((VeerSpeed + VeerDifference), LeftMotor);
  LeftMotorForward();
  PWM_SetDuty(VeerSpeed, RightMotor);
  RightMotorForward();
}

// VeerRight
void VeerRightHalf(void)
{
  // Set the LeftMotor to move slower than the right motor in the forward direction
  PWM_SetDuty((VeerSpeed), LeftMotor);
  LeftMotorForward();
  PWM_SetDuty((VeerSpeed+VeerDifference), RightMotor);
  RightMotorForward();
}

//Turn both drive motors off
void Stop(void)
{
  //Want both lines to stay low
  PWM_SetDuty(StopSpeed, LeftMotor);
  LeftMotorForward();
  PWM_SetDuty(StopSpeed, RightMotor);
  RightMotorForward();
}

void MotorDirInit(void){
  // set up port D by enabling the peripheral clock  
	HWREG(SYSCTL_RCGCGPIO) |= PORT_D; 
  //waiting for the peripheral to be ready
	while ((HWREG(SYSCTL_PRGPIO) & PORT_D) != PORT_D) 
  {
		;
	}
  //set the direction of RightMotorDir and LeftMotorDir to digital output
	HWREG(GPIO_PORTD_BASE + GPIO_O_DEN) |= (RightMotorDirHI | LeftMotorDirHI);	
	HWREG(GPIO_PORTD_BASE + GPIO_O_DIR) |= (RightMotorDirHI | LeftMotorDirHI);	
}

static void LeftMotorForward(void){
	//Write the left motor direction pin lo
  HWREG(GPIO_PORTD_BASE + (GPIO_O_DATA + ALL_BITS)) &= (LeftMotorDirLO);
}

static void RightMotorForward(void){
	//Write the right motor direction pin lo
  HWREG(GPIO_PORTD_BASE + (GPIO_O_DATA + ALL_BITS)) &= (RightMotorDirLO);
}

static void LeftMotorReverse(void){
	//write the left motor direction pin hi
  HWREG(GPIO_PORTD_BASE + (GPIO_O_DATA + ALL_BITS)) |= (LeftMotorDirHI);
}

static void RightMotorReverse(void){
	//write the right motor pin hi
  HWREG(GPIO_PORTD_BASE + (GPIO_O_DATA + ALL_BITS)) |= (RightMotorDirHI);
}