/*  botlink.c   9-11-1999  by Dale Heatherington */

/* To compile: gcc -o botlink botlink.c -lslang */

#include <stdio.h>
#include <termio.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <sys/time.h>
#include <slang/slang.h>

#define BAUDRATE B2400

#define bit08 unsigned char
#define bit16 unsigned short int
#define SYNC0  0xff
#define SYNC1  0xf0
#define SYNC2  0x55


#define MODE_STOP 0
#define MODE_FINDLINE 1
#define MODE_CENTER 2
#define MODE_ROTATE_L 3
#define MODE_ROTATE_R 4
#define MODE_TRACKLINE_RAMP_UP 5
#define MODE_TRACKLINE 6
#define MODE_FREE_ROTATE_R 0x24
#define MODE_BACK_OUT 0x25
#define MODE_ROTATE_BACK_OUT 0x26
#define MODE_BACKUP_AND_TURN_AROUND 0x27
#define MODE_BACKUP_TO_LINE 0x28
#define MODE_BACK_OUT_RIGHT 0x29
#define MODE_BACK_OUT_LEFT 0x2a


static bit08 get_byte();
static void get_bytes(bit08 *, bit16);
static bit16 get_word();
static void get_data(bit16 *, bit16);

static void send_byte(bit08);
static void send_word(bit16);
static void send_data(bit16 *, bit16);

static void timed_read(bit08 *, bit16);
static int open_tty(char *name) ;


int tty;

struct packet {
   bit08 address;
   bit08 count;
   bit08 data[31];
};

union raw_packet {
   struct   packet pkt;
   bit08    raw[32];
};

union raw_packet  rxdata,txdata;




/*---------------------------------------------------------------------*/
print_hex(bit08 *cp, int size)
{  int i;
   
   for(i=0;i<size;i++){
      SLsmg_printf("%02X ",cp[i]);
   }
   SLsmg_printf("\n");
}


fprint_hex(FILE* f, bit08 *cp, int size){
   int i;
   for(i=0;i<size;i++){
      fprintf(f,"%02X ",cp[i]);
   }
   fprintf(f,"\n");
}



print_hex_word(bit16 *wp, int size){
   int i;
  
  for(i=0;i<size;i++){
     SLsmg_printf("%04X ",wp[i]);
  }
  SLsmg_printf("\n");
}
   

/*---------------------------------------------------------------------*/

static int 
open_tty(char *name)
{
   struct termios termios;


   tty = open(name, O_RDWR, 0777);
   if (tty == -1) {
      perror("open of com port failed");

      exit(1);
   }

   if (tcgetattr(tty, &termios) == -1) {
      perror("getting attributes of programmer port failed");
      exit(1);
   }
   termios.c_iflag = IGNBRK | IGNPAR;
   termios.c_oflag = 0;
   termios.c_cflag = BAUDRATE | CS8 | CREAD | CLOCAL;
   termios.c_lflag = 0;
   termios.c_cc[VMIN] = 0; /* Timed read */
   termios.c_cc[VTIME] = 1; /* 0.1 second timeout  */
   if (tcsetattr(tty, TCSANOW, &termios) == -1) {
      perror("setting attributes of com port failed");
      exit(1);
   }
}

/*----------------------------------------*/
static void
send_byte(bit08 byte)
{
   int modem_bits = 0;
   bit08 bbuf[1];

   bbuf[0] = byte;
   write(tty, bbuf, 1);
   //log_write(byte);
   ioctl(tty, TCSBRK, 1); /* Wait until the characters have been output */
   //usleep(WAIT);          /* wait a fixed time - needs this for some reason */

}

/*-----------------------------------------*/
static bit08
get_byte()
{
   bit08 buf[1];

   timed_read(buf, 1);
   return buf[0];
}

/*---------------------------------------------*/

static void
get_bytes(bit08 *buffer, bit16 length)
{
   timed_read(buffer, length);

}

/*---------------------------------------------------*/


static void
get_data(bit16 *buffer, bit16 length)
{ unsigned int i,j,k;
   timed_read((bit08 *)buffer, length * 2);

   for (i=0;i<length;i++) {
      buffer[i] = ntohs(buffer[i]);  //Exchange hi and lo bytes for Intel cpu byte order
   }
}

