#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <ctype.h>
#include <unistd.h>
#include <fcntl.h>
#include <sys/time.h>
#include <sys/types.h>
#include <sys/ioctl.h>
#include <termios.h>

#define ROBOT_DEV "/dev/ttyACM0"
#define ROBOT_BAUD B115200

/* G-code parameters for robot */
#define MOVERATE 5000
#define EXTRUDE_EXTRA 2.
#define EXTRUDE_RATE 1000

#define BUFSZ 4096

int robot_fd = -1;
int file_fd = -1;
char *file_name = NULL;

struct buffer_rec {
	char buf[BUFSZ];
	int rofs, wofs;
	int line_no;
};

struct buffer_rec robot_buf = { "" };
struct buffer_rec file_buf = { "" };
struct buffer_rec console_buf = { "" };
struct buffer_rec override_buf = { "" };

int ok_seen = 0;
int start_seen = 0;
int command_no = 0;	/* for "N" argument */
int resend = 0;
int paused = 0;
char last_cmd[BUFSZ] = "";

char saved_x[100];
char saved_y[100];
char saved_z[100];
char saved_e[100];
char saved_f[100];
double sum_e = 0.;

char resume_lines[BUFSZ];


void do_pause(void);


int
buf_fill(struct buffer_rec *b, int fd)
{
	int toread = 0;
	int n;
	if (b->rofs > b->wofs) {
		toread = (b->rofs - b->wofs) - 1;
	} else {
		toread = (BUFSZ - b->wofs) - ((b->rofs == 0) ? 1 : 0);
	}
	if (toread == 0) {
		return 0;
	}
	n = read(fd, &b->buf[b->wofs], toread);
	if (n <= 0) {
		/* EOF or error */
		return -1;
	}
	b->wofs += n;
	if (b->wofs == BUFSZ) {
		b->wofs = 0;
	}
	return n;
}

int
buf_peekline(struct buffer_rec *b)
{
	int i;
	for (i = b->rofs; i != b->wofs; i = (i+1) % BUFSZ) {
		char c = b->buf[i];
		if ((c == '\r') || (c == '\n')) {
			return 1;
		}
	}
	return 0;
}

/* NB - the returned buffer will be overwritten in a subsequent call */
char *
buf_getline(struct buffer_rec *b)
{
	static char ret[BUFSZ];
	int i;
	int ls = b->rofs;
	for (i = b->rofs; i != b->wofs; i = (i+1) % BUFSZ) {
		char c = b->buf[i];
		if ((c == '\r') || (c == '\n')) {
			if (ls > i) {
				int n = BUFSZ-ls;
				memcpy(ret, &b->buf[ls], n);
				memcpy(&ret[n], b->buf, i);
				ret[n+i] = 0;
			} else {
				memcpy(ret, &b->buf[ls], i-ls);
				ret[i-ls] = 0;
			}
			b->rofs = (i+1) % BUFSZ;
			if (ret[0]) {
				b->line_no++;
			}
			return ret;
		}
	}
	return NULL;
}

/* drop DTR for 1s, seems to restart arduino */
void
do_reset(void)
{
	unsigned long i;
	if (robot_fd < 0) {
		return;
	}
	i = TIOCM_DTR;
	if (ioctl(robot_fd, TIOCMBIC, &i)) {
		perror("ioctl TIOCMBIC(TIOCM_DTR)");
	}
	sleep(1);
	i = TIOCM_DTR;
	if (ioctl(robot_fd, TIOCMBIS, &i)) {
		perror("ioctl TIOCMBIC(TIOCM_DTR)");
	}
	start_seen = 0;
	command_no = 0;
	sum_e = 0.;
}

