#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"
#include <termios.h>
#define _GNU_SOURCE
#include <string.h>
#include <errno.h>
#include <stdlib.h>
#include "servocmd.h"

typedef struct group_s {

   char * name;
   unsigned int channels[3];

} group_t;   
   


servoBus_t   servoBus;
unsigned int positions[32];
char         updates[32];
  
group_t  Legs[6];
unsigned int currleg;
  
void statem(unsigned char in);
enum CMDCODES {INITPING = 0, PING, LISTEN, IGNORE, POWERUP, POWERDOWN, SETPOSITION, GETPOSITION, NU1, NU2, NU3, NU4, NU5, NU6, NU7, GETMODEL };
   

   
void statem(unsigned char in) {

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

  static enum state_t state = SYNC;
  static unsigned char channel;
  static unsigned int 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);
      value = value << 8;
      value |= in;
      positions[channel & 31] = value;      
      updates[channel & 31] ++;
      state = SYNC;
      break; 
  }


}



int gencommand( char * buffer, int inbytes) {b


  int i ,j;
  char * tempbuff;

  
  tempbuff = strndup(buffer, inbytes);

  for (i = 0; i < inbytes; i++) {
    
    switch(tempbuff[i]) {
    
      
    
      case 'A':  // 0 on
        OneTimeListen( &servoBus, legs[currleg].channels[0] );  // tell servo to listen to following command
        SetFlags( &servoBus, 1<<SERVOON); // send command
      break;
      
      case 'a': // 0 off
        OneTimeListen( &servoBus, legs[currleg].channels[0]);  // tell servo to listen to following command
        SetFlags( &servoBus, 1<<SERVOOFF); // send command
      break;
      
      case 'B': // 1 on
        OneTimeListen( &servoBus, legs[currleg].channels[1]);  // tell servo to listen to following command
        SetFlags( &servoBus, 1<<SERVOON); // send command
      break;
      
      case 'b':  // 1 off
        OneTimeListen( &servoBus, legs[currleg].channels[1]);  // tell servo to listen to following command
        SetFlags( &servoBus, 1<<SERVOOFF); // send command
      break;
      
      case 'C': // 2 on
        OneTimeListen( &servoBus, legs[currleg].channels[2]);  // tell servo to listen to following command
        SetFlags( &servoBus, 1<<SERVOON); // send command
      break;
      
      case 'c': // 2 off
        OneTimeListen( &servoBus, legs[currleg].channels[2]);  // tell servo to listen to following command
        SetFlags( &servoBus, 1<<SERVOOFF); // send command
      break;
      
      case '7': 
        positions[0]++;
        OneTimeListen( &servoBus, legs[currleg].channels[0]);  // tell servo to listen to following command
        SetServoPostion( &servoBus, positions[0] );        
      break;
      
      case '1': 
       positions[0]--;
       OneTimeListen( &servoBus, legs[currleg].channels[0]);  // tell servo to listen to following command
       SetServoPostion ( &servoBus, positions[0]);
      break;
      
      case '8': 
       positions[1]++;
       OneTimeListen( &servoBus, legs[currleg].channels[1]);  // tell servo to listen to following command
       SetServoPostion ( &servoBus, positions[1]);
      break;
      
      case '2': 
       positions[1]--;
       OneTimeListen( &servoBus, legs[currleg].channels[1]);  // tell servo to listen to following command
       SetServoPostion ( &servoBus, positions[1]);
      break;
      
      case '9': 
       positions[2]++;
       OneTimeListen( &servoBus, legs[currleg].channels[2]);  // tell servo to listen to following command
       SetServoPostion ( &servoBus, positions[2]);
      break;
      
      case '3': 
       positions[2]--;
       OneTimeListen( &servoBus, legs[currleg].channels[2]);  // tell servo to listen to following command
       SetServoPostion ( &servoBus, positions[2]);
      break;
    }
    

    
    
    
  }
  
  free(tempbuff);
  return j;
}

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

   int ctrlport, robotport;
   int i; //, j;
   int  bytes;
   char buffer[1024];
 //  char sendbuff[1024];
  // char * cb = NULL;

   fd_set rfds;
   struct timeval tv;
   int retval;
   struct termios tio;
   
   legs[0].channels = {00,01,02};
   legs[1].channels = {04,05,06};
   legs[2].channels = {08,09,10};
   legs[3].channels = {12,13,14};
   legs[4].channels = {03,07,16};
   legs[5].channels = {11,15,17};
   
   currleg = 0;
   
   // set stdin to realtime
    if ( tcgetattr( fileno( stdin ), &tio ) < 0 )    {
      fprintf( stderr, "Unable to retrieve terminal settings: %s\n", strerror( errno ));
      exit( 5 );
    }

    tio.c_lflag &= ~( ICANON | ECHO );
    tio.c_cc[VTIME] = 0;
    tio.c_cc[VMIN] = 1;

    if ( tcsetattr( fileno( stdin ), TCSANOW, &tio ) < 0 )    {
      fprintf( stderr, "Unable to update terminal settings: %s\n", strerror( errno ));
      exit( 6 );
    }      
   
   if ((ctrlport = initPort("/dev/ttyUSB0", B9600)) == -1) {
      printf("Unable to open manual control port \n ");
      return 1;
   } 
   
   if ((robotport = initPort("/dev/ttyS0",B9600)) == -1) {
      printf("Unable to open robot port \n ");
      return 1;
   }
   
   servoBus.busFD = robotport;
   
   
   positions[0] = 512;
   positions[1] = 512;
   positions[2] = 512;
   
   printf("\
'A' servo A on\n\
'a' servo A off\n\
'B' servo B on\n\
'b' servo B off\n\
'C' servo C on\n\
'c' servo C off\n\
+----------+\n\
 7   8   9\n\
 ^   ^   ^\n\
 A   B   C\n\
 v   v   v\n\
 1   2   3\n\
+----------+\n\
Positions:\n");
      
   while (1) {
      FD_ZERO(&rfds);
      FD_SET(ctrlport, &rfds);
      FD_SET(0, &rfds);
      tv.tv_sec  = 0 ;
      tv.tv_usec = 250000;
      retval = select(ctrlport+1, &rfds, NULL, NULL, &tv);

      if (retval == -1) {
            perror("select()");
            return (-1);
      } else if (retval) {              
         if ( FD_ISSET(ctrlport, &rfds) ) {            
            bytes = read(ctrlport, &buffer , 1023 );
            if (bytes != 0) {
              for( i = 0; i < bytes; i++) {
                statem(buffer[i]);                             
	      }	      
            }
         }
	 if (FD_ISSET(0, &rfds)) {
	   bytes = read(0, &buffer , 1023 );
	   bytes = gencommand( buffer, bytes);
	//   printf("%d :", bytes);
	 //  printf("\n");
	   write(robotport, buffer, bytes);	   

	 }
	 
      }
      
      for(i = 0; i < 3; i++) {
     //   if (updates[i] > 8) {
	//  updates[i] = 0;
	  
	  OneTimeListen     ( &servoBus, i ) ;
	  SetServoPostion   ( &servoBus, positions[i] ); 
	  
	//}
      }
   
    
      printf("0:%04u  1:%04u 2:%04u \r", positions[0], positions[1], positions[2]);
      
      fflush(stdout);
   } 
   
   close(ctrlport);
   close(robotport);
   return 0;
}