/*
 *                            COPYRIGHT
 *
 *  camv-rnd - electronics-related CAM viewer
 *  Copyright (C) 2019,2024 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 <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <assert.h>

#include "gcode_vm.h"
#include "gcode.tab.h"

#ifdef M_PI
#	define G_PI M_PI
#else
#	define G_PI 3.141592654
#endif

#define DEG2RAD (G_PI/180.0)
#define RAD2DEG (180.0/G_PI)

/* Implementation idea borrowed from an old gcc (GPL'd) */
static double gcode_round(double x)
{
	double t;

/* We should check for inf here, but inf is not in C89 */

	if (x >= 0.0) {
		t = ceil(x);
		if (t - x > 0.5)
			t -= 1.0;
		return t;
	}

	t = ceil(-x);
	if ((t + x) > 0.5)
		t -= 1.0;
	return -t;
}


#define LIST_APPEND(FIRST, LAST, ITEM) \
	do { \
		if (FIRST != NULL) { \
			LAST->next = ITEM; \
			LAST = ITEM; \
		} \
		else \
			FIRST = LAST = ITEM; \
	} while(0)

static gcode_inst_t *gcode_append_main(gcode_prg_t *ctx)
{
	gcode_inst_t *ex = malloc(sizeof(gcode_inst_t));
	LIST_APPEND(ctx->first, ctx->last, ex);
	ex->next = NULL;
	ctx->ip++;
	return ex;
}

static gcode_inst_t *gcode_append_2nd(gcode_prg_t *ctx)
{
	gcode_inst_t *ex = malloc(sizeof(gcode_inst_t));
	LIST_APPEND(ctx->first2, ctx->last2, ex);
	ex->next = NULL;
	ctx->ip2++;
	return ex;
}

gcode_inst_t *gcode_append(gcode_prg_t *ctx, gcode_i_t inst, double payload)
{
	gcode_inst_t *ex = (ctx->delay == GCP_DELAY_OFF) ? gcode_append_main(ctx) : gcode_append_2nd(ctx);
	ex->inst = inst;
	ex->payload = payload;
	ex->lineno = ctx->line;
	ex->do_pop = 0;
	return ex;
}

void gcode_delayed(gcode_prg_t *ctx, gcode_delay_t delay)
{
	if ((delay == GCP_DELAY_APPLY) && (ctx->first2 != NULL)) {
		LIST_APPEND(ctx->first, ctx->last, ctx->first2);
		ctx->last = ctx->last2;
		ctx->first2 = ctx->last2 = NULL;
		ctx->ip += ctx->ip2;
		ctx->ip2 = 0;
	}
	else
		ctx->delay = delay;
}

void gcode_set_lineno(gcode_prg_t *ctx, int lineno)
{
	ctx->lineno = lineno;
	if (ctx->set_lineno != NULL)
		ctx->set_lineno(ctx, lineno);
}

void gcode_dump_inst(const char *prefix, gcode_inst_t *inst)
{
	printf("%s", prefix);

	switch(inst->inst) {
		case PUSH_NUM: printf("PUSH_NUM(%f)\n", inst->payload); break;
		case ADD:    printf("ADD\n"); break;
		case SUB:    printf("SUB\n"); break;
		case MUL:    printf("MUL\n"); break;
		case DIV:    printf("DIV\n"); break;
		case ASSIGN: printf("ASSIGN\n"); break;
		case PARAM:  printf("PARAM(%d)\n", (int)inst->payload); break;
		case DO:     printf("DO\n\n"); break;

		case T_NUM:  printf("NUM(%f)\n", inst->payload); break;
		case T_DEC:  printf("DEC(%d)\n", (int)inst->payload); break;
		case T_LINENO: printf("N(%d)\n", (int)inst->payload); break;
		case T_ATAN: printf("ATAN\n"); break;
		case T_ACOS: printf("ACOS\n"); break;
		case T_ASIN: printf("ASIN\n"); break;
		case T_ABS:  printf("ABS\n"); break;
		case T_COS:  printf("COS\n"); break;
		case T_SIN:  printf("SIN\n"); break;
		case T_TAN:  printf("TAN\n"); break;
		case T_FIX:  printf("FIX\n"); break;
		case T_FUP:  printf("FUP\n"); break;
		case T_EXP:  printf("EXP\n"); break;
		case T_LN:   printf("LN\n"); break;
		case T_ROUND:printf("ROUND\n"); break;
		case T_SQRT: printf("SQRT\n"); break;
		case T_MOD:  printf("MOD\n"); break;
		case T_OR:   printf("OR\n"); break;
		case T_XOR:  printf("XOR\n"); break;
		case T_AND:  printf("AND\n"); break;
		case 'M':
		case 'G':
			printf("%c%02d\n", inst->inst, (int)inst->payload); break;
		default:
			if ((inst->inst >= 'A') && (inst->inst <= 'Z'))
				printf("%c %f\n", inst->inst, inst->payload);
			else
				printf("*invalid instruction* %d\n", inst->inst);
	}
}

