#include "system.h"
#include "haptics.h"

#define EN_PIN       BIT6
#define PWM_PIN      BIT5

static unsigned char index = 0;
static unsigned char timer_counter = 0;
static unsigned char timer_counter_expected = 0;
static const Waveform *pHapticsWaveform = 0;

void haptics_init(void)
{
   P4DIR |= EN_PIN;          // P4.6 Output
   P4OUT &= ~(EN_PIN);       // Disable Amplifier
   
   P4DIR |= PWM_PIN;         // P4.5 Output
}

void haptics_send_waveform(const Waveform *pWaveform)
{
  pHapticsWaveform = pWaveform;
  index = 0;
  
  P4OUT |= EN_PIN;          	             //Enable Amplifier, Start PWM
 
  if (pWaveform->data[0] == 0x01)
  {
    P4OUT |= PWM_PIN;
  }
  else
  {
    P4OUT &= ~(PWM_PIN); 
  }

  timer_counter = 0;
  timer_counter_expected = pWaveform->data[1];
  
  WDTCTL = WDT_MDLY_0_5;                       // WDT 4ms, interval timer
  IE1 |= WDTIE;                                // Enable WDT interrupt
}

// Watchdog Timer interrupt service routine
#pragma vector=WDT_VECTOR
__interrupt void watchdog_timer_isr(void)
{
   timer_counter++;
   
   if (timer_counter == timer_counter_expected)
   {
      timer_counter = 0;
      
      index += 2;          
      if (index < pHapticsWaveform->length)
      {
          P1OUT |= EN_PIN;          	             //Enable Amplifier, Start PWM
      
          if (pHapticsWaveform->data[index] == 0x01)
          {
             P4OUT |= PWM_PIN;
          }
          else
          {
             P4OUT &= ~(PWM_PIN); 
          }
          
          timer_counter_expected = pHapticsWaveform->data[index + 1];
      }
      else
      {
          P4OUT &= (~EN_PIN);                // Disable Amplifier          
          P4OUT &= ~(PWM_PIN);               // PWM Output = LOW
            
          IE1 &= (~WDTIE);  
          WDTCTL = WDTPW + WDTHOLD;
      }
   }
}

