/*
SQ,16
ST,255
*/

#include <stdio.h>
#include <termios.h>
#include <fcntl.h>
#include <string.h>
#include <errno.h>
#include <stdlib.h>
#include <sys/ioctl.h>


ssize_t write_or_exit(int fd, const unsigned char *buf) {
	ssize_t ret = write(fd, buf, strlen(buf));
	if (ret != strlen(buf)) {
		printf("write failed: %s\n", strerror(errno));
		exit(-1);
	}
	return ret;
}


print_string(const unsigned char *buf, int len) {
	unsigned int i;
	printf("'");
	for(i=0; i<len; i++) {
		unsigned char c=buf[i];
		if (c >= 32 && c <= 127) {
		  printf("%c", c);
		} else {
		  printf("<%02X>", c);
		}
	}
	printf("'");
}


ssize_t read_or_exit(int fd, unsigned char *buf, const char *buf_compare) {
	printf("expecting '%s' ", buf_compare);
	fflush(stdout);
  ssize_t ret = read(fd, buf, strlen(buf_compare));
  if (ret != strlen(buf_compare)) {
		printf("read failed: %s ret=%i\n", strerror(errno), ret);
		printf("got: ");
		print_string(buf, strlen(buf_compare));
		exit(-1);
  }
  if(strncmp(buf, buf_compare, strlen(buf_compare))!=0) {
		printf("got: ");
		print_string(buf, strlen(buf_compare));
		exit(-1);
  }
	printf("\n");
	return ret;
}


int main(int argc, char *argv[]) {
	static int fd = -1;
  struct termios portset;
  unsigned char in[255];
  int hs=0;

	printf("opening port...\n", strerror(errno));
  fd = open(argv[1], O_RDWR | O_NOCTTY | O_NDELAY);
  if (fd == -1) {
		printf("open failed: %s\n", strerror(errno));
		return -1;
  }

// S~,3
//	int status;
//	if (ioctl(fd, TIOCMGET, &status) == -1) { /* get the serial port status */
//		printf("TIOCMGET failed: %s\n", strerror(errno));
//		return -1;
//  }
//	status |= TIOCM_DTR;
//	if (ioctl(fd, TIOCMSET, &status) == -1) { /* set the serial port status */
//		printf("TIOCMSET failed: %s\n", strerror(errno));
//		return -1;
//  }

  if (tcgetattr(fd, &portset) == -1) {
		printf("tcgetattr failed: %s\n", strerror(errno));
		return -1;
  }

  cfmakeraw(&portset);
//  portset.c_cflag &= ~HUPCL;        /* clear the HUPCL bit */
  if (tcsetattr(fd, TCSANOW, &portset) == -1) {
		printf("tcsetattr failed: %s\n", strerror(errno));
		return -1;
  }

  /* blocking read */
  if (fcntl(fd, F_SETFL, 0) == -1) {
		printf("fcntl failed: %s\n", strerror(errno));
		return -1;
  }

	write_or_exit(fd, "$$$");
  read_or_exit(fd, in, "CMD\x0D\x0A");
	if(strcmp(argv[2],"init")==0) {
// S~,0
		write_or_exit(fd, "S*,0400\x0D\x0A");
	  read_or_exit(fd, in, "AOK\x0D\x0A");
	  usleep(2);
		write_or_exit(fd, "S*,0404\x0D\x0A");
	  read_or_exit(fd, in, "AOK\x0D\x0A");
		printf("waiting for 4D...\n");
		usleep(500*1000);

		write_or_exit(fd, "U,9600,N\x0D\x0A");
	  read_or_exit(fd, in, "AOK\x0D\x0A");

		write_or_exit(fd, "U");
	  read_or_exit(fd, in, "\x06");
	  if(hs) write_or_exit(fd, "Q\x0F"); // SZ,1104
	  else write_or_exit(fd, "Q\x0D"); // SU,11

		printf("waiting for RN41...\n");
		sleep(2);
		write_or_exit(fd, "$$$");
	  read_or_exit(fd, in, "CMD\x0D\x0A");

		if(hs) {
			write_or_exit(fd, "R,1\x0D\x0A");
		  read_or_exit(fd, in, "Reboot!\x0D\x0A");
		} else {
			write_or_exit(fd, "U,115K,N\x0D\x0A");
		  read_or_exit(fd, in, "AOK\x0D\x0A");
		}

//write_or_exit(fd, "\x54\x41\x01\x01\xFF\xFF");
//read_or_exit(fd, in, "\x06");
	} else {
		write_or_exit(fd, "F,1\x0D\x0A");
	  read_or_exit(fd, in, "END\x0D\x0A");
	}
}