/*----------------------------------------------------*/
static void
timed_read(bit08 *buffer, bit16 length)
{
   int total = length;
   int count;
   int i;

   while (length) {
      count = read(tty, (char *)buffer, length);
      if (count <= 0) {
         fprintf(stderr,
                 "read failed: wanted %d got %d error %d\n",
                 total, total - length, count);
         exit(3);
      }
      for (i=0; i < count; i++) {

         //log_read(buffer[i]);
      }


      length -= count;
      buffer += count;
   }
}
/*-------------------------------------------------------*/
void crc(bit08 data, bit16 *crc16)

#define MSB 0x8000
#define  MASK 0x8005
{  int k;
   bit16 c,d,t ;
      
      c = data << 8 ;
      d = c;
      for (k=0;k<8;k++) {
         *crc16 = (c & MSB) ^ *crc16;

         if (*crc16 & MSB) {
            *crc16 = *crc16 << 1;
            *crc16 = *crc16 ^ MASK;
         } else 
            *crc16 = *crc16 << 1;
         d = d << 1;
         c = d;
      }
  
}

/*-------------------------------------------------------*/
void
send_pkt(union raw_packet *data)
{  int i,j;
   int pktsize;
   bit08 c,c1,c2;
   bit16 crc16 = 0;

   c = SYNC0;
   for(i=0;i<8;i++) write(tty,&c,1); //leading rubouts

   c = SYNC1;
   write(tty,&c,1);         //Send sync 1
   c = SYNC2;
   write(tty,&c,1);         //Send sync 2

   j = data->pkt.count +2;  //Add two for count and address 
   
   for (i=0;i<j;i++) {      //Send address,count and data
      crc(data->raw[i],&crc16);
      write(tty,&data->raw[i],1);
   }

   c1 = crc16 >> 8;         //send crc H
   write(tty,&c1,1);
   c2 = crc16 & 0xff;       //send crc L
   write(tty,&c2,1);

      //debug
   /*
   pktsize = data->pkt.count + 1;
   data->raw[pktsize++] = c1;
   data->raw[pktsize++] = c2;
   SLsmg_gotorc(0,0);
   print_hex(data->raw,pktsize);
   crc16 = 0;
   for(i=0;i<pktsize;i++){
      crc(data->raw[i],&crc16);
      
   }

   SLsmg_gotorc(1,0);
   SLsmg_write_string("CRC= ");
   print_hex_word(&crc16,1);
   //sleep(10);
   */

}

/*------------------------------------------------------*/
int
recv_pkt(union raw_packet *data)
{  int i,j,count,pktsize;
   bit08 sync,a;
   bit16 crc16 = 0;
   static int x = 0;

   count = 1;
   while((count == 1) && (sync != SYNC1)){
   count = read(tty,&sync, 1);
     }
   if((count <= 0) || (sync != SYNC1)) return 1;

     count = read(tty,&sync,1);

     if((count <= 0) || (sync != SYNC2)) return 1;
   count = read(tty,&data->pkt.address,2); // get address and count
  
   if((count <= 0) || (data->pkt.count > 30)) return 2;
   pktsize = data->pkt.count + 2; //add 2 for crc bytes
   i = 2;
   j = pktsize + 2;
   while((count > 0) && (i < j)){
      count = read(tty,&data->raw[i],1);
      i++;

   }

     if(i != j) return 3;
   
   for(i=0;i<j;i++){
      crc(data->raw[i],&crc16);
       }
  
   if(crc16 != 0) return 4;

   return 0;
}  