int
open_robot(char *fn)
{
	int fd;
	struct termios t;

	fd = open(fn, O_RDWR);
	if (fd < 0) {
		perror(fn);
		return -1;
	}
	robot_fd = fd;
		
	tcgetattr(fd, &t);

	cfsetispeed(&t, ROBOT_BAUD);
	cfsetospeed(&t, ROBOT_BAUD);
	t.c_cflag = (t.c_cflag & ~CSIZE) | CS8;	/* 8 bits */
	t.c_cflag &= ~(PARENB|PARODD);		/* parity "none" */
	t.c_cflag &= ~CSTOPB;			/* 1 stop bit */
	t.c_cflag |= CRTSCTS;			/* RTS/CTS flow control */
	t.c_iflag &= ~(IXON|IXOFF|IXANY);
	t.c_cflag |= CLOCAL;	/* don't SIGPIPE/whatev if remote hangs up */
	t.c_cflag |= CREAD;			/* "enable receiver" */
	t.c_cflag &= ~HUPCL;			/* don't hangup on close */

	/* "raw, no echo mode": */
	t.c_iflag = IGNBRK;
	t.c_lflag = 0;
	t.c_oflag = 0;
	t.c_cc[VMIN] = 1;
	t.c_cc[VTIME] = 5;

	tcsetattr(fd, TCSANOW, &t);

#if 0
	/* The perl version needs a reset at the start, but for some reason with
	 * C, it resets on its own, and no point in a double reset... */
	do_reset();
#else
	start_seen = 0;
	command_no = 0;
#endif

	return fd;
}

void
console_in(void)
{
	buf_fill(&console_buf, 0);
}

void
robot_in(void)
{
	int i;
	char *s, *t;
	buf_fill(&robot_buf, robot_fd);
	while ((s = buf_getline(&robot_buf))) {
		if (!*s) {
			continue;
		}
		printf("read:%s\n", s);
		for (i = 0; s[i]; i++) {
			if (isupper(s[i])) {
				s[i] = tolower(s[i]);
			}
		}
		if ((t=strstr(s, "last line: "))) {
			t+=11;
			command_no = strtoul(t, NULL, 10)+1;
		}

		if (start_seen && !strncmp(s, "ok", 2)) {
			ok_seen = 1;
		} else if (!start_seen && strstr(s, "start")) {
			start_seen = 1;
			ok_seen = 1;
		} else if (strstr(s, "resend:")) {
			resend = 1;
		} else if (strstr(s, "error")) {
			if (strstr(s,"checksum mismatch")) {
				/* expect they will send us a "resend" command,
				 * so don't bother pausing, assume the resend
				 * will take care of it */
			} else {
				do_pause();
			}
		}
	}
}

int
get_int(char *s)
{
	int ret = 0;
	int sign = 1;
	if (*s == '-') {
		sign = -1;
		s++;
	}
	while (isdigit(*s)) {
		ret = (ret*10) + (*s-'0');
		s++;
	}
	return ret*sign;
}

double
get_double(char *s)
{
	double ret = 0.;
	double sign = 1.;
	double div = 1.0;
	int after_dec = 0;
	if (*s == '-') {
		sign = -1.;
	}
	while (isdigit(*s) || (*s == '.')) {
		if (*s == '.') {
			after_dec = 1;
		} else {
			ret = (ret * 10.) + ((double)(*s-'0'));
			if (after_dec) {
				div *= 10.;
			}
		}
	}
	return sign*ret/div;
}

void
number_strcpy(char *dst, char *src)
{
	while ((*src == '.') || (*src == '-') || isdigit(*src)) {
		*dst++ = *src++;
	}
	*dst = 0;
}

void
save_xyze(char **args)
{
	args -= 'A';
	if (args['G']) {
		int gcode = get_int(args['G']);
		if ((gcode == 0) || (gcode == 1) || (gcode == 92)) {
			if (args['X']) {
				number_strcpy(saved_x, args['X']);
			}
			if (args['Y']) {
				number_strcpy(saved_y, args['Y']);
			}
			if (args['Z']) {
				number_strcpy(saved_z, args['Z']);
			}
			if (args['E']) {
				char new_e[100];

				number_strcpy(new_e, args['E']);

				if (gcode != 92) {
					double e0, e1;
					e0 = strtod(saved_e, NULL);
					e1 = strtod(new_e, NULL);
					sum_e += e1-e0;
				}

				strcpy(saved_e, new_e);
			}
			if (args['F']) {
				number_strcpy(saved_f, args['F']);
			}
		} else if (gcode == 28) {
			saved_x[0] = 0;
			saved_y[0] = 0;
			saved_z[0] = 0;
			saved_e[0] = 0;
			saved_f[0] = 0;
			sum_e = 0.;
		}
	}
}

