#include "cmd.h"#include "tty.h"#include "ctrl.h"#include <stdlib.h>#include <sys/poll.h>#include <signal.h>#define V 10#define DIS 3#define ANG 10int fd;struct termios tio;char buf[20];unsigned char rbuf[20];char bbuf[4] = {HEAD2, 0, 0, AA_ONLY};int bbsn;inline void cleanup() {	buf[0] = STOP;	write(fd, buf, 1);	tcsetattr(0, TCSANOW, &tio);	printf("\nclean up\n");}void msend(void *buf, int n) {	int t;	if ((t = write(fd, buf, n)) != n) {		if (t == -1)			perror("write");		else			fprintf(stderr, "write error %d\n", t);		exit(-1);	}}int main() {	unsigned char sn, bsn;	int res_n;	// init ttys	if ((fd = ttys_init(0)) < 0) {		return -1;	}	// init ctrl	if (ctrl_init(fd) < 0) {		return -1;	}	// init tty (key control)	tty_set(&tio);	// clean up at exit	if (atexit(cleanup)) {		fprintf(stderr, "atexit error\n");		return -1;	}	// clean up at ^c exit	if (signal(SIGINT, exit) == SIG_ERR) {		fprintf(stderr, "signal error\n");		return -1;	}	struct pollfd ufds[2] = {{0, POLLIN}, {fd, POLLIN}};	sn = bsn = 1;	res_n = 0;	for (;;) {		if (poll(ufds, 2, -1) <= 0) {			fprintf(stderr, "poll error\n");			exit(-1);		}		if (ufds[1].revents & POLLIN) {			int n;			if ((n = read(fd, rbuf + res_n, sizeof(rbuf) - res_n)) == -1) {				perror("read");				exit(-1);			}			n += res_n;			res_n = 0;			int i = 1;			for (;;) {				int type = rbuf[i] & 0xc0;				int k = rbuf[i] & 0x3f;				if (k > 7) {					fprintf(stderr, "echo len too long\n");					print_buf(rbuf, n);					exit(-1);				}				if (i + k >= n + 2) {	// current len not enough					printf("notice: complex \n");					print_buf(rbuf, n);					res_n = n + 1 - i;					copy_buf(rbuf, rbuf+i-1, res_n);					printf("move trailing %d bytes\n", res_n);					print_buf(rbuf, res_n);					break;				}				switch (k) {					case 2:						if (type != 0) {							fprintf(stderr, "echo error 2\n");							print_buf(rbuf, n);							exit(-1);						}						printf("%x done\n", rbuf[i-1]);						// stop on demand						if (bbsn == bsn) {							msend(bbuf, 4);							++sn;							printf("stop on done\n");						}						break;					case 3:						if (rbuf[i-1] != sn) {							fprintf(stderr, "echo error 3\n");							print_buf(rbuf, n);							exit(-1);						}						break;					case 5:						printf("IR: %x\n", sn);						//printf("IR: %d %d %d %d\n", rbuf[i+1], rbuf[i+2], rbuf[i+3], rbuf[i+4]);						break;					case 6:						if (!(type == 0 || type == M_RD)) {							fprintf(stderr, "echo error 6\n");							print_buf(rbuf, n);							exit(-1);						} 						if (type == M_RD) {							printf("%x RD\n", rbuf[i-1]);							//printf("RD: %d %d %d %d\n", rbuf[i+1], rbuf[i+2], rbuf[i+3], rbuf[i+4]);						}						else {							bsn = rbuf[i-1];							printf("%x begin clean\n", bsn);							//printf("clear: %d %d %d %d\n", rbuf[i+1], rbuf[i+2], rbuf[i+3], rbuf[i+4]);						}						break;					case 7:						if (type != 0) {							fprintf(stderr, "echo error 7\n");							print_buf(rbuf, n);							exit(-1);						}						bsn = rbuf[i-1];						printf("%x begin dirty(%x)\n", bsn, rbuf[i+1]);						//printf("dirty: %d %d %d %d\n", rbuf[i+2], rbuf[i+3], rbuf[i+4], rbuf[i+5]);						break;					default:						printf("what is this?\n");						print_buf(rbuf, n);						exit(-1);				}								i += k;				if (i > n) break;				else if (i == n) {	// trailing 1					res_n = 1;					rbuf[0] = rbuf[n-1];					break;				}			}		}		if (ufds[0].revents & POLLIN) {			char c;			if (read(0, &c, 1) != 1) {				fprintf(stderr, "read error\n");				exit(-1);			}							switch (c) {			case 4:			case 'q':				exit(0);			case 'p':			case ' ':				printf("stop\n");				buf[0] = STOP;				msend(buf, 1);				break;			case 'k':				buf[0] = HEAD3;				buf[1] = V;				buf[2] = V;				buf[3] = DIS;				buf[4] = AA_DISTANCE;				msend(buf, 5);				++sn;				printf("k %x\n", sn);				bbsn = sn;				break;			case 'j':				buf[0] = HEAD3;				buf[1] = -V;				buf[2] = -V;				buf[3] = -DIS;				buf[4] = AA_DISTANCE;				msend(buf, 5);				++sn;				printf("j %x\n", sn);				bbsn = sn;				break;			case 'h':				buf[0] = HEAD3;				buf[1] = -V;				buf[2] = V;				buf[3] = -ANG;				buf[4] = AA_ANGLE;				msend(buf, 5);				++sn;				printf("h %x\n", sn);				bbsn = sn;				break;			case 'l':				buf[0] = HEAD3;				buf[1] = V;				buf[2] = -V;				buf[3] = ANG;				buf[4] = AA_ANGLE;				msend(buf, 5);				++sn;				printf("l %x\n", sn);				bbsn = sn;				break;			case 'i':				buf[0] = IR;				msend(buf, 1);				++sn;				printf("i %x\n", sn);				break;			default:				printf("key undefined: %c\n", c);			}		}	}	return 0;}