/**
 * @brief Telescope driver
 * @date 
 * @version
 *
*/

#include "simpletools.h"                   // Include simple tools
#include "propeller.h"
#include "fdserial.h"

void checkButtons(void);


#define RmotorPin 17
#define DmotorPin 19
#define RdirPin   16 // West = high
#define DdirPin   18 // North= high
#define FDtx 10
#define FDrx 11

#define westPin    4
#define northPin   5
#define eastPin    6
#define southPin   7
#define modePin    ?


fdserial *fd; 

int b1, b2, b3, b4;
int Rspeed[] = {10, 40, 60, 80}; // <-- speed or pulse per second
int Dspeed[] = {8, 40, 60, 80};  // <-- speed or pulse per second
int ck;
char data;
char mode;
char Buffer[8];
int i;
char receiveBuffer[8];


int main()                                    // Main function
{                                             // add startup code here

  low(RmotorPin);                             // <--set pin as output
  low(RdirPin);                               // <-- set default direction
  low(DmotorPin);
  low(DdirPin);
  
  b1 = input(westPin);
  b2 = input(northPin);
  b3 = input(eastPin);
  b4 = input(southPin);
  
  fd = fdserial_open(FDrx,FDtx, 0, 115200);

  while(1)
  {
    i = 0;                                // Add main loop code here.
    /**
     * Hand control sends 3 bytes MD<nl>
     * M - mode value 0 - 3
     * D - Direction (W N E S)
     */
    while(fdserial_rxCount(fd) > 0)
    {
      Buffer[i++] = fdserial_rxChar(fd);  //<--Capture one of the four bytes
      pause(50);
    } 

    if (i == 3)                          // <--valid serial packet
    {
      mode = Buffer[0] - '0';            // <--make numeric value from text
      data = Buffer[1];
      print("Mode: %c, Direction: %c \n", Buffer[0], Buffer[1]);
    }
    else
    {
      mode = 0;
      data = ' ';
    }
    i = 0;
  
    checkButtons(); //<-- check buttons which override serial

    switch(data)
    {
       case 'W':
         high(RdirPin); // West movement
         square_wave(RmotorPin, 0, Rspeed[mode]);
         break;

       case 'N':
         high(DdirPin); // North movement
         square_wave(DmotorPin, 1, Dspeed[mode]);
         break;

       case 'E':
         low(RdirPin); // East movement
         square_wave(RmotorPin, 0, Rspeed[mode]);
         break;

       case 'S':
         low(DdirPin); // South movement
         square_wave(DmotorPin, 1, Dspeed[mode]);
         break;
   }   
     
    pause(500);
    square_wave_stop();
  }
}

/**
 * Check if one of the local buttons was pushed
 */
void checkButtons()
{
  int i;
  
  i = input(westPin);
  if (i != b1)
  {
    if (i == 0)
    {
      data = 'W';
    }
    b1 = i;
  }

  i = input(northPin);
  if (i != b2)
  {
    if (i == 0)
    {
      data = 'N';
    }
    b2 = i;
  }

  i = input(eastPin);
  if (i != b3)
  {
    if (i == 0)
    {
      data = 'E';
    }
    b3 = i;
  }

  i = input(southPin);
  if (i != b4)
  {
    if (i == 0)
    {
      data = 'S';
    }
    b4 = i;
  }
}