/*
 * ----------------------------------------------------------------------------
 * "THE BEER-WARE LICENSE" (Revision 42):
 * <phk@FreeBSD.ORG> wrote this file.  As long as you retain this notice you
 * can do whatever you want with this stuff. If we meet some day, and you think
 * this stuff is worth it, you can buy me a beer in return.   Poul-Henning Kamp
 * ----------------------------------------------------------------------------
 * $Id: emit_gcode.c,v 1.5 2009/06/03 19:04:42 phk Exp $
 *
 */

#include <assert.h>
#include <stdio.h>
#include <math.h>
#include <sys/queue.h>
#include "plt2g.h"

#define PROBE_N	4

static int px[PROBE_N][PROBE_N], py[PROBE_N][PROBE_N], pz[PROBE_N][PROBE_N];
static int dx, dy;
static int saved_lifts, lifts;

static void
do_probe(struct job *j, int lx, int ly, int lz)
{
	(void)j;
	px[lx][ly] = lx * dx;
	py[lx][ly] = ly * dy;
	pz[lx][ly] = lz;
	fprintf(j->fo, "(PROBE[%d,%d] %.3f %.3f -> %d)\n",
	    lx, ly, 1e-3 * px[lx][ly], 1e-3 * py[lx][ly], pz[lx][ly]);
	fprintf(j->fo, "O100 call [%.3f] [%.3f] [#2] [#4]\n",
	    1e-3 * px[lx][ly], 1e-3 * py[lx][ly]);
	fprintf(j->fo, "#%d=#5063\n", pz[lx][ly]);
}

static char *
depth(int x, int y)
{
	static char buf[BUFSIZ];
	int lx, ly;

	double t, u;

	lx = x / dx;
	ly = y / dy;
	assert(lx < PROBE_N - 1);
	assert(ly < PROBE_N - 1);

	t = (x - px[lx][ly]) / (double)(px[lx+1][ly+1] - px[lx][ly]);
	u = (y - py[lx][ly]) / (double)(py[lx+1][ly+1] - py[lx][ly]);

	sprintf(buf, "[%.3f*#%d + %.3f*#%d + %.3f*#%d + %.3f*#%d + #3]",
	    (1-t)*(1-u), pz[lx][ly],
	    (  t)*(1-u), pz[lx+1][ly],
	    (  t)*(  u), pz[lx+1][ly+1],
	    (1-t)*(  u), pz[lx][ly+1]);

	return (buf);
}

static void
emit_segment(struct job *j, struct segment *s)
{
	struct vector *v;
	int nx, ny;
	double q;

	(void)j;
	v = TAILQ_FIRST(&s->vectors);
	if (v->x0 == HOME_X && v->y0 == HOME_Y)
		return;
	fprintf(j->fo, "\n");

	if (hypot(v->x0 - j->bit_x, v->y0 - j->bit_y) > bit_width) {
		fprintf(j->fo, "G00 Z[#2]\n");
		fprintf(j->fo, "G00 X%.3f Y%.3f\n", 1e-3*v->x0, 1e-3 *v->y0);
		lifts++;
	} else {
		saved_lifts++;
	}
	fprintf(j->fo, "G01 X%.3f Y%.3f Z%s F100\n",
	    1e-3*v->x0, 1e-3 *v->y0, depth(v->x0, v->y0));
	j->bit_x = j->x0;
	j->bit_y = j->y0;
	TAILQ_FOREACH(v, &s->vectors, list) {
		/* Probe the vector 9 times along its length and emit
		 * any points that would warrant a different depth estimate
		 */
		for (q = .1; q < 1.0; q += .1) {
			nx = v->x0 + q * (v->x1 - v->x0);
			ny = v->y0 + q * (v->y1 - v->y0);
			if (nx / dx == j->bit_x / dx &&
			    ny / dy == j->bit_y / dy)
				continue;
		}
		fprintf(j->fo, "G01 X%.3f Y%.3f Z%s\n",
		    1e-3*nx, 1e-3 *ny, depth(nx, ny));
		j->bit_x = nx;
		j->bit_y = ny;
	}
}

void
emit_gcode(struct job *j)
{
	struct run *r;
	int lx, ly, i, z;

	j->fo = fopen("_.ngc", "w");
	assert(j->fo != NULL);
	lx = 0;
	ly = 0;
	lifts = 0;
	saved_lifts = 0;

	fprintf(j->fo, "(XXX: bla bla bla)\n");

	fprintf(j->fo, "\n");
	fprintf(j->fo, "(Things you can change:)\n");
	fprintf(j->fo, "#1=12	(Clearance height)\n");
	fprintf(j->fo, "#2=1	(Traverse height)\n");
	fprintf(j->fo, "#3=-.1	(Route depth)\n");
	fprintf(j->fo, "#4=-1	(Probe depth)\n");

	fprintf(j->fo, "\n");
	fprintf(j->fo, "(Things you should not change:)\n");
	fprintf(j->fo, "G21	(mm)\n");
	fprintf(j->fo, "G90	(Abs coords)\n");

	fprintf(j->fo, "\n");
	fprintf(j->fo, "M05	(Stop Motor)\n");
	fprintf(j->fo, "G00 Z[#1]	(Safe height)\n");
	fprintf(j->fo, "G00 X0 Y0	(.. on the ranch)\n");

	fprintf(j->fo, "\n");
	fprintf(j->fo, "(probe grid)\n");
	fprintf(j->fo, "(params: x y traverse_height probe_depth)\n");
	fprintf(j->fo, "O100 sub\n");
	fprintf(j->fo, "G00 X[#1] Y[#2] Z[#3]\n");
	fprintf(j->fo, "G38.2 Z[#4] F25\n");
	fprintf(j->fo, "G00 Z[#3]\n");
	fprintf(j->fo, "O100 endsub\n");
	z = 2000;
	dx = (j->x1 + 100) / (PROBE_N - 1);
	dy = (j->y1 + 100) / (PROBE_N - 1);
	for (lx = 0; lx < PROBE_N; lx++) {
		if (lx & 1)
			for (ly = PROBE_N - 1; ly >= 0; ly--)
				do_probe(j, lx, ly, z++);
		else
			for (ly = 0; ly < PROBE_N; ly++)
				do_probe(j, lx, ly, z++);
	}

	fprintf(j->fo, "\n");
	fprintf(j->fo, "(Gentlemen, start your engines)\n");
	fprintf(j->fo, "M03 S1000\n");

	fprintf(j->fo, "G00 X0 Y0 Z[#2]	(.. on the ranch)\n");
	j->bit_x = j->bit_y = 0;

	TAILQ_FOREACH(r, &j->runs, list) {
		if (r->seglist == NULL)
			continue;
		for (i = 0; i < r->nseg; i++)
			emit_segment(j, r->seglist[i]);
	}
 	fprintf(j->fo, "\n");
 	fprintf(j->fo, "M05		(Motor off)\n");
	fprintf(j->fo, "G00 Z[#1]	(Safe height)\n");
	fprintf(j->fo, "M02\n");
	fclose(j->fo);
	fprintf(stderr, "Bit Width = %d -> saved lifts = %d, lifts = %d\n",
	    bit_width, saved_lifts, lifts);
}