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

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

void DoSlow(int);
void DoFast(int);

#define RmotorPin 17
#define DmotorPin 19
#define RdirPin 16
#define DdirPin 18
#define FDrx 11
#define FDtx 10

fdserial *trm;
fdserial *fd; 

int b1, b2, b3, b4;

int ck;
int data;
volatile char Buffer[8];
int i;
volatile char receiveBuffer[8];
int SlowFast = 0;

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

  low(RmotorPin);                             // <--set pin as output
  low(RdirPin);                               // <-- set default direction
  low(DmotorPin);
  low(DdirPin);
  
  trm = fdserial_open(31,30,0,115200);
  fd = fdserial_open(0,1,0,115200);

  i = 0;
 
  while(1)
  {
                                            // Add main loop code here.
   i = fdserial_rxCheck(trm);
   
   i = fdserial_rxCheck(fd);               // <-- if we have serial data use it
   
   while(fdserial_rxCheck(fd) > 0)
   {
     Buffer[i++] = fdserial_rxChar(fd);  //<--Capture one of the four bytes
     pause(5);
   } 
   
   if(i > 0)                             // <--we have serial data
   {
     if (i ==4)
      {                          //<--valid serial packet
       data = 'A';
      }       
     else
      {
       data = ' ';
      }       
       i = 0;
      print("Buffer[0]: %d, Buffer[1]: %d, Buffer[2]: %d, Buffer[3]: %d\n",
          Buffer[0], Buffer[1], Buffer[2], Buffer[3]);
     } 
   
   b1 = input(4);                  //<--get button input which overrides serial
   b2 = input(5);
   b3 = input(6); 
   b4 = input(7);
   
   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);
       DoSlow(RmotorPin);
       if(b1 == 0)
        data = ' ';
       break;
       
     case 'N':
       high(DdirPin);
       DoSlow(DmotorPin);
       if(b2 == 0)
         data = ' ';
       break;
       
     case 'E':
       
       low(RdirPin);
       DoSlow(RmotorPin);
       if (b3 == 0)
         data = ' ';
       break;
       
     case 'S':
       
       low(DdirPin);
       DoSlow(DmotorPin);
       if (b4 == 0)
         data = ' ';
       break;
       
 
     case 'A': 
     
       SlowFast = 1;
       break;
       
     default: //motors stop
       
       low(RdirPin);
       low(DdirPin);
       break;
   }   
     
    pause(10);
  }  
}

void DoSlow(int motor)
{
  for (int i=0;i<6;i++)
  {
    high(motor);
    low(motor);
    pause(167);
  }
}

void DoFast(int motor)
{
  for (int i=0;i<100;i++)
  {
    high(motor);
    low(motor);
    pause(10);
  }
}