#include <stdio.h>
#include <malloc.h>
#include "returnValues.h"
#include <unistd.h>
#include <string.h>
#include "intercmd.h"
#include "servocmd.h"
#include "ssls.h"
#include <termios.h>

// -lreadline
#include <readline/readline.h>
#include <readline/history.h>



//#define debug(k)   printf("%s\n", (k));
#define debug(k)  /* (k) */

#define REPLYSZ       512
#define VERSION       2.0



void         *console       ( ) ;
int           doCmd         ( char * string) ;
Status_t      cmdHelp       ( char *s ) ;
Status_t      cmdListen     ( char *s ) ;
Status_t      cmdIgnore     ( char *s ) ;
Status_t      cmdSetpos     ( char *s ) ;
Status_t      cmdCachepos   ( char *s ) ;
Status_t      cmdPowerup    ( char *s ) ;
Status_t      cmdPowerdown  ( char *s ) ;
Status_t      cmdGocache    ( char *s ) ;
Status_t      cmdEmstop     ( char * s) ;

servoBus_t   servoBus;

int main(int argc, char** argv) {
  int retCode = 0;
  int robotport;
  
  if ((robotport = initPort("/dev/ttyS0",B9600)) == -1) {
     printf("Unable to open robot port \n ");
     return 1;
  }
  servoBus.busFD = robotport;
  
  console();
  
  close(robotport);
  return(retCode);
}




void *console() {
 
  char *  string;
  string  = NULL;

  
  printf(" Type \"help\" for help. \n");
  do {   
   
   if (&string) { free(string); string = NULL; }
   string =  readline("> ");
   add_history (string);   
         
  } while((string != NULL) && (!doCmd(string))); 

}


int doCmd(char * string) {

// hey doc, these gotta match!
  enum cmdnums { 
         EMPTY = 0,
         HELP,
	 QUIT,
	 EXIT,
	 LISTEN,
	 IGNORE,
	 SETPOS,
	 CACHEPOS,
	 POWERUP,
	 POWERDOWN,
	 GOCACHE, 
	 EMSTOP,	 
         UNKNOWN
         
  };  
  
  static char * commands[] = { 
         " ",
         "help",
	 "quit", 
	 "exit",
	 "listen",
         "ignore",
         "setpos",
         "cachepos",
         "powerup",
         "powerdown",
         "gocache",
	 "x",
         "" 
  };
  
    chomp(string); 
       
    switch( getCommand(commands, &string)) {
    
      case HELP:      cmdHelp(NULL);         break;
	 
      case QUIT:
      case EXIT:
         return 1;
         break;
	 
      case LISTEN:    cmdListen(string);     break;	 
      case IGNORE:    cmdIgnore(string);     break;	 
      case SETPOS:    cmdSetpos(string);     break;	 
      case CACHEPOS:  cmdCachepos(string);   break;	 
      case POWERUP:   cmdPowerup(NULL);      break;	 
      case POWERDOWN: cmdPowerdown(NULL);    break;	 
      case GOCACHE:   cmdGocache(NULL);      break;	 
      case EMSTOP:    cmdEmstop(NULL);       break;
	 
      case UNKNOWN:   printf("unrecognized command.\n");      break;
    
    }
    
    return 0;

}





/*********************************
  print command help
*********************************/

Status_t      cmdHelp     ( char *s ) {

printf( "\
help                         print help (your looking at it)    \n\
quit                         exit                               \n\
listen   servonumber          \n\
ignore   servonumber           \n\
setpos   position             set listening servos position \n\
cachepos position     \n\
powerup  \n\
powerdown \n\
gocache  \n\
");

 return OK;

}


Status_t cmdListen (char * s) {

  char * temp = s;

  if ((s==NULL) || (strlen(s) == 0)) {
    printf("Need address of servo to set listen\n");
    return BadArg;
  } else {   
     do {
       Listen(&servoBus,getNumber(temp, &temp));
     } while(*temp != NULL);
     return OK;
  }
}

  
Status_t cmdIgnore (char * s) {
  char * temp = s;
  if ((s==NULL) || (strlen(s) == 0)) {
    printf("Need address of servo to clear listen\n");
    return BadArg;
  } else {   
     do {
       Ignore(&servoBus, getNumber(temp, &temp));
     } while (*temp != NULL);
     return OK;
  }
}

Status_t cmdSetpos(char * s) {
  if ((s==NULL) || (strlen(s) == 0)) {
    printf("Need position \n");
    return BadArg;
  } else {   
     SetServoPostion(&servoBus, getNumber(s, NULL) );
     return OK;
  }
}

Status_t cmdCachepos (char * s) {
  if ((s==NULL) || (strlen(s) == 0)) {
    printf("Need position\n");
    return BadArg;
  } else {   
     SetCachedPosition(&servoBus, getNumber(s, NULL) );
     return OK;
  }
  return OK;
}

Status_t cmdPowerup (char * s) {
  SetFlags(&servoBus, 1<<SERVOON );
  return OK;
}

Status_t cmdPowerdown (char * s) {
  SetFlags(&servoBus, 1<<SERVOOFF   );
  return OK;
}

Status_t cmdGocache (char * s) {
  SetFlags(&servoBus, 1<<ENGCACHE  );
  return OK;
}

Status_t cmdEmstop ( char * s) {
  Listen(  &servoBus, 256);
  SetFlags(&servoBus, 1<<SERVOOFF );
  Ignore(  &servoBus, 256);
  return OK;
}