#include <io430x16x.h>
#include <intrinsics.h>
#include "globals.h"

int main(void)
{
  // General settings
  WDTCTL = WDTPW + WDTHOLD;// + WDTTMSEL;  // Stop watchdog timer
  DCOCTL |= DCO2 + DCO0;               // set internal DCO to maximum frequency (8 MHz)
  BCSCTL1 |= RSEL2 + RSEL1 + RSEL0;    // set internal DCO to maximum frequency (8 MHz)
  BCSCTL1 |= XT2OFF + XTS + DIVA_0;     // XT2 off (+XTS if crystal1 present)

  P1DIR |= 0x01;                        // Set P1.0 to output direction
  P2DIR |= 0x1F;                        // Set P2.0-P2.1 to output direction (RF90, RF180, Gx, Gy, Gz)
                                        // (Gy changes to DAC output later)
  P3SEL |= 0x30;                        // P3.4,5 = USART0 TXD/RXD
  P5SEL |= 0x70;                        // output ACLK, MCLK and SMCLK to pins (just for measurement purposes)

  // Check OSCFault flag
  unsigned int j;
  do
  {
  IFG1 &= ~OFIFG;                           // Clear OSCFault flag
  for (j = 0xFF; j > 0; j--);               // Time for flag to set
  }
  while ((IFG1 & OFIFG));                   // OSCFault flag still set?

  BCSCTL2 = SELM_3 + DIVM_0 + DIVS_0; //+ SELS                 // MCLK sourced from crystal1

  // UART settings
  UCTL0 |= SWRST + CHAR + PENA;             // 8-bit characters, odd parity
  ME1 |= UTXE0 + URXE0;                     // Enable USART0 TXD/RXD
  UTCTL0 |= SSEL0;                          // UCLK= ACLK
  //UTCTL0 |= SSEL1;                        // UCLK= SMCLK
  UBR00 = 0xA0;                             // 4MHz 0x22 115200 (0x68 38400,0xA0 9600)
  UBR10 = 0x01;                             // 4MHz 0x00 115200
  UMCTL0 = 0x5B;                            // 4MHz 0xDD 115200 modulation
  UCTL0 &= ~SWRST;                          // Initialize USART state machine
  IE1 |= URXIE0;                            // Enable USART0 RX/TX interrupt (URXIE0 has to be cleared when using DMA)

  
  /* wait forever */
  for (;;)
  {
    // wait for timer or ADC interrupts
  }
  return 0;
}

/* Timer B0 interrupt service routine */
#pragma vector=TIMERB0_VECTOR
__interrupt void Timer_B (void)
{
}

#pragma vector=ADC_VECTOR
__interrupt void ADC12ISR (void)
{
}

#pragma vector=UART0RX_VECTOR
__interrupt void usart0_rx (void)
{

  __disable_interrupt();

  if (configMode > 0)
  {
    URCTL0 &= ~URXWIE;
    switch(configMode)
    {
    case 1:
      tEcho = RXBUF0;
      configMode++;
      break;
    case 2:
      tEcho = (tEcho << 8) | RXBUF0;
      configMode++;
      break;
    case 3:
      tRep = RXBUF0;
      configMode++;
      break;
    case 4:
      tRep = (tRep << 8) | RXBUF0;
      configMode++;
      break;
    case 5:
      tRF90 = RXBUF0;
      configMode++;
      break;
    case 6:
      tRF180 = RXBUF0;
      configMode++;
      break;
    case 7:
      nrOfCycles = RXBUF0;
      configMode++;
      break;
   case 8:
      Gx = RXBUF0;
      configMode++;
      break;
    case 9:
      Gx = (Gx << 8) | RXBUF0;
      configMode++;
      break;
    case 10:
      gyNrOfCycles = RXBUF0;
      configMode++;
      break;
    case 11:
      configMode=0;
      //conversions
      tRep *= 1000; // conversion ms to us
      tRep += tA/2 + tEcho + tC;
      tRepTemp = tRep;
      tRF90adj = tRF90/1.6;
      tRF180adj = tRF180/1.6;
      tB=tC;
      break;
    }
  }
  else if (RXBUF0 == 'a')
  {
  }
  else if (RXBUF0 == 'b')
  {
  }
  else if (RXBUF0 == 'c')
  {
  }
  else if (RXBUF0 == 'z')
  {
    // setup times tR and tE
    configMode = 1;
    URCTL0 &= ~URXWIE;
  }
  else
  {
  }
}