/*------------------------------------------------------*/
void
print_info(union raw_packet *pkt, int row, int col)
{
   char left_speed,right_speed,track,rmfb,lmfb;
   char line_find,speed;
   unsigned char battery,bot_mode,left_detect,right_detect,timer;
   unsigned char portb, c, portc, line_cor;
   float battery_voltage;
     
   char *mode;
   char *mode_idle = "STOPPED";
   char *mode_find = "Hunting for Line";
   char *mode_center = "Centering on line";
   char *mode_right = "Rotating Right";
   char *mode_left = "Rotating Left";
   char *mode_found    = "Line Found, begin tracking";
   char *mode_tracking = "Tracking Line at full speed";

   left_speed = pkt->pkt.data[1];
   right_speed = pkt->pkt.data[2];
   track = pkt->pkt.data[3];
   line_find = pkt->pkt.data[4];
   right_detect = pkt->pkt.data[5];
   left_detect = pkt->pkt.data[6];
   battery = pkt->pkt.data[7];
   speed = pkt->pkt.data[8];
   bot_mode = pkt->pkt.data[9];
   lmfb = pkt->pkt.data[10];
   rmfb = pkt->pkt.data[11];
   timer = pkt->pkt.data[12];
   portb = pkt->pkt.data[13];
   portc = pkt->pkt.data[14];
   line_cor = pkt->pkt.data[15];

   battery_voltage = ((float)battery/256) * 15;

   SLsmg_gotorc(row++,col);

   SLsmg_printf("LEFT: %4d  RIGHT:%4d  LINE TRACK:%4d",lmfb,
                rmfb,track);
   SLsmg_gotorc(row++,col);
   SLsmg_printf("SPEED:%4d  L-DET:%4d  R-DET:%4d  BATTERY:%4.1f",
                 speed,left_detect,right_detect,battery_voltage);
   SLsmg_gotorc(row++,col);
   SLsmg_printf("LINE FIND:%4d  MODE:%4d  TIMER:%4d",line_find, bot_mode,timer);

   SLsmg_gotorc(row++,col);
   SLsmg_printf("IR sensor: ");
   c = ~portb & 6;
   switch(c){
      case 6:  SLsmg_printf("CENTER");
               break;
      case 4:  SLsmg_printf("RIGHT  ");
               break;
      case 2:  SLsmg_printf("LEFT   ");
               break;
   default:    SLsmg_printf("       ");
   }

   SLsmg_gotorc(row++,col);
   SLsmg_printf("Line correlation value: %4d ",line_cor);
   
      

   SLsmg_gotorc(row+2,col);
   switch(bot_mode){
   case  0: mode = mode_idle;
            break;
   case 1:  mode = mode_find;
            break;
   case 2:  mode = mode_center;
            break;
   case 3:  mode = mode_left;
            break;
   case 4:  mode = mode_right;
            break;
   case 5:  mode = mode_found;
            break;
   case 6:  mode = mode_tracking;
            break;
   case MODE_BACK_OUT:
            mode = "Backing out";
            break;
   case MODE_ROTATE_BACK_OUT:
            mode = "Rotating 180";
            break;
   case MODE_FREE_ROTATE_R:
            mode = "Rotating Right to avoid object";
            break;
   case MODE_BACKUP_TO_LINE:
            mode = "Backup to line";
            break;
   case MODE_BACKUP_AND_TURN_AROUND:
            mode = "Backup and turn around";
            break;
   case MODE_BACK_OUT_RIGHT:
            mode = "Backup and turn right";
            break;
   case MODE_BACK_OUT_LEFT:
            mode = "Backup and turn left";
            break;
          
   default: mode = "Mode Unknown!";
            break;
   }

   SLsmg_printf("                                     "); //erase line
   SLsmg_gotorc(row+2,col);                                 //print bot mode
   SLsmg_printf("%s",mode);

}

/*------------------------------------------------------*/
void looptest()
{
   int rc,i;
   bit08 type;
   char ch;
   FILE* f;
   strcpy(txdata.pkt.data,"Data test");
   txdata.pkt.count = strlen(txdata.pkt.data);
   txdata.pkt.address = 1;
   send_pkt(&txdata);

   

   f = fopen("botdata.log", "w");

     if (f == NULL) {
        fprintf(stderr,"failed...cannot create file botdata.log\n");
        exit(1);
     }

   i = 1;
   while(1 ==1){
      if((rc = recv_pkt(&rxdata)) == 0) {
         SLsmg_gotorc(2,0);
         print_hex(rxdata.raw,rxdata.pkt.count+2); //Print raw packet in hex
         fprint_hex(f,rxdata.raw,rxdata.pkt.count+2); //Print raw packet in hex to file
         type = rxdata.pkt.data[0];
         if(type == 1){
            
            print_info(&rxdata,4,0);
         }
         SLsmg_gotorc(0,0);
         SLsmg_refresh();
      }

      if(SLang_input_pending(0)){
         ch = SLang_getkey();
         if(ch == 'q') {
            fclose(f);
            return;
         }
      }

   }
         

}

/*------------------------------------------------------*/

int main(int argc, char *argv[])
{  char ch = '\0';
   bit08 b;


   open_tty("/dev/ttyS0");
   

   SLang_init_tty(-1,0,1);
   SLtt_get_terminfo();
   SLsmg_init_smg();

   SLsmg_gotorc(0,20);
   //SLtt_set_color(0,NULL,"white","blue");
   SLsmg_write_string("Robot Data Link");
   
   
   SLsmg_refresh();
   looptest();

   /*
   while (ch != 'q') {
      ch = SLang_getkey();
      printf("read: %c 0x%x\n", isprint(ch) ? ch : ' ', ch);
   }
    */
   SLsmg_reset_smg();
   SLang_reset_tty();


   exit(0);
}