/*
 *                            COPYRIGHT
 *
 *  camv-rnd - electronics-related CAM viewer
 *  Copyright (C) 2019 Tibor 'Igor2' Palinkas
 *
 *  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.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
 *
 *  Contact:
 *    Project page: http://repo.hu/projects/camv-rnd
 *    lead developer: http://repo.hu/projects/camv-rnd/contact.html
 *    mailing list: camv-rnd (at) list.repo.hu (send "subscribe")
 */

#include <librnd/config.h>
TODO("remove this")
#include <stdio.h>

#include "gcode_vm.h"
#include "gcode_exec.h"


typedef enum {
	MOVE_TRAVEL,
	MOVE_LINEAR,
	MOVE_CW,
	MOVE_CCW,
	MOVE_NOP
} move_t;

typedef struct {
	double x, y, z;
	double ox, oy, oz; /* current coordinate system offsets by the program coord system */
	double ox92, oy92, oz92; /* current coordinate system offsets by G92*/
	double tool_dia;
	unsigned imperial:1;
	unsigned stop:1;
	unsigned relative:1;
	move_t move;
} g_state_t;

typedef struct gcode_exec_s {
	const gcode_execute_op_t *ops;
	g_state_t last_state, state;
	double reg['Z'-'A'+1];
	char reg_set['Z'-'A'+1];
	struct {
		unsigned setcrd10:1; /* G10 */
		unsigned setcrd92:1; /* G92 */
		unsigned move:1;
		unsigned home:1;
		unsigned home_idx:1; /* home position index is reg P when home == 1 */
		int pcs; /* program coordinate system index */
	} cmd;
	double px[10], py[10], pz[10]; /* program coordinate systems (G54...G59); 0 is for G53, and is always 0;0;0 */
} gcode_exec_t;


void gcode_execute_init(gcode_prg_t *prg, const gcode_execute_op_t *ops)
{
	gcode_exec_t *ctx;
	prg->exec_data = ctx = calloc(sizeof(gcode_exec_t), 1);
	prg->execute_code = gcode_execute_code;
	ctx->ops = ops;
}

void gcode_execute_uninit(gcode_prg_t *prg)
{
	free(prg->exec_data);
}

static double COORD(gcode_exec_t *ctx, double last, double v)
{
	if (ctx->state.imperial) v = v * 25.4;
	if (ctx->state.relative) v += last;
	return v;
}

#define CRD2REAL(st, axis) (st.axis + st.o ## axis + st.o ## axis ## 92)