void
do_pause(void)
{
	double z;
	int i = 0;

	if ((file_fd < 0) || paused) {
		return;
	}

	if (!saved_x[0] || !saved_y[0] || !saved_z[0]) {
		i += sprintf(resume_lines+i, "G28\n");
	}
	i += sprintf(resume_lines+i, "G0 F%d\n", MOVERATE);
	if (saved_x[0]) {
		i += sprintf(resume_lines+i, "G0 X%s\n", saved_x);
	}
	if (saved_y[0]) {
		i += sprintf(resume_lines+i, "G0 Y%s\n", saved_y);
	}
	if (saved_z[0]) {
		i += sprintf(resume_lines+i, "G0 Z%s\n", saved_z);
	}
	if (saved_e[0]) {
		if (EXTRUDE_EXTRA) {
			/* prime the pump (for bowden) */
			i += sprintf(resume_lines+i, "G92 E0\n");
			i += sprintf(resume_lines+i, "G0 E%.1f F%d\n",
					(float)EXTRUDE_EXTRA, EXTRUDE_RATE);
		}
		i += sprintf(resume_lines+i, "G92 E%s\n", saved_e);
	} else {
		i += sprintf(resume_lines+i, "G92 E0\n");
	}
	if (saved_f[0]) {
		i += sprintf(resume_lines+i, "G0 F%s\n", saved_f);
	}

	paused = 1;

	printf("paused printing.  full resume would be:\n");
	printf("--------------\n");
	printf("%s", resume_lines);
	printf("/load %s %d\n", file_name, file_buf.line_no);
	printf("--------------\n");

	if ((saved_z[0]) && ((z=strtod(saved_z, NULL)) < 200.)) {
		override_buf.rofs = 0;
		override_buf.wofs = sprintf(override_buf.buf,
				"G92 E0\nG0 F%d Z%.1f\n", MOVERATE, z+10.);
	} else {
		override_buf.rofs = 0;
		override_buf.wofs = sprintf(override_buf.buf,
				"G92 E0\nG0 F%d\n", MOVERATE);
	}
}

void
do_load(char *fn, int lineno)
{
	if (file_fd >= 0) {
		close(file_fd);
	}

	memset(&file_buf, 0, sizeof file_buf);
	paused = 0;

	file_fd = open(fn, O_RDONLY);

	if (file_fd >= 0) {
		file_name = strdup(fn);
		while (file_buf.line_no < lineno) {
			if (!buf_getline(&file_buf)) {
				buf_fill(&file_buf, file_fd);
			}
		}
	} else {
		file_name = NULL;
		perror(fn);
	}
}

void
do_slash(char *s)
{
	if (!strncmp(s, "help", 4)) {
		printf("/pause, /resume, /load <fn> [lineno], /reset, /quit\n");
	} else if (!strncmp(s, "pause", 5)) {
		do_pause();
	} else if (!strncmp(s, "resume", 6)) {
		strcpy(override_buf.buf, resume_lines);
		override_buf.rofs = 0;
		override_buf.wofs = strlen(override_buf.buf);
		paused = 0;
	} else if (!strncmp(s, "load", 4)) {
		char *fn;
		int lineno=0;
		s += 4;
		while (isspace(*s)) s++;
		fn = s;
		while (*s && !isspace(*s)) s++;
		if (*s) {
			*s = 0;
			s++;
			while (isspace(*s)) s++;
			if (isdigit(*s)) {
				lineno=get_int(s);
			}
		}

		do_load(fn, lineno);
	} else if (!strncmp(s, "reset", 5)) {
		do_reset();
	} else if (!strncmp(s, "quit", 4)) {
		if (robot_fd >= 0) close(robot_fd);
		if (file_fd >= 0) close(file_fd);
		exit(0);
	}
}

void
do_write(int fd, char *buf, int len)
{
	int i;
	int wrote = 0;
	while (wrote < len) {
		i = write(fd, buf+wrote, len-wrote);
		if (i < 0) {
			perror("write");
			return;
		}
		wrote += i;
	}
}

int
do_checksum(char *s)
{
	int ret = 0;
	while (*s) {
		ret ^= *s++;
	}
	return ret;
}

void
do_command(char *s)
{
	char buf[BUFSZ+100];
	char *args[26] = { NULL };
	int i;
	int in_parens = 0;

	if (*s == '/') {
		do_slash(s+1);
		return;
	}

	strcpy(last_cmd, s);

	i = 0;
	while (*s) {
		char c = *s++;
		if (c == '(') {
			in_parens = 1;
			continue;
		} else if (c == ')') {
			in_parens = 0;
			continue;
		} else if (in_parens) {
			continue;
		} else if (c == ';') {
			break;
		} else if (isspace(c)) {
			continue;
		} else if (islower(c)) {
			c = toupper(c);
		}
		buf[i++] = c;
		if (isupper(c)) {
			args[c-'A'] = &buf[i];
		}
	}
	if (!i) {
		/* empty commands just anger the robot */
		return;
	}
	if (!args['N'-'A']) {
		i += sprintf(&buf[i], "N%d", ++command_no);
	}
	i += sprintf(&buf[i], "*%d\n", do_checksum(buf));

	if (saved_z[0]) {
		printf("[Z=%s,E=%.1f] ", saved_z, sum_e);
	}
	printf("wrote:%s", buf);
	do_write(robot_fd, buf, i);

	ok_seen = 0;
	save_xyze(args);
}

int
nextline(void)
{
	char *s = NULL;
	if (resend) {
		s = last_cmd;
		command_no--;
		resend = 0;
	} else if ((s = buf_getline(&override_buf))) {
	} else if (buf_peekline(&console_buf)) {
		if (!paused && (file_fd >= 0)) {
			/* we are currently printing something */
			do_pause();
			if (!(s = buf_getline(&override_buf))) {
				/* pause may or may not add an override_buf */
				s = buf_getline(&console_buf);
			}
		} else {
			s = buf_getline(&console_buf);
		}
	} else if ((s = buf_getline(&console_buf))) {
		if (!*s && (file_fd >= 0) && !paused) {
			s = "/pause";
		}
	} else if (!paused && (file_fd >= 0)) {
		s = buf_getline(&file_buf);
		while (!s) {
			if (buf_fill(&file_buf, file_fd) < 0) {
				printf("EOF\n");
				close(file_fd);
				file_fd = -1;
				file_name = NULL;
				memset(&file_buf, 0, sizeof file_buf);
				break;
			}
			s = buf_getline(&file_buf);
		}
	}
	if (s) {
		do_command(s);
		return 1;
	}
	return 0;
}

int
main(int argc, char **argv)
{
	if (argc == 2) {
		do_load(argv[1], 0);
	} else if (argc > 2) {
		fprintf(stderr, "usage: %s [input.gcode]\n", argv[0]);
		return -1;
	}

	robot_fd = open_robot(ROBOT_DEV);
	if (robot_fd < 0) {
		return -1;
	}

	while (1) {
		fd_set ifd;
		int n;

		FD_ZERO(&ifd);
		FD_SET(0, &ifd);
		FD_SET(robot_fd, &ifd);
		n = select(robot_fd+1, &ifd, NULL, NULL, NULL);
		if (n < 0) {
			perror("select");
			sleep(1);	/* pray it resolves itself */
		}
		if (FD_ISSET(0, &ifd)) {
			console_in();
		}
		if (FD_ISSET(robot_fd, &ifd)) {
			robot_in();
		}
		while (ok_seen && nextline()) {
		}
	}
	return 0;
}