/**
 * @brief Hand Control Program for Telescope
 * @author 
 * @date March 6, 2022
 * @version 1.0
 * 
*/

#include "simpletools.h"
#include "serial.h"

void setup(void);
void setModeLeds(int);
void readModePin(void);
void readDirectionPins(void);


#define westPin    0
#define northPin   1
#define eastPin    2
#define southPin   3
#define modePin    4
#define lowLedPin  7
#define med1LedPin 8
#define med2LedPin 9
#define highLedPin 10

#define rx 5
#define tx 6


char ModeSwitch = 0;
char West, North, East, South;
char direction = 0;
char mode = 0;
serial *conn;

int main()
{
  int i;
  
  setup();

  while(1)
  {
    readModePin();
    readDirectionPins();
    if (direction != 0)
    {
      writeDec(conn, mode);
      writeChar(conn, direction);
      writeChar(conn, '\n');
      direction = 0;
    }
    pause(50);
  }  
}

void setup()
{
  setModeLeds(0);
  conn = serial_open(rx, tx, 0, 115200);
  ModeSwitch = input(modePin);
  West = input(westPin);
  North = input(northPin);
  East = input(eastPin);
  South = input(southPin);
}

/**
 * turn on led of current mode value
 */
void setModeLeds(int m)
{
  m = m + lowLedPin;
  
  low(lowLedPin);
  low(med1LedPin);
  low(med2LedPin);
  low(highLedPin);

  switch (m)
  {
    case 0: high(lowLedPin);
            break;
    case 1: high(med1LedPin);
            break;
    case 2: high(med2LedPin);
            break;
    case 3: high(highLedPin);
            break;
  }
}

/**
 * mode = 0 to 3
 */
void readModePin()
{
  int i;
  
  i = input(modePin);
  if (i != ModeSwitch)
  {
    if (i == 0)
    {
      mode++;
      mode = mode & 3;
      setModeLeds(mode);
    }
    ModeSwitch = i;
  }
}

/**
 * get move direction
 * 0 - none
 * 1 - west
 * 2 - north
 * 3 - east
 * 4 - south
 */
void readDirectionPins()
{
  int i;
  
  i = input(westPin);
  if (i != West)
  {
    if (i == 0)
      direction = 'W';
    West = i;
  }
  i = input(northPin);
  if (i != North)
  {
    if (i == 0)
      direction = 'N';
    North = i;
  }
  i = input(eastPin);
  if (i != East)
  {
    if (i == 0)
      direction = 'E';
    East = i;
  }
  i = input(southPin);
  if (i != South)
  {
    if (i == 0)
      direction = 'S';
    South = i;
  }
}