/* Execute order: of relevant commands:
	T               (executed in gcode_execute_code_())
	M6              (executed/ignored in gcode_execute_code_())
	G20,G21         (executed in gcode_execute_code_())
	G54..G59.*      (executed in gcode_execute_code_())
	G90,G91         (executed in gcode_execute_code_())
	G28,G30 | G10 | G92*              STEP1
	G0,G1,G2,G3,G80..G89 plus G53     STEP2
	M0,M1,M2,M30,M60                  STEP3
*/
static int gcode_execute_code_do(gcode_prg_t *prg)
{
	gcode_exec_t *ctx = prg->exec_data;


/*	printf("EXEC DO  ---\n");*/

	/* update x;y;z */
	ctx->state.x = COORD(ctx, ctx->last_state.x, ctx->reg['X'-'A']);
	ctx->state.y = COORD(ctx, ctx->last_state.y, ctx->reg['Y'-'A']);
	ctx->state.z = COORD(ctx, ctx->last_state.z, ctx->reg['Z'-'A']);

	/* STEP1 */
	if (ctx->cmd.home) {
		int idx = 0;

		ctx->ops->travel(prg, CRD2REAL(ctx->last_state, x), CRD2REAL(ctx->last_state, y), CRD2REAL(ctx->last_state, z), CRD2REAL(ctx->state, x), CRD2REAL(ctx->state, y), CRD2REAL(ctx->state, z));
		if (ctx->cmd.home_idx)
			idx = (int)ctx->reg['P'-'A'];
		if (idx > 1) {
			prg->error(prg, 1, "Invalid home index (only one is supported for now)");
			return -1;
		}

		ctx->state.x = prg->params[5161+20*idx];
		ctx->state.y = prg->params[5162+20*idx];
		ctx->state.z = prg->params[5163+20*idx];
		ctx->ops->travel(prg, CRD2REAL(ctx->last_state, x), CRD2REAL(ctx->last_state, y), CRD2REAL(ctx->last_state, z), CRD2REAL(ctx->state, x), CRD2REAL(ctx->state, y), CRD2REAL(ctx->state, z));

		if (ctx->cmd.setcrd10) {
			prg->error(prg, 1, "can't use G10 and homing in the same line!");
			return -1;
		}
	}
	else if (ctx->cmd.setcrd10) {
		int idx;
		if (ctx->reg['L'-'A'] != 2) {
			prg->error(prg, 1, "G10 requires L2");
			return -1;
		}
		idx = (int)ctx->reg['P'-'A'];
		if ((idx < 1) || (idx > 9)) {
			prg->error(prg, 1, "Invalid P value for G10 L2 - must be between 1 and 9");
			return -1;
		}
		if (ctx->state.relative) {
			prg->error(prg, 1, "Can not use G10 with relative coordinates");
			return -1;
		}
		ctx->px[idx] = CRD2REAL(ctx->last_state, x) - COORD(ctx, 0, ctx->reg['X'-'A']);
		ctx->py[idx] = CRD2REAL(ctx->last_state, y) - COORD(ctx, 0, ctx->reg['Y'-'A']);
		ctx->pz[idx] = CRD2REAL(ctx->last_state, z) - COORD(ctx, 0, ctx->reg['Z'-'A']);
	}
	else if (ctx->cmd.setcrd92) {
		if (ctx->state.relative) {
			prg->error(prg, 1, "Can not use G92 with relative coordinates");
			return -1;
		}
		if (!ctx->reg_set['X'-'A'] && !ctx->reg_set['Y'-'A'] && !ctx->reg_set['Z'-'A']) {
			prg->error(prg, 1, "At least one of X, Y or Z is needed for G92");
			return -1;
		}
		ctx->state.ox92 = CRD2REAL(ctx->last_state, x) - COORD(ctx, 0, ctx->reg['X'-'A']);
		ctx->state.oy92 = CRD2REAL(ctx->last_state, y) - COORD(ctx, 0, ctx->reg['Y'-'A']);
		ctx->state.oz92 = CRD2REAL(ctx->last_state, z) - COORD(ctx, 0, ctx->reg['Z'-'A']);
	}

	/* STEP2 */
	if (ctx->reg_set['X'-'A'] || ctx->reg_set['Y'-'A'] || ctx->reg_set['Z'-'A']) {
		switch(ctx->state.move) {
			case MOVE_TRAVEL: ctx->ops->travel(prg, CRD2REAL(ctx->last_state, x), CRD2REAL(ctx->last_state, y), CRD2REAL(ctx->last_state, z), CRD2REAL(ctx->state, x), CRD2REAL(ctx->state, y), CRD2REAL(ctx->state, z)); break;
			case MOVE_LINEAR: ctx->ops->linear(prg, CRD2REAL(ctx->last_state, x), CRD2REAL(ctx->last_state, y), CRD2REAL(ctx->last_state, z), CRD2REAL(ctx->state, x), CRD2REAL(ctx->state, y), CRD2REAL(ctx->state, z)); break;
			case MOVE_CW:
			case MOVE_CCW:
				prg->error(prg, 1, "arc not yet implemented");
				break;
			case MOVE_NOP: break;
		}
	}


	/* update state */
	ctx->last_state = ctx->state;

	/* reset per line states */
	memset(ctx->reg_set, 0, sizeof(ctx->reg_set));
	memset(&ctx->cmd, 0, sizeof(ctx->cmd));

	/* STEP3 */
	return ctx->state.stop;
}

#define CN(code,num) (unsigned)((((unsigned)code << 16) | (unsigned)(num*10)))
#define CNF(code,num,f) (unsigned)((((unsigned)code << 16) | ((unsigned)(num*10) + (unsigned)f)))
#define CND(code,num) (unsigned)((((unsigned)code << 16) | (unsigned)((double)num*10.0)))


