//
// BlimpBot
// CSE 466 final project
// Josh Green, Baron Oldenburg, Henry Baba-Weiss
//
#include "bsp.h"
#include "mrfi.h"
#include "nwk_types.h"
#include "nwk_api.h"
#include "bsp_leds.h"
#include "bsp_buttons.h"
#include "../virtual_com_cmds.h"

#include "app_remap_led.h"

#define SPIN_ABOUT_A_SECOND NWK_DELAY(1000)

static void uartSendByte(char byte);
static uint8_t sRxCallback(linkID_t);
static void setupLink(void);

/* UART RX ISR */
__interrupt void USCI0RX_ISR(void);

/* link ID */
static linkID_t sLinkID2 = 0;

/* State machine for how we should interpret the UART responses from the blimp */
typedef enum {
  STATE_COMMAND,
  STATE_WAITING_IR,
  STATE_SENDING_IR
} UARTReceiveState;

static UARTReceiveState uartReceiveState = STATE_COMMAND;

/* response packet, 0xFF indicates waiting */
static char response[8] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
static char response_len = 1;

void main (void)
{
  // Enable pin for the Imperial March
  P2DIR |= 0x02;
  P2OUT &= ~0x02;

  BSP_Init();

  /* initialize UART */
  COM_Init();

  /* This call will fail because the join will fail since there is no Access Point
   * in this scenario. But we don't care -- just use the default link token later.
   * We supply a callback pointer to handle the message returned by the peer.
   */
  SMPL_Init(sRxCallback);

  /* set up static radio link. */
  setupLink();

  /* main control loop */
  while (1) {
    if (BSP_LED1_IS_ON())
      BSP_TOGGLE_LED1();
    if (BSP_LED2_IS_ON())
      BSP_TOGGLE_LED2();

    // wait for a response packet
    if (*response != 0xFF) {
      while (SMPL_SendOpt(sLinkID2, response, response_len, SMPL_TXOPTION_ACKREQ) != SMPL_SUCCESS)
        ;

      *response = 0xFF;
    }
  }
}

// Sets up the static wireless link to the base station
static void setupLink()
{
  addr_t baseStationAddr;
  baseStationAddr.addr[0] = 0x41;
  baseStationAddr.addr[1] = 0x41;
  baseStationAddr.addr[2] = 0x41;
  baseStationAddr.addr[3] = 0x41;

  while (SMPL_Commission(&baseStationAddr, 0x3E, 0x3E, &sLinkID2) != SMPL_SUCCESS)
  	;

  /* turn on RX. default is RX off. */
  SMPL_Ioctl( IOCTL_OBJ_RADIO, IOCTL_ACT_RADIO_RXON, 0 );
}

// Callback to handle received messages over wireless from the base station
static uint8_t sRxCallback(linkID_t port)
{
  uint8_t msg[8], len;

  /* is the callback for the link ID we want to handle? */
  if (port == sLinkID2)
  {
    /* yes. go get the frame. we know this call will succeed. */
    if ((SMPL_Receive(sLinkID2, msg, &len) == SMPL_SUCCESS) && len)
    {
      // Sanity check; show that we've received a packet
      if (!BSP_LED1_IS_ON())
        BSP_TOGGLE_LED1();

      // Default response is just one byte; certain commands will
      // update this if needed in the switch statement
      response_len = 1;

      switch ((packetType)*msg) {
        case MOTOR_RESPONSE:
          *response = MOTOR_RESPONSE;  // Just send an acknowledge packet for now

          uartSendByte(*(msg + 1));

          // Hack because left/right have no speed associated with them
          if (*(msg + 1) != UART_LEFT && *(msg + 1) != UART_RIGHT)
            uartSendByte(*(msg + 2));
          break;
        case SET_AUTOHEIGHT_RESPONSE:
          *response = SET_AUTOHEIGHT_RESPONSE;
          uartSendByte(UART_SET_AUTOHEIGHT);
          break;
        case TOGGLE_AUTOHEIGHT_RESPONSE:
          *response = TOGGLE_AUTOHEIGHT_RESPONSE;
          uartSendByte(UART_AUTOHEIGHT);
          break;
        case IR_RESPONSE:
          *response = 0xFF;  // Wait for response from the blimp over UART
          uartReceiveState = STATE_WAITING_IR;
          uartSendByte(*(msg + 1));
          break;
        case EFIELD_RESPONSE:
          *response = EFIELD_RESPONSE;
          break;
        case COMPASS_RESPONSE:
          *response = COMPASS_RESPONSE;
          break;
        case BATTERY_RESPONSE:
          *response = BATTERY_RESPONSE;
          break;
        case ACTIVATE_RESPONSE:
          *response = ACTIVATE_RESPONSE;
          uartSendByte(UART_ACTIVATE);
          break;
        case DEACTIVATE_RESPONSE:
          *response = DEACTIVATE_RESPONSE;
          uartSendByte(UART_DEACTIVATE);
          break;
        case SONG_PLAY_ONCE_RESPONSE:
          *response = SONG_PLAY_ONCE_RESPONSE;
          P2OUT |= 0x02;
          __delay_cycles(100);
          P2OUT &= ~0x02;
          break;
        case SONG_TOGGLE_LOOP_RESPONSE:
          *response = SONG_TOGGLE_LOOP_RESPONSE;
          P2OUT ^= 0x02;
          break;
        default:
          return 0;
      }

      return 1;
    }
  }
  /* keep frame for later handling */
  return 0;
}

/*------------------------------------------------------------------------------
* USCIA interrupt service routine
------------------------------------------------------------------------------*/
#pragma vector=USCIAB0RX_VECTOR
__interrupt void USCI0RX_ISR(void)
{
  static char response_buf[8] = {0};
  static int cur_byte = 0;
  int i;
  char ch = UCA0RXBUF;

  // Sanity check; show that we've received a UART command
  if (!BSP_LED2_IS_ON())
    BSP_TOGGLE_LED2();

  if (uartReceiveState == STATE_WAITING_IR)
  {
    switch(ch)
    {
      case UART_IR_FORWARD:
      case UART_IR_BACK:
      case UART_IR_LEFT:
      case UART_IR_RIGHT:
      case UART_IR_DOWN:
        response_buf[0] = IR_RESPONSE;
        response_buf[1] = ch;

        cur_byte = 2;
        uartReceiveState = STATE_SENDING_IR;
        break;
    }
  }
  else if (uartReceiveState == STATE_SENDING_IR)
  {
    response_buf[cur_byte] = ch;
    cur_byte++;

    if (ch == 0)
    {
      uartReceiveState = STATE_COMMAND;
      response_len = cur_byte;  // cur_byte is one past the last char in the string, which is the len

      for (i = 0; i < cur_byte; i++)
        response[i] = response_buf[i];

      cur_byte = 0;
    }
  }
}

// Convenience method to send one byte of data over the UART
static void uartSendByte(char byte)
{
  char str[2] = {0};
  str[0] = byte;

  TXString(str, 1);
}