#ifndef __BORLANDC__
#include <msp430x14x.h>
#endif
#include <stdio.h>

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

#define GPIO_PORT4IO_LED    (0x80)
#define GPIO_PORT4IO_ATTN   (0x40)
#define GPIO_PORT4IO_HISIDE (0x20)
#define GPIO_PORT6IO_GFTEST (0x04)
#define GPIO_PORT2IO_KVHPOWER (0x01)

static boolean ledState;
static boolean attnState;
static boolean hiSideState;
static boolean gfTestState;
static boolean kvhPowerState;

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

int kvhOnHandler(char *cmdStr)
{
  gpioSetKvhPower(true);
  return 0;
}

int kvhOffHandler(char *cmdStr)
{
  gpioSetKvhPower(false);
  return 0;
}

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

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

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

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

void gpioInit(void)
{
#ifndef __BORLANDC__
#ifdef CONFIG_SIDEARM_HW_REV4
  P1DIR = 0x00;
  P1OUT = 0x00;
  P1SEL = 0x00;

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

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

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

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

  P6DIR = 0x04;
  P6OUT = 0x00;
  P6SEL = 0x0B;
    
#else
#error "GPIO not initialized!"
#endif
#endif

  ledState = false;
  attnState = false;
  hiSideState = false;
  gfTestState = false;
  kvhPowerState = false;

  cmdparseAddCommand("kvh", "on", kvhOnHandler);
  cmdparseAddCommand("kvh", "off", kvhOffHandler);

  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)
{
  if (onOff == attnState)
    return;
    
  if (onOff == true)
    P4OUT |= GPIO_PORT4IO_ATTN;
  else
    P4OUT &= ~GPIO_PORT4IO_ATTN;

  attnState = onOff;
}

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;
}

void gpioSetKvhPower(boolean onOff)
{
  if (onOff == kvhPowerState)
    return;

  if (onOff == true)
    P2OUT |= GPIO_PORT2IO_KVHPOWER;
  else
    P2OUT &= ~GPIO_PORT2IO_KVHPOWER;

  kvhPowerState = onOff;
}