#include <sys/time.h>
#include <sys/types.h>
#include <stdio.h>   /* Standard input/output definitions */
#include <unistd.h>  /* UNIX standard function definitions */
#include "ssls.h"


unsigned int positions[32];
  
void statem(char in) ;
 
 
 
void statem(char in) {

 enum state_t { SYNC, CHANNEL, VALUE1, VALUE2 };

  static enum state_t state = SYNC;
  static char channel, value;

  switch(state) {
    case SYNC:
      if (in == 0) state = CHANNEL;
      break;
    case CHANNEL:
      if (in > 4) {   // sync error
        state = SYNC;
        break;
      }
      channel = in;
      state = VALUE1;
      break;
    case VALUE1:
      value = in;
      state = VALUE2;
      break;
    case VALUE2:
      //printf("\nChannel: %02d :Value %04u", channel, (value<<8)|in);
      positions[channel&31] = ((unsigned int)value<<8)|(unsigned int)in;
      state = SYNC;
      break; 
  }


}



int main(int argc, char **argv) {

   int fd, i;
   int  bytes;
   char buffer[1024];
   char * cb = NULL;
   

   fd_set rfds;
   struct timeval tv;
   int retval;
   
   
   if ((fd = initPort("/dev/ttyS0")) == -1) {
      printf("Unable to open serial port \n ");
      return 1;
   } 
   
      
   while (1) {
      FD_ZERO(&rfds);
      FD_SET(fd, &rfds);
      tv.tv_sec  = 0 ;
      tv.tv_usec = 250000;
      retval = select(fd+1, &rfds, NULL, NULL, &tv);

      if (retval == -1) {
            perror("select()");
            return (-1);
      } else if (retval) {      
         if ( FD_ISSET(fd, &rfds) ) {            
            bytes = read(fd, &buffer , 1023 );
            if (bytes != 0) {
          //    printf("Bytes > %d: ", bytes);
              for( i = 0; i < bytes; i++) {
                statem(buffer[i]);                               
	      }
            }
         }
      }
      
      printf("0:%04u  1:%04u 2:%04u 3:%04u\r", positions[0], positions[1], positions[2], positions[3]);
      fflush(stdout);
   } 
   
   close(fd);
   return 0;
}