void gcode_dump_prg(const char *prefix, gcode_prg_t *prg)
{
	gcode_inst_t *i;
	for(i = prg->first; i != NULL; i = i->next)
		gcode_dump_inst(prefix, i);
}

static void push(gcode_prg_t *prg, double d)
{
	double *s = vtd0_alloc_append(&prg->stack, 1);
	*s = d;
}

static double pop(gcode_prg_t *prg)
{
	assert(prg->stack.used > 0);
	return prg->stack.array[--prg->stack.used];
}

int gcode_execute(gcode_prg_t *ctx)
{
	gcode_inst_t *i;
	double a, b;
	int res, idx, limit = sizeof(ctx->params)/sizeof(ctx->params[0]);
	char tmp[256];

	ctx->lineno = -1;
	ctx->ip = 0;
	for(i = ctx->first; i != NULL; i = i->next, ctx->ip++) {
		switch(i->inst) {
			case PUSH_NUM: push(ctx, i->payload); break;
			case ADD:      a = pop(ctx); b = pop(ctx); push(ctx, a+b); break;
			case SUB:      a = pop(ctx); b = pop(ctx); push(ctx, a-b); break;
			case MUL:      a = pop(ctx); b = pop(ctx); push(ctx, a*b); break;
			case DIV:      a = pop(ctx); b = pop(ctx); push(ctx, a/b); break;
			case T_LINENO: ctx->lineno = i->payload; break;
			case T_ATAN:   a = pop(ctx); b = pop(ctx); push(ctx, atan2(a, b)*RAD2DEG); break;
			case T_ACOS:   a = pop(ctx); push(ctx, acos(a)*RAD2DEG); break;
			case T_ASIN:   a = pop(ctx); push(ctx, asin(a)*RAD2DEG); break;
			case T_ABS:    a = pop(ctx); push(ctx, fabs(a)); break;
			case T_COS:    a = pop(ctx); push(ctx, cos(a*DEG2RAD)); break;
			case T_SIN:    a = pop(ctx); push(ctx, sin(a*DEG2RAD)); break;
			case T_TAN:    a = pop(ctx); push(ctx, tan(a*DEG2RAD)); break;
			case T_FIX:    a = pop(ctx); push(ctx, floor(a)); break;
			case T_FUP:    a = pop(ctx); push(ctx, ceil(a)); break;
			case T_EXP:    a = pop(ctx); push(ctx, exp(a)); break;
			case T_LN:     a = pop(ctx); push(ctx, log(a)); break;
			case T_ROUND:  a = pop(ctx); push(ctx, gcode_round(a)); break;
			case T_SQRT:   a = pop(ctx); push(ctx, sqrt(a)); break;
			case T_MOD:    a = pop(ctx); b = pop(ctx); push(ctx, fmod(a,b)); break;
			case T_OR:     a = pop(ctx); b = pop(ctx); push(ctx, (a!=0) || (b!=0)); break;
			case T_XOR:    a = pop(ctx); b = pop(ctx); push(ctx, ((a!=0) || (b!=0)) && ((a==0) || (b==0))); break;
			case T_AND:    a = pop(ctx); b = pop(ctx); push(ctx, (a!=0) && (b!=0)); break;
			case ASSIGN:
				a = pop(ctx); b = pop(ctx);
				idx = (int)b;
				if ((idx < 0) || (idx >= limit)) {
					sprintf(tmp, "Error: assignment out of range for parameter %d\n", idx);
					ctx->error(ctx, 1, tmp);
					return -1;
				}
				else
					ctx->params[idx] = a;
				break;
			case PARAM:
				a = pop(ctx);
				idx = (int)a;
				if ((idx < 0) || (idx >= limit)) {
					sprintf(tmp, "Error: assignment out of range for parameter %d\n", idx);
					ctx->error(ctx, 1, tmp);
					return -1;
				}
				else
					push(ctx, ctx->params[idx]);
				break;
			case DO:
				res = ctx->execute_code(ctx, i->inst, 0);
				if (res != 0)
					return res;
				break;
			default:
				if ((i->inst >= 'A') && (i->inst <= 'Z')) {
					if (i->do_pop)
						a = pop(ctx);
					else
						a = i->payload;
					res = ctx->execute_code(ctx, i->inst, a);
					if (res != 0)
						return res;
				}
		}
	}
	return 0;
}