#include <msp430x14x.h>
#include <stdio.h>

#include "basic.h"
#include "config.h"
#include "cmdparse.h"
#include "gpio.h"

#define GPIO_PORT4IO_LED    (0x80)

#define GPIO_PORT4IO_HISIDE (0x20)
#define GPIO_PORT6IO_GFTEST (0x04)

static boolean hiSideState;
static boolean gfTestState;
static boolean ledState;

int gftestOnHandler(char *cmdStr);
int gftestOffHandler(char *cmdStr);
int hisideOnHandler(char *cmdStr);
int hisideOffHandler(char *cmdStr);

void gpioInit(void)
{


  _DINT();// Disable Interrupts

  //
  // [Ports]
  //
#if 0
  P1DIR=0xFF;
  P1OUT=0x00;
  P1SEL=0x00;
  P1IES=0x00;
  P1IE=0x00;

  P2DIR=0xFF;
  P2OUT=0x00;
  P2SEL=0x00;
  P2IES=0x00;
  P2IE=0x00;

  P3DIR=0x00;
  P3OUT=0x00;
  P3SEL=0xF0;

  P4DIR=0xFF;
  P4OUT=0x00;
  P4SEL=0x00;

  P5DIR=0x7A;
  P5OUT=0x00;
  P5SEL=0x0F;

  P6DIR=0xFF;
  P6OUT=0x00;
  P6SEL=0x00;
  _EINT();// Enable Interrupts

#else
// sidearm
  P1DIR = 0x00;
  P1OUT = 0x00;
  P1SEL = 0x00;

  P2DIR = 0x01;
  P2OUT = 0x00;
  P2SEL = 0x00;

  P3DIR = 0x00;
  P3OUT = 0x00;
  P3SEL = 0xF0;

  P4DIR = 0xEB;
  P4OUT = 0x00;
  P4SEL = 0x00;

  P5DIR = 0x40;
  P5OUT = 0x00;
  P5SEL = 0x40;

  P6DIR = 0x04;
  P6OUT = 0x00;
  P6SEL = 0x08;
  
#endif

  hiSideState = false;
  gfTestState = false;
  ledState = false;

  cmdparseAddCommand("gftest", "on", gftestOnHandler);
  cmdparseAddCommand("gftest", "off", gftestOffHandler); 

  cmdparseAddCommand("hiside", "on", hisideOnHandler);
  cmdparseAddCommand("hiside", "off", hisideOffHandler); 

}

void gpioSetLed(boolean onOff)
{
  if (onOff == ledState)
    return;

  if (onOff == true)
    P4OUT |= GPIO_PORT4IO_LED;
  else
    P4OUT &= ~GPIO_PORT4IO_LED;
  
  ledState = onOff;
}

void gpioSetAttn(boolean onOff)
{
  return;
}

void gpioSetHiSide(boolean onOff)
{
  if (onOff == hiSideState)
    return;

  if (onOff == true)
    P4OUT |= GPIO_PORT4IO_HISIDE;
  else
    P4OUT &= ~GPIO_PORT4IO_HISIDE;

  hiSideState = onOff;
}

void gpioSetGfTest(boolean onOff)
{
  if (onOff == gfTestState)
    return;

  if (onOff == true)
    P6OUT |= GPIO_PORT6IO_GFTEST;
  else
    P6OUT &= ~GPIO_PORT6IO_GFTEST;

  gfTestState = onOff;
}


int gftestOnHandler(char *cmdStr)
{
  gpioSetGfTest(true);
  return 0;
}

int gftestOffHandler(char *cmdStr)
{
  gpioSetGfTest(false);
  return 0;
}

int hisideOnHandler(char *cmdStr)
{
  gpioSetHiSide(true);
  gpioSetLed(true);
  return 0;
}

int hisideOffHandler(char *cmdStr)
{
  gpioSetHiSide(false);
  gpioSetLed(false);
  return 0;
}