/*
  Blank Simple Project.c
  http://learn.parallax.com/propeller-c-tutorials 
*/
#include "simpletools.h" 
#include "propeller.h"                     // Include simple tools
#include "fdserial.h"

void convert(void);  //<-- add this

#define RmotorPin 17
#define DmotorPin 16
#define RdirPin 18
#define DdirPin 15
#define FDRX 0
#define FDTX 1


fdserial *fd;
int b1, b2, b3, b4;

int ck;
int data;
char Buffer[32];

int i;  

int main()
{ 
  low(RmotorPin);                 //<-- set pin as output
  low(RdirPin);                   //<-- default direction
  low(DmotorPin);
  low(DdirPin);
  
  fd = fdserial_open(FDRX, FDTX, 0, 115200);
  
  //clkset(0x6b,5000000);           //<-- Not needed, does nothing.
  
  pwm_start(600);                 //<-- generate a pulse every 600 milliseconds
  //pwm_start(20000);
 
  i = 0;
  while(1)
  {
    while(fdserial_rxCount(fd) > 0) //<-- check if we have data
    {
      Buffer[i++] = fdserial_rxChar(fd); //<-- Capture one of the four bytes
      pause(5); //<-- wait a bit for more data
    }
    
    if (i > 0)                   //<-- we have serial data
    {
      if (i == 4)
      {
        data = 'A';
      }
      else
        data = ' ';

      i = 0;
      printi("%d,%d,%d,%d\n", Buffer[0], Buffer[1], Buffer[2], Buffer[3]);
    }

    b1 = input(3);               //<-- get button input which overrides serial
    b2 = input(4);
    b3 = input(5);
    b4 = input(6); 
    
    if (b1 == 1)
      data = 'W';
     
    if (b2 == 1)
      data = 'N';
     
    if (b3 == 1)
      data = 'E';
     
    if(b4 == 1)
      data = 'S';
     
    switch(data)
    {
      case 'W':
         high(RdirPin);                //<-- set direction before starting motor
         pwm_set(RmotorPin,0,500);     //<-- turn on motor to 83% - 500/600
         if (b1 == 0)                  //<-- button released
          data = ' ';
         break;
         
      case 'N': 
         high(DdirPin);
         pwm_set(DmotorPin,1,500);
         if (b2 == 0)
          data = ' ';
         break;
         
      case 'E':
         low(RdirPin);
         pwm_set(RmotorPin,0,500);
         if (b3 == 0)
          data = ' ';
         break;
         
      case 'S':
         low(DdirPin);
         pwm_set(DmotorPin,1,500);
         if (b4 == 0)
          data = ' ';
         break;
         
      case 'A':
         set_output(RdirPin, Buffer[1]);
         set_output(DdirPin, Buffer[3]);
         pwm_set(RmotorPin, 0, Buffer[0]);
         pwm_set(DmotorPin, 1, Buffer[2]);
         break;
         
      default:                          //<-- no buttons or serial data all stop
         pwm_set(RmotorPin,0,50);       //<-- set motor speed to 8% - 50/600
         pwm_set(DmotorPin,1, 0);       //<-- set motor to stop 0% - 0/600
         low(RdirPin);                  //<-- set direction to default
         low(DdirPin);
         break;
    }
    pause(250);                        //<-- wait 250 milliseconds or 1/4 of a second.
  }
}

void convert() //<-- add this function
{
  int Dp, Dd, Rp, Rd;
  
  sscanf(Buffer, "%d,%d,%d,%d", &Dp, &Dd, &Rp, &Rd);
  Buffer[0] = Dp;
  Buffer[1] = Dd;
  Buffer[2] = Rp;
  Buffer[3] = Rd;
  Buffer[4] = 0;
}