/*
 * cngt.c - Tool change utility for MDX-15/20
 *
 * Written 2010-2011 by Werner Almesberger
 * Copyright 2010-2011 Werner Almesberger
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 */


#include <stdlib.h>
#include <stdio.h>
#include <ctype.h>

#include "serial.h"
#include "getkey.h"


#define	MAX_POS	10
#define	STEP_XY		5	/* xy movement step, 5 mm */
#define	STEP_FINE_XY	0.5	/* xy movement fine step, 0.5 mm */
#define	STEP_Z		1	/* z movement step, 1 mm */
#define	STEP_FINE_Z	0.1	/* z movement step, 0.1 mm */


static double pos_x[MAX_POS];
static double pos_y[MAX_POS];
static double z0, height;
static double cx, cy, cz;
static int positioning = 0;


#define UNITS(mm)	((mm)*40.0)


static void move(void)
{
	serial_printf("!PZ%.1f,0;PD%.1f,%.1f\n",
	    UNITS(cz), UNITS(cx), UNITS(cy));
}


static void up(void)
{
	cz = z0+height;
	move();
}


static void down(void)
{
	cz = z0;
	move();
}


static int do_key(char c)
{
	double step_xy = STEP_FINE_XY;
	double step_z = STEP_FINE_Z;

	if (isupper(c)) {
		step_xy = STEP_XY;
		step_z = STEP_Z;
	}

	/* Anything but x/y positioning */

	switch (c) {
	case 'U':
	case 'u':
		if (positioning) {
			cz += step_z;
			move();
		} else {
			up();
		}
		return 0;
	case 'D':
	case 'd':
		if (positioning) {
			cz -= step_z;
			move();
		} else {
			down();
		}
		return 0;
	case 'q':
		return 1;
	default:
		break;
	}

	/* Only x/y positioning */

	if (!positioning)
		up();

	switch (c) {
	case 'H':
	case 'h':
		cx -= step_xy;
		break;
	case 'J':
	case 'j':
		cy -= step_xy;
		break;
	case 'K':
	case 'k':
		cy += step_xy;
		break;
	case 'L':
	case 'l':
		cx += step_xy;
		break;
	default:
		if (positioning)
			return 0;
		if (c < '0' || c > '9')
			return 0;
		cx = pos_x[c-'0'];
		cy = pos_y[c-'0'];
	}

	move();

	return 0;
}


static void gp_minmax(const char *name,
    double *xa, double *ya, double *xb, double *yb)
{
	FILE *file;
	char buf[1024];
	double x, y, z;
	int n;
	int first = 1;

	file = fopen(name, "r");
	if (!file) {
		perror(name);
		exit(1);
	}
	while (fgets(buf, sizeof(buf), file)) {
		if (*buf == '#')
			continue;
		n = sscanf(buf, "%lf %lf %lf\n", &x, &y, &z);
		switch (n) {
		case 3:
			/* fall through */
		case 2:
			if (first || x < *xa)
				*xa = x;
			if (first || x > *xb)
				*xb = x;
			if (first || y < *ya)
				*ya = y;
			if (first || y > *yb)
				*yb = y;
			first = 0;
			break;
		default:
			break;
		}
	}
	fclose(file);
}


static void usage(const char *name)
{
	fprintf(stderr,
"usage: %s z0 height [file | x y ...]\n"
"       %s z0\n"
    , name, name);
	exit(1);
}


int main(int argc, char **argv)
{
	double xa = 0, ya = 0, xb = 0, yb = 0;
	int i;
	char c;

	if (argc < 2)
		usage(*argv);

	z0 = atof(argv[1]);

	if (argc == 2) {
		positioning = 1;
		height = 0;
	} else {
		height = atof(argv[2]);
		if (argc & 1) {
			for (i = 3; i != argc; i += 2) {
				pos_x[(i-3)/2] = atof(argv[i]);
				pos_y[(i-3)/2] = atof(argv[i+1]);
			}
		} else {
			if (argc != 4)
				usage(*argv);
			gp_minmax(argv[3], &xa, &ya, &xb, &yb);
			pos_x[1] = pos_x[4] = pos_x[7] = xa;
			pos_x[2] = pos_x[5] = pos_x[8] = (xa+xb)/2;
			pos_x[3] = pos_x[6] = pos_x[9] = xb;
			pos_y[1] = pos_y[2] = pos_y[3] = ya;
			pos_y[4] = pos_y[5] = pos_y[6] = (ya+yb)/2;
			pos_y[7] = pos_y[8] = pos_y[9] = yb;
		}
	}

	serial_open("/dev/ttyUSB0");
	serial_printf("\nIN;!MC0\n");

	cx = pos_x[0];
	cy = pos_y[0];
	cz = z0+height;
	move();

	while ((c = getkey())) {
		if (do_key(c))
			break;
		if (positioning)
			printf("%f %f %f\r\n", cx, cy, cz);
	}

	if (!positioning)
		up();

	serial_close();
	return 0;
}