#include <stdio.h>
#include <errno.h>
#include <stdlib.h>
#include <unistd.h>
#include <sys/fcntl.h>
#include <sys/types.h>
#include <termios.h>

static int tty_fd = -1;

static void
*tty_arm_probe(s)
char *s;
{
    struct termios t;

    if (s == 0 || strncmp("/dev/", s, 5) != 0)
    {
        fprintf(stderr, "'%s' should start with '/dev/'\n", s);
        return 0;
    }

    tty_fd = open(s, O_RDWR|O_EXCL|O_NOCTTY);

    if (tty_fd == -1)
    {
        fprintf(stderr, "Couldn't open '%s'\n", s);
        return 0;
    }

    memset(&t, 0, sizeof(t));
    memset(&t, 0, sizeof(t));
    t.c_iflag = 0;
    t.c_oflag = 0;
    t.c_cflag = B115200 | CS8 | CLOCAL | CREAD;
    t.c_lflag = 0;
    /* Read one character, must arrive within 100 ms */
    t.c_cc[VMIN] = 0;
    t.c_cc[VTIME] = 1;
    if (tcsetattr(tty_fd, TCSANOW, &t) == -1) {
        fprintf(stderr, "Couldn't set attributes of '%s'\n", s);
        close(tty_fd);
        return 0;
    }
    return &tty_fd;
}

unsigned char HEX_2_INT_TABLE[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 
            0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 
            0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 2, 3, 4, 5, 
            6, 7, 8, 9, 0, 0, 0, 0, 0, 0, 0, 10, 11, 12, 13, 14, 15, 0, 0, 
            0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 
            0, 0, 0, 10, 11, 12, 13, 14, 15, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 
            0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
            
int hexstr2int(char *hexstr) {
    register unsigned int length, i, value, shift;
    for (length = 0; length < 9; length++) if (!hexstr[length]) break;
    shift = (length - 1) * 4;
    for (i = value = 0; i < length; i++, shift -= 4)
        value += HEX_2_INT_TABLE[(unsigned int)hexstr[i] & 127] << shift;
    return value;
}

int main() {
  FILE * fileptr = fopen("write.txt","rb");
  if(fileptr == NULL) {fprintf(stderr,"could not open file"); exit(1);}
  
  unsigned char byte1[1+1];
  unsigned char byte2[2+1];
  unsigned char dummy;
  unsigned char b1,b2,b3, return1,cmd[5];

  while(!feof(fileptr)) {

	  fread(byte1,1,1,fileptr); byte1[1] = '\0';
	  fread(&dummy,1,1,fileptr);
	  fread(byte2,1,2,fileptr); byte2[2] = '\0';
	  fread(&dummy,1,1,fileptr);
    
	  b1 = (char) hexstr2int(byte1); b2 = (char) hexstr2int(byte2); b3 = 0;
	  return1 = '\r';
	   	  
	  printf("byte1: %x, byte2: %x\n",b1,b2);

	  fflush(stdin);
          scanf("%c",&dummy);

	if(b1 == 6){
	  cmd[0] = '6';}
	else if(b1 == 3)
	  cmd[0] = '3';
	else cmd[0] = '4';	 
	  cmd[1] = return1; // format?
	  cmd[2] = b3; 

	  tty_arm_probe("/dev/ttyS0");
	  write(tty_fd, cmd, 2);
  }	

  fclose(fileptr);
/*

  unsigned char b1,b2,b3, cmd[3];

  b1 = '6';
  b2 = '\r';
  b3 = 0;

  cmd[0] = b1;
  cmd[1] = b2;
  cmd[2] = b3;

  tty_arm_probe("/dev/ttyS0");
  write(tty_fd, cmd, 2);
*/
}