/*
 * cngt.c - Tool change utility for MDX-15/20
 *
 * Written 2010 by Werner Almesberger
 * Copyright 2010 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 "serial.h"
#include "getkey.h"


#define	MAX_POS	10
#define	STEP	5	/* xy movement step, 5 mm */


static double pos_x[MAX_POS];
static double pos_y[MAX_POS];
static double z0, height;
static double x, y, z;


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


static void move(void)
{
	serial_printf("!PZ%.1f,0;PD%.1f,%.1f\n", UNITS(z), UNITS(x), UNITS(y));
}


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


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


static int do_key(char c)
{
	switch (c) {
	case 'u':
		up();
		break;
	case 'd':
		down();
		break;
	case 'h':
		up();
		x -= STEP;
		move();
		break;
	case 'j':
		up();
		y -= STEP;
		move();
		break;
	case 'k':
		up();
		y += STEP;
		move();
		break;
	case 'l':
		up();
		x += STEP;
		move();
		break;
	case 'q':
		return 1;
	default:
		if (c < '0' || c > '9')
			break;
		up();
		x = pos_x[c-'0'];
		y = pos_y[c-'0'];
		move();
	}
	return 0;
}


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


int main(int argc, char **argv)
{
	int i;
	char c;

	if (argc < 4)
		usage(*argv);
	if (!(argc & 1))
		usage(*argv);

	z0 = atof(argv[1]);
	height = atof(argv[2]);
	for (i = 3; i != argc; i += 2) {
		pos_x[(i-3)/2] = atof(argv[i]);
		pos_y[(i-3)/2] = atof(argv[i+1]);
	}

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

	x = pos_x[0];
	y = pos_y[0];
	z = z0+height;
	move();

	while ((c = getkey()))
		if (do_key(c))
			break;
	up();
	serial_close();
	return 0;
}