static int gcode_execute_code_(gcode_prg_t *prg, int code, double param)
{
	gcode_exec_t *ctx = prg->exec_data;

	if ((code == 'G') || (code == 'M')) {
		switch(CND(code, param)) {
			case CN('G', 0): ctx->state.move = MOVE_TRAVEL; ctx->cmd.move = 1; break;
			case CN('G', 1): ctx->state.move = (!prg->cfg.laser || prg->laser_on) ? MOVE_LINEAR : MOVE_TRAVEL; ctx->cmd.move = 1; break;
			case CN('G', 2): ctx->state.move = MOVE_CW; ctx->cmd.move = 1; break;
			case CN('G', 3): ctx->state.move = MOVE_CCW; ctx->cmd.move = 1; break;
			case CN('G', 4): ctx->state.move = MOVE_NOP; ctx->cmd.move = 1; break; /* dwell */

			case CN('G', 10): ctx->cmd.setcrd10 = 1; break;

			case CN('G', 17): break;
			case CN('G', 18): case CN('G', 19): prg->error(prg, 1, "Only the XY plane is supported - no 3d milling!"); return -1;

			case CN('G', 20): ctx->state.imperial = 1; break;
			case CN('G', 21): ctx->state.imperial = 0; break;

			case CN('G', 28): ctx->cmd.home = 1; ctx->cmd.home_idx = 0; break;
			case CN('G', 30): ctx->cmd.home = 1; ctx->cmd.home_idx = 1; break;

			case CN('G', 41): case CN('G', 42): case CN('G', 43): case CN('G', 44): case CN('G', 49): prg->error(prg, 1, "Tool compensation not supported"); return -1;

			case CN('G', 53): ctx->cmd.pcs = 0; break;
			case CN('G', 54): ctx->cmd.pcs = 1; break;
			case CN('G', 55): ctx->cmd.pcs = 2; break;
			case CN('G', 56): ctx->cmd.pcs = 3; break;
			case CN('G', 57): ctx->cmd.pcs = 4; break;
			case CN('G', 58): ctx->cmd.pcs = 5; break;
			case CN('G', 59): ctx->cmd.pcs = 6; break;
			case CNF('G', 59,1): ctx->cmd.pcs = 7; break; /* 59.1 */
			case CNF('G', 59,2): ctx->cmd.pcs = 8; break; /* 59.2 */
			case CNF('G', 59,3): ctx->cmd.pcs = 9; break; /* 59.3 */

			case CN('G', 61): case CN('G', 64): break; /* corner physics */

			case CN('G', 90): ctx->state.relative = 0; break;
			case CN('G', 91): ctx->state.relative = 1; break;

			case CN('G', 92): ctx->cmd.setcrd92 = 1; break;
			case CNF('G', 92,1): case CNF('G', 92,2):
			case CNF('G', 92,3): case CNF('G', 92,4): prg->error(prg, 1, "G92.* not supported"); return -1;

			case CN('G', 95): case CN('G', 96): case CN('G', 97): break; /* ignore spindle/feed speed */

			case CN('M', 1): case CN('M', 60): break; /* temporary program stop */
			case CN('M', 0): case CN('M', 2): case CN('M', 30): ctx->state.stop = 1; break;

			/* spindle start/stop: draw on/off for laser, ignore for anything else */
			case CN('M', 3): case CN('M', 4): if (prg->cfg.laser) prg->laser_on = 1; break;
			case CN('M', 5):                  if (prg->cfg.laser) prg->laser_on = 0; break;

			case CN('M', 7): case CN('M', 8): case CN('M', 9): break; /* ignore coolant */
			case CN('M', 10): case CN('M', 11): break; /* ignore pallet clamp */
			case CN('M', 13): break; /* ignore spindle+coolant */
			case CN('M', 48): case CN('M', 49): break; /* ignore feedrate */
			case CN('M', 52): break; /* ignore tool unload */

			default:
				{
					char tmp[256];
					sprintf(tmp, "WARNING: unhandleg code: %c%d", code, (int)param);
					prg->error(prg, 1, tmp);
				}
		}
	}
	else {
		ctx->reg[code - 'A'] = param;
		ctx->reg_set[code - 'A'] = 1;
		switch(code) {
			case 'T':
TODO("tool code: tools[(int)param];");
				ctx->state.tool_dia = 1;
				break;
		}
	}

/*	printf("EXEC %c %f\n", code, param); */
	return 0;
}


int gcode_execute_code(gcode_prg_t *prg, int code, double param)
{
	if (code == DO)
		return gcode_execute_code_do(prg);
	return gcode_execute_code_(prg, code, param);
}