/* Yes this is GPLvX and written by Stefan de Konink */

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

void calibrate(int fd, fd_set *lbesb, char axis) {
	unsigned short int finished = 0;

	write(fd, (axis == 'C' ? "C":"Z"), 2);

	printf("Calibration initialised\n");

	while(select(fd+1, lbesb, NULL, NULL, NULL)) {
		char buf[128];
		unsigned int nbytes = read(fd, &buf, 127);
		if (nbytes < 0) {
			perror("\nread");
		} else {
			unsigned int i;
			
			if (nbytes > 0 && buf[0] == '$') {
				write(fd, (axis == 'C' ? "C":"Z"), 2);
			}

			for (i = 0; i < nbytes; i++) {
				if  (buf[i] == '.')
					finished++;
				else
					finished = 0;
#if debug
				putchar(buf[i]);
#endif

			}

			if (finished > 10) break;
#if debug
			putchar('\n');
#endif
		}
	}

	write(fd, "     ", 6);
	printf("Calibration finished\n");
}

int main() {
	fd_set lbesb;
	struct termios ttyset;
	int fd;

	if (getenv("QUERY_STRING") != NULL) {
		printf("Content-type: text/plain\n\n");
	}
	
	fd = open("/dev/ttyUSB0", O_RDWR | O_NONBLOCK);

	if (fd < 0) {
		perror("Open:");
		printf("Opening the compass failed\n");
		exit(EXIT_FAILURE);
	}

#if debug
	fprintf(stderr, "Device opened\n");
#endif
	
	tcgetattr(fd,&ttyset);

	ttyset.c_cc[VMIN] = 1;

	cfsetspeed(&ttyset, B19200);

	ttyset.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL | IXON);
	ttyset.c_oflag &= ~OPOST;
	ttyset.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN);

	ttyset.c_cflag &= ~(PARENB | PARODD | CRTSCTS | CSIZE);
	ttyset.c_cflag |= CS8;

	tcsetattr(fd, TCSANOW, &ttyset);

#if debug
	fprintf(stderr, "TTY settings changed\n");
#endif

	FD_ZERO(&lbesb);
	FD_SET(fd, &lbesb);

	printf("Welcome at the calibration tool, we will calibrate the compass by rotating the camera slowly around its Z axis. It must be leveled on a table or any other spirit leveled construction. Make sure the compass is leveled, otherwise the results may vary! (Starting in 10s)\n");

	sleep(10);

	calibrate(fd, &lbesb, 'C'); /* XY */

	printf("You have calibrated the XY plane of the compass, now turn over the camera on its side, to calibrate the Z-axis. Again rotate the camera. (Starting in 10s)\n");

	sleep(10);

	calibrate(fd, &lbesb, 'Z'); /* YZ */

	printf("That was all, if you still have problems, just run this tool again.");

	exit(EXIT_SUCCESS);
}