/****************************************************************************
 Module
   PWM.c

 Revision
   1.0.1

 Description
   This module creates PWM controls with the TIVA

 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"

// **********************************************************************
// PROTOTYPES
void RestoreDC(uint8_t);
void PWM_SetDuty(uint8_t duty, uint8_t pin);

// READABILITY DEFINES
#define GenA_Normal (PWM_0_GENA_ACTCMPAU_ONE | PWM_0_GENA_ACTCMPAD_ZERO)
#define GenB_Normal (PWM_0_GENB_ACTCMPBU_ONE | PWM_0_GENB_ACTCMPBD_ZERO)

#define LeftMotorPin  BIT6HI //PWM0GENA //M0PWM0
#define RightMotorPin BIT7HI //PWM0GENB //M0PWM1
#define FlywheelPin   BIT5HI //PWM1GENB //M0PWM3
#define IR_EmitterPin BIT4HI //PWM2GENA //M0PWM4
#define ServoPin      BIT0HI //PWM3GENA //M0PWM6

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

// PeriodInMS * PWMTicksPerMS
#define PeriodInTicks_40Hz PWMTicksPerMS*25
#define PeriodInTicks_200Hz PWMTicksPerMS*5
#define PeriodInTicks_250Hz PWMTicksPerMS*4
#define PeriodInTicks_500Hz PWMTicksPerMS*2
#define PeriodInTicks_1000Hz PWMTicksPerMS
#define PeriodInTicks_2000Hz PWMTicksPerMS/2
#define PeriodInTicks_3000Hz PWMTicksPerMS/3
#define PeriodInTicks_4000Hz PWMTicksPerMS/4
#define PeriodInTicks_10000Hz PWMTicksPerMS/10

// MODULE LEVEL VARIABLES
static uint16_t Period_PWM = PeriodInTicks_2000Hz;
static uint16_t Period_PWM_Flywheel = PeriodInTicks_2000Hz; 
static uint16_t Period_PWM_Servo = PeriodInTicks_250Hz; 
static uint16_t Period_PWM_IR = PeriodInTicks_200Hz; 
uint32_t PulseWidthInTicks;

// **********************************************************************
// PUBLIC FUNCTIONS //
// **********************************************************************

// Initialize the PWM to the num of Pins desired/specified
void PWM_Init(void)
{
  // start by enabling the clock to set up the PWM Module (PWM0)
  HWREG(SYSCTL_RCGCPWM) |= SYSCTL_RCGCPWM_R0;
 
   // enable the clock to Port B
  HWREG(SYSCTL_RCGCGPIO) |= SYSCTL_RCGCGPIO_R1;
  
  //enable the clock to Port D 
   HWREG(SYSCTL_RCGCGPIO) |= SYSCTL_RCGCGPIO_R3;
  
  //enable the clock to Port E 
  HWREG(SYSCTL_RCGCGPIO) |= SYSCTL_RCGCGPIO_R4;
  
  // Select the PWM clock
  // SystemClock/32 gives you 1 tick = 1 us
  HWREG(SYSCTL_RCC) = (HWREG(SYSCTL_RCC) & ~SYSCTL_RCC_PWMDIV_M) | (SYSCTL_RCC_USEPWMDIV | SYSCTL_RCC_PWMDIV_32);
    
  // Make sure that the PWM module clock has started 
  //PRPWM is the perpheral ready register for PRM. R0 is for PWM module 0
  while ((HWREG(SYSCTL_PRPWM) & SYSCTL_PRPWM_R0) != SYSCTL_PRPWM_R0);
      
  // disable the PWM 0 while initializing (motors)
  HWREG(PWM0_BASE+ PWM_O_0_CTL) = 0;
  
  // disable the PWM0_ PWM1 while initializing (flywheel)
  HWREG(PWM0_BASE+ PWM_O_1_CTL) = 0;
  
  //disable the PWM0_PWM2 while initializing (IR Emitter)
  HWREG(PWM0_BASE+ PWM_O_2_CTL) = 0;
  
   //disable the PWM0_PWM2 while initializing (servo)
  HWREG(PWM0_BASE+ PWM_O_3_CTL) = 0;
  
  // program generators to go to (for PWM0GENA/PWMGENB):
  // 1 at rising compare A/B
  // 0 at falling compare A/B
  HWREG(PWM0_BASE + PWM_O_0_GENA) = GenA_Normal;
  HWREG(PWM0_BASE + PWM_O_0_GENB) = GenB_Normal;
  
  //program generators to go to (for PWM1GENA/PWM1GENB): 
  //1 at rising compare A
  //0 at falling compare A
  //flywheel (GenB required for PB5)
  HWREG(PWM0_BASE + PWM_O_1_GENB) = GenB_Normal; 
  
  //program generators to go to (for PWM2GENA): 
  //1 at rising compare A
  //0 at falling compare A
  //IR Emitter
  HWREG(PWM0_BASE + PWM_O_2_GENA) = GenA_Normal; 
    
  //program generators to go to (for PWM1GENA/PWM1GENB): 
  //1 at rising compare A
  //0 at falling compare A
  //Servo 
  HWREG(PWM0_BASE + PWM_O_3_GENA) = GenA_Normal;  

  // Set the init period with:
  // Load = 1/2 * Period
  // also program the match compare register to 1/2 the desired high time
  // Motors
  HWREG(PWM0_BASE + PWM_O_0_LOAD) = (Period_PWM)>>1;
  
  //Set the initial period with 
  //Load = 1/2*period 
  //also program the match compare register to 1/2 the desired high time
  //flywheel
   HWREG(PWM0_BASE + PWM_O_1_LOAD) = (Period_PWM_Flywheel)>>1;
   
  //Set the initial period with 
  //Load = 1/2*period 
  //also program the match compare register to 1/2 the desired high time
  //IR_emitter
   HWREG(PWM0_BASE + PWM_O_2_LOAD) = (Period_PWM_IR)>>1;
   
  //Set the initial period with 
  //Load = 1/2*period 
  //also program the match compare register to 1/2 the desired high time
  //servo
   HWREG(PWM0_BASE + PWM_O_3_LOAD) = (Period_PWM_Servo)>>1;
    
  // Set the init duty to 0 (Motors) 
  HWREG(PWM0_BASE + PWM_O_0_GENA) = PWM_0_GENA_ACTZERO_ZERO;
  // Set the init duty to 0 (Motors) 
  HWREG(PWM0_BASE + PWM_O_0_GENB) = PWM_0_GENB_ACTZERO_ZERO;
  
  // Set the init duty to 0 (Flywheel) 
    HWREG(PWM0_BASE + PWM_O_1_GENB) = PWM_0_GENB_ACTZERO_ZERO;
 //   HWREG(PWM0_BASE + PWM_O_1_GENB) = GenB_Normal;
  //  HWREG(PWM0_BASE + PWM_O_1_CMPB) = (HWREG(PWM0_BASE + PWM_O_1_LOAD)*(100 - 75)/100);

  // Set the init duty to 0 (IR_Emitter) 
    HWREG(PWM0_BASE + PWM_O_2_GENA) = PWM_0_GENA_ACTZERO_ZERO;
  //  HWREG(PWM0_BASE + PWM_O_2_GENA) = GenA_Normal;
 //   HWREG(PWM0_BASE + PWM_O_2_CMPA) = (HWREG(PWM0_BASE + PWM_O_2_LOAD)*(100 - 75)/100);

  // Set the init duty to 0 (Servo) 
  HWREG(PWM0_BASE + PWM_O_3_GENA) = PWM_0_GENA_ACTZERO_ZERO;
//    HWREG(PWM0_BASE + PWM_O_3_GENA) = GenA_Normal;
//    HWREG(PWM0_BASE + PWM_O_3_CMPA) = (HWREG(PWM0_BASE + PWM_O_3_LOAD)*(100 - 75)/100);

  // Enable the PWM outputs
  HWREG(PWM0_BASE + PWM_O_ENABLE) |= (PWM_ENABLE_PWM1EN | PWM_ENABLE_PWM0EN);

// Enable the PWM outputs (flywheel)
  HWREG(PWM0_BASE + PWM_O_ENABLE) |= (PWM_ENABLE_PWM3EN);

// Enable the PWM outputs (IR emitter)
  HWREG(PWM0_BASE + PWM_O_ENABLE) |= (PWM_ENABLE_PWM4EN);
  
//Enable the PWM outputs (servo)   
  HWREG(PWM0_BASE + PWM_O_ENABLE) |= (PWM_ENABLE_PWM6EN);

  // Configure PortB pins to be PWM outputs
  // Select the alternate function for PB6&7 (Motors) 
  HWREG(GPIO_PORTB_BASE + GPIO_O_AFSEL) |= (LeftMotorPin | RightMotorPin);
  
  //Configure PortB pins to be PWM outputs
  //Selecting the alternate function for PB5
  HWREG(GPIO_PORTB_BASE + GPIO_O_AFSEL) |= FlywheelPin; 

  //Configure PortE pins to be PWM outputs
  //Selecting the alternate function for PE4
  HWREG(GPIO_PORTE_BASE + GPIO_O_AFSEL) |= IR_EmitterPin; 

   //Configure PortD pins to be PWM outputs
  //Selecting the alternate function for PD0
  HWREG(GPIO_PORTD_BASE + GPIO_O_AFSEL) |= ServoPin; 

  // now map the PWM to those pins - mux 4 val we want to specficy on bits 6&7 (Motors)
  HWREG(GPIO_PORTB_BASE + GPIO_O_PCTL) |= (HWREG(GPIO_PORTB_BASE + GPIO_O_PCTL) & 0x00ffffff) + (4<<(7*BitsPerNibble)) + (4<<(6*BitsPerNibble));
  
  //now map the PWM to those pins--mux 4 val we want to specify on bit 5 (Flywheel)
  HWREG(GPIO_PORTB_BASE + GPIO_O_PCTL) |= (HWREG(GPIO_PORTB_BASE + GPIO_O_PCTL) & 0x00ffffff) + (4<<(5*BitsPerNibble));

  //now map the PWM to those pins--mux 4 val we want to specify on bit 4 (IR_Emitter)
  HWREG(GPIO_PORTE_BASE + GPIO_O_PCTL) |= (HWREG(GPIO_PORTE_BASE + GPIO_O_PCTL) & 0x00ffffff) + (4<<(4*BitsPerNibble));

  //now map the PWM to those pins--mux 4 val we want to specify on bit 0 (Servo)
  HWREG(GPIO_PORTD_BASE + GPIO_O_PCTL) |= (HWREG(GPIO_PORTD_BASE + GPIO_O_PCTL) & 0x00ffffff) + (4<<(0*BitsPerNibble));

  // enable pins 6&7 on portB for digital I/O
  HWREG(GPIO_PORTB_BASE + GPIO_O_DEN) |= (LeftMotorPin | RightMotorPin);

  // enable pin 5 on portB for digital I/O
  HWREG(GPIO_PORTB_BASE + GPIO_O_DEN) |= (FlywheelPin);

  // enable pin 4 on portE for digital I/O
 HWREG(GPIO_PORTE_BASE + GPIO_O_DEN) |= (IR_EmitterPin);

  // enable pin 0 on portE for digital I/O
  HWREG(GPIO_PORTD_BASE + GPIO_O_DEN) |= (ServoPin);
  
  // make pin 6&7 on Port B into outputs
  HWREG(GPIO_PORTB_BASE + GPIO_O_DIR) |= (LeftMotorPin | RightMotorPin);

  // make pin 5 on Port B into outputs
  HWREG(GPIO_PORTB_BASE + GPIO_O_DIR) |= (FlywheelPin);

  // make pin 4 on Port E into outputs
  HWREG(GPIO_PORTE_BASE + GPIO_O_DIR) |= (IR_EmitterPin);

  // make pin 0 on Port D into outputs
  HWREG(GPIO_PORTD_BASE + GPIO_O_DIR) |= (ServoPin);

  // set the up/down count mode, enable the PWM generator, and make the updates locally sync to zero count (Motors)
  HWREG(PWM0_BASE + PWM_O_0_CTL) = (PWM_0_CTL_MODE | PWM_0_CTL_ENABLE | PWM_0_CTL_GENAUPD_LS | PWM_0_CTL_GENBUPD_LS);
    
  // set the up/down count mode, enable the PWM generator, and make the updates locally sync to zero count (flywheel)
  HWREG(PWM0_BASE + PWM_O_1_CTL) = (PWM_0_CTL_MODE | PWM_0_CTL_ENABLE | PWM_0_CTL_GENAUPD_LS | PWM_0_CTL_GENBUPD_LS);
  
   // set the up/down count mode, enable the PWM generator, and make the updates locally sync to zero count (ir emitter)
  HWREG(PWM0_BASE + PWM_O_2_CTL) = (PWM_0_CTL_MODE | PWM_0_CTL_ENABLE | PWM_0_CTL_GENAUPD_LS | PWM_0_CTL_GENBUPD_LS);
  
  // set the up/down count mode, enable the PWM generator, and make the updates locally sync to zero count (servo)
  HWREG(PWM0_BASE + PWM_O_3_CTL) = (PWM_0_CTL_MODE | PWM_0_CTL_ENABLE | PWM_0_CTL_GENAUPD_LS | PWM_0_CTL_GENBUPD_LS);

}

void RestoreDC(uint8_t pin)
{
  // set the action back to the normal actions
  if (pin == 0)
  {
    HWREG(PWM0_BASE + PWM_O_0_GENA) = GenA_Normal;
  }
  else if (pin == 1)
  {
    HWREG(PWM0_BASE + PWM_O_0_GENB) = GenB_Normal;
  }
  else if (pin == FlywheelPin)
  {
    HWREG(PWM0_BASE + PWM_O_1_GENB) = GenB_Normal;
  }
  else if (pin == 7) //Pin 7 is a placeholder for ServoPin
  {
     HWREG(PWM0_BASE + PWM_O_3_GENA) = GenA_Normal;
  }
  else if (pin == IR_EmitterPin)
  {
     HWREG(PWM0_BASE + PWM_O_2_GENA) = GenA_Normal;
  }

}

// Set the Duty of specified pins (will automatically set one to the inverse of the other)
void PWM_SetDuty(uint8_t duty, uint8_t pin)
{
  if (pin == 0)
  {
    // Check if desired duty is 100
    if (duty == 100)
    {
      HWREG(PWM0_BASE + PWM_O_0_GENA) = PWM_0_GENA_ACTZERO_ONE;
    }
    else if (duty == 0) // check if 0
    {
      HWREG(PWM0_BASE + PWM_O_0_GENA) = PWM_0_GENA_ACTZERO_ZERO;
    }
    else // do normal duty set
    {
      // First restore the DC
      RestoreDC(pin);
      // Set the Duty: Period/2 - DesiredHighTime/2
      HWREG(PWM0_BASE + PWM_O_0_CMPA) = (HWREG(PWM0_BASE + PWM_O_0_LOAD)*(100 - duty)/100);
    }
  }
  
  else if (pin == 1)
  {
    // Check if desired duty is 100
    if (duty == 100)
    {
      HWREG(PWM0_BASE + PWM_O_0_GENB) = PWM_0_GENB_ACTZERO_ONE;
    }
    else if (duty == 0) // check if 0
    {
      HWREG(PWM0_BASE + PWM_O_0_GENB) = PWM_0_GENB_ACTZERO_ZERO;
    }
    else // do normal duty set
    {
      // First restore the DC
      RestoreDC(pin);
      // Set the Duty: Period/2 - DesiredHighTime/2
      HWREG(PWM0_BASE + PWM_O_0_CMPB) = (HWREG(PWM0_BASE + PWM_O_0_LOAD)*(100 - duty)/100);
    }
  }
  
  else if(pin == FlywheelPin)
  {
  //  printf("We got to setting the Flywheel Pin");
    // Check if desired duty is 100
    if (duty == 100)
    {
      HWREG(PWM0_BASE + PWM_O_1_GENB) = PWM_0_GENB_ACTZERO_ONE;
    }
    else if (duty == 0) // check if 0
    {
      HWREG(PWM0_BASE + PWM_O_1_GENB) = PWM_0_GENB_ACTZERO_ZERO;
    }
    else // do normal duty set
    {
      // First restore the DC
      RestoreDC(pin);
      // Set the Duty: Period/2 - DesiredHighTime/2
      HWREG(PWM0_BASE + PWM_O_1_CMPB) = (HWREG(PWM0_BASE + PWM_O_1_LOAD)*(100 - duty)/100);
    }
     
  }
  
    else if(pin == 7) //7 is a placeholder for ServoPin
  {
   
    // Check if desired duty is 100
    if (duty == 100)
    {
      HWREG(PWM0_BASE + PWM_O_3_GENA) = PWM_0_GENA_ACTZERO_ONE;
    }
    else if (duty == 0) // check if 0
    {
      HWREG(PWM0_BASE + PWM_O_3_GENA) = PWM_0_GENA_ACTZERO_ZERO;
    }
    else // do normal duty set
    {
      // First restore the DC
      RestoreDC(pin);
      // Set the Duty: Period/2 - DesiredHighTime/2
      HWREG(PWM0_BASE + PWM_O_3_CMPA) = (HWREG(PWM0_BASE + PWM_O_3_LOAD)*(100 - duty)/100);
    }
     
  }
  
    else if(pin == IR_EmitterPin) //7 is a placeholder for ServoPin
  {
   
    // Check if desired duty is 100
    if (duty == 100)
    {
      HWREG(PWM0_BASE + PWM_O_2_GENA) = PWM_0_GENA_ACTZERO_ONE;
    }
    else if (duty == 0) // check if 0
    {
      HWREG(PWM0_BASE + PWM_O_2_GENA) = PWM_0_GENA_ACTZERO_ZERO;
    }
    else // do normal duty set
    {
      // First restore the DC
      RestoreDC(pin);
      // Set the Duty: Period/2 - DesiredHighTime/2
      HWREG(PWM0_BASE + PWM_O_2_CMPA) = (HWREG(PWM0_BASE + PWM_O_2_LOAD)*(100 - duty)/100);
    }
     
  }

}

void PWM_SetPulseWidth(float PulseWidth){
        PulseWidthInTicks = PulseWidth * PWMTicksPerMS;
        RestoreDC(7);
      // Set the Duty: Period/2 - DesiredHighTime/2
      HWREG(PWM0_BASE + PWM_O_3_CMPA) = (HWREG(PWM0_BASE + PWM_O_3_LOAD)-(PulseWidthInTicks/2));
}