/* vm.c */

#include <stdio.h>
#include <assert.h>
#include <stdlib.h>
#include <string.h>
#include <err.h>
#include <sys/time.h>
#include <poll.h>
#include <signal.h>
#include <fcntl.h>
#include <errno.h>
#include <math.h>
#include <stdarg.h>

#include "vm.h"
#include "code.h"
#include "dmx.h"
#include "hash.h"
#include "mem.h"
#include "midi.h"
#include "beatdetect.h"
#include "map3d.h"
#include "mouse.h"
#include "cmdsocket.h"
#include "sql.h"

#define DEBUG 0

typedef char instr;

struct vm_thread {
	instr *pc;
	stkentry *sp;
	stkentry *stackbase;
	size_t stacksize;
	int queue;
	struct timeval time;
	struct vm_thread *next;
	struct vm_thread *prev;
};

#define VM_MAXPOLLFD	16

#define ARRAYBLOCKSIZE	512

#define SLEEPTIME_SEC	60
#define SLEEPTIME_NSEC	0

struct pollfd vm_pollfd[VM_MAXPOLLFD];
int vm_pollfdqueue[VM_MAXPOLLFD];

int vm_npollfds;

volatile int vm_caughtsignal;

struct vm_thread *vm_queues[VM_MAXQUEUES];


struct vm_thread *vm_current = NULL;

instr *vm_codearea = NULL;
size_t vm_codesize = 0;
int vm_threads = 0;

struct hashentry *fnhash[HASHSIZE];
struct hashentry *globhash[HASHSIZE];
struct hashentry *arrayhash[HASHSIZE];

#define VM_CODESIZE 65536
#define VM_STACKSIZE 65536

#define VM_STRING_MAX 1024

#define GLOB_MAXNAMELEN	1024

void vm_destroy(struct vm_thread *);
void vm_unqueue(struct vm_thread *);
void vm_queue(struct vm_thread *, int);

#include "abi.h"

extern vm_intfn vm_intfn_table[];
extern const int vm_intfn_size;

void stack_push(struct vm_thread *thread, stkentry e);
void stack_pop(struct vm_thread *thread, int count);
stkentry stack_get(struct vm_thread *thread, int count);
float stack_getreal(struct vm_thread *thread, int count);
char *stack_getstr(struct vm_thread *thread, int len, int count);
void stack_poke(struct vm_thread *thread, int count, stkentry value);
void stack_pokereal(struct vm_thread *thread, int count, float value);

int vm_intfn___nop(void)
{
	/* Do nothing */
	return 1;
}

/*
 * global_store and global_load are special. 
 * The compiler does not allocate space for a return value.
 * They also change the stack pointer.
 */
int vm_intfn___global_store(void)
{
	int len, next, value;
	struct hashentry *hashptr;
	char varname[GLOB_MAXNAMELEN+1];
	char *vptr;

	len = stack_get(vm_current, 0);
	next = (len + sizeof(stkentry)-1) / sizeof(stkentry) + 1;

	if (len > GLOB_MAXNAMELEN)
		goto gstoreout;

	vptr = stack_getstr(vm_current, len, 0);
	value = stack_get(vm_current, next);

	strncpy(varname, vptr, len);
	varname[len] = '\0';

	hashptr = hash_lookup(globhash, varname, 1);
	assert(hashptr != NULL);

	if (hashptr->name == NULL) {
		hashptr->name = safe_malloc(strlen(varname)+1);
		strcpy(hashptr->name, varname);
	}

	hashptr->value = value;

gstoreout:
	stack_pop(vm_current, next+1);

	return 1;
}

int vm_intfn___global_load(void)
{
	int len, next, value;
	struct hashentry *hashptr;
	char varname[GLOB_MAXNAMELEN+1];
	char *vptr;

	len = stack_get(vm_current, 0);
	next = (len + sizeof(stkentry)-1) / sizeof(stkentry) + 1;

	if (len > GLOB_MAXNAMELEN) {
		value = 0;
		goto gloadout;
	}

	vptr = stack_getstr(vm_current, len, 0);

	strncpy(varname, vptr, len);
	varname[len] = '\0';

	hashptr = hash_lookup(globhash, varname, 0);
	if (hashptr)
		value = hashptr->value;
	else
		value = 0;

gloadout:
	stack_pop(vm_current, next);
	stack_push(vm_current, value);

	return 1;
}

int vm_intfn___global_array_store(void)
{
	int len, next, value, index;
	struct hashentry *hashptr;
	long *arrayptr;
	char varname[GLOB_MAXNAMELEN+1];
	char *vptr;

	len = stack_get(vm_current, 0);
	next = (len + sizeof(stkentry)-1) / sizeof(stkentry) + 1;

	if (len > GLOB_MAXNAMELEN)
		goto gstorearrayout;

	vptr = stack_getstr(vm_current, len, 0);
	index = stack_get(vm_current, next);
	value = stack_get(vm_current, next+1);

	strncpy(varname, vptr, len);
	varname[len] = '\0';

	hashptr = hash_lookup(arrayhash, varname, 1);
	assert(hashptr != NULL);

	if (hashptr->name == NULL) {
		hashptr->name = safe_malloc(strlen(varname)+1);
		strcpy(hashptr->name, varname);
		hashptr->value = (int)safe_malloc(ARRAYBLOCKSIZE*sizeof(long));
		hashptr->flags = ARRAYBLOCKSIZE;
	}

	if (index >= hashptr->flags) {
		hashptr->flags = index + ARRAYBLOCKSIZE;
		hashptr->flags -= hashptr->flags % ARRAYBLOCKSIZE;
		hashptr->value = (int)safe_realloc((void *)hashptr->value,
		    hashptr->flags * sizeof(long));
	}

	arrayptr = (long *)hashptr->value;
	arrayptr[index] = value;

gstorearrayout:
	stack_pop(vm_current, next+2);

	return 1;
}

int vm_intfn___global_array_load(void)
{
	int len, next, value, index;
	struct hashentry *hashptr;
	long *arrayptr;
	char varname[GLOB_MAXNAMELEN+1];
	char *vptr;

	len = stack_get(vm_current, 0);
	next = (len + sizeof(stkentry)-1) / sizeof(stkentry) + 1;

	if (len > GLOB_MAXNAMELEN) {
		value = 0;
		goto gloadarrayout;
	}

	vptr = stack_getstr(vm_current, len, 0);
	index = stack_get(vm_current, next);

	strncpy(varname, vptr, len);
	varname[len] = '\0';

	hashptr = hash_lookup(arrayhash, varname, 0);
	if (hashptr) {
		arrayptr = (long *)hashptr->value;
		if (index < hashptr->flags) {
			value = arrayptr[index];
		} else {
			value = 0;
		}
	} else
		value = 0;

gloadarrayout:
	stack_pop(vm_current, next+1);
	stack_push(vm_current, value);

	return 1;
}

int vm_intfn_printint(void)
{
	printf("%d", stack_get(vm_current, 1));
	return 1;
}

int vm_intfn_printreal(void)
{
	printf("%f", (float)stack_getreal(vm_current, 1));
	return 1;
}

int vm_intfn_printstr(void)
{
	char format[13];
	int len = stack_get(vm_current, 1);

	snprintf(format, 13, "%%.%ds", len);
	printf(format, stack_getstr(vm_current, len, 1));

	return 1;
}

int vm_intfn_dmxsetchannel(void)
{
	int channel;
	int value;

	channel = stack_get(vm_current, 2);
	value = stack_get(vm_current, 1);
//	printf("channel %d = %d\n", channel, value);
	dmx_setchannel(channel, value);
	return 1;
}

int vm_intfn_dmxoutput(void)
{
	dmx_output();
	return 1;
}

int vm_intfn_gettime(void)
{
	struct timeval tv;
	if (gettimeofday(&tv, NULL))
		err(1, "gettimeofday() failed");
	stack_poke(vm_current, 1, tv.tv_sec);	/* arg1 */
	stack_poke(vm_current, 0, tv.tv_usec);	/* return value */
	return 1;
}

int vm_intfn_waittime(void)
{
	struct vm_thread *ptr;
	struct vm_thread *last;
	vm_current->time.tv_sec = stack_get(vm_current, 2);
	vm_current->time.tv_usec = stack_get(vm_current, 1);

	ptr = vm_queues[VM_TIMEQ];
	last = NULL;
	/*
	 * We need to insert into time queue in time order. Because
	 * of this, we're going to manually insert into the queue here.
	 * Make sure that this is kept in sync with vm_queue().
	 */
	while (ptr && timercmp(&vm_current->time, &ptr->time, >=)) {
		last = ptr;
		ptr = ptr->next;
	}
	vm_current->prev = last;
	vm_current->next = ptr;
	vm_current->queue = VM_TIMEQ;
	if (last)
		last->next = vm_current;
	else
		vm_queues[VM_TIMEQ] = vm_current;
	if (ptr)
		ptr->prev = vm_current;

	vm_current = NULL;
	return 1;
}

int vm_intfn_wait(void)
{
	int queue = stack_get(vm_current, 1);

	if ((queue < VM_USERQMIN) || (queue >= VM_MAXQUEUES)) {
		printf("Attempt to wait on invalid wait queue\n");
		vm_destroy(vm_current);
		vm_current = NULL;
	}

	vm_unqueue(vm_current);
	vm_queue(vm_current, queue);
	vm_current = NULL;
	return 1;
}

void vm_wakeup(int queue)
{
	struct vm_thread *thread;

	while ((thread = vm_queues[queue]) != NULL) {
		vm_unqueue(thread);
		vm_queue(thread, VM_RUNQ);
	}
}

int vm_intfn_wakeup(void)
{
	int queue = stack_get(vm_current, 1);

	if ((queue < VM_USERQMIN) || (queue >= VM_MAXQUEUES)) {
		printf("Attempt to wake up invalid wait queue\n");
		return 1;
	}
	vm_wakeup(queue);
	return 1;
}

int vm_intfn_spawn(void)
{
	char buf[VM_STRING_MAX];
	int len = stack_get(vm_current, 1);
	if (len > VM_STRING_MAX) {
		printf("Excessive string length - can't spawn\n");
		return 1;
	}
	strncpy(buf, stack_getstr(vm_current, len, 1), len);
	buf[len] = '\0';
	vm_spawn(buf);
	return 1;
}

int vm_intfn_midi_read(void)
{
	while (midi_read()) {
		if (midi_filter()) {
			int button, value;
			midi_getcmd(&button, &value);
			stack_poke(vm_current, 1, button);   /* arg1 */
			stack_poke(vm_current, 0, value);  /* return value */
			return 1;
		}
	}
	vm_queue(vm_current, VM_MIDIQ);
	vm_current = NULL;
	return 0;
}

int vm_intfn_mouse_read(void)
{
	while (mouse_read()) {
		if (mouse_filter()) {
			int x, y, z;
			mouse_getpos(&x, &y, &z);
			stack_poke(vm_current, 2, x);   /* arg1 */
			stack_poke(vm_current, 1, y);   /* arg2 */
			stack_poke(vm_current, 0, z);  /* return value */
			return 1;
		}
	}
	vm_queue(vm_current, VM_MOUSEQ);
	vm_current = NULL;
	return 0;
}

int vm_intfn_cmdsocket_listen(void)
{
	stack_poke(vm_current, 0,
	    cmdsocket_listen(stack_get(vm_current, 1)));
	return 1;
}

int vm_intfn_cmdsocket_prefix(void)
{
	char buf[VM_STRING_MAX];
	int len = stack_get(vm_current, 1);

	strncpy(buf, stack_getstr(vm_current, len, 1), len);
	buf[len] = '\0';

	cmdsocket_prefix(buf);

	return 1;
}

int vm_intfn_cmdsocket_accept(void)
{
	if (!cmdsocket_accept()) {
	    vm_queue(vm_current, VM_CMDLISTENQ);
	    vm_current = NULL;
	    return 0;
	}
	return 1;
}

int vm_intfn_cmdsocket_read(void)
{
	if (!cmdsocket_read()) {
		vm_queue(vm_current, VM_CMDREADQ);
		vm_current = NULL;
		return 0;
	}
	return 1;
}

int vm_intfn_cmdsocket_write(void)
{
	int off = stack_get(vm_current, 1);
	int len = stack_get(vm_current, 2);
	char *buffer = stack_getstr(vm_current, len, 2);
	int newoff;

	newoff = cmdsocket_write(buffer, len, off);
	stack_poke(vm_current, 1, newoff);

	if (newoff != len) {
		vm_queue(vm_current, VM_CMDWRITEQ);
		vm_current = NULL;
		return 0;
	}
	return 1;
}

#ifdef NOT_YET

#define MIN(a, b) ((a < b) ? a : b)

#define MORE_QUERY_N(x, n) do {						\
		off = strncpy(buf+off, (x), MIN(VM_STRING_MAX-off, (n)) - buf;\
		if (off >= VM_STRING_MAX) {				\
		    printf("Excessive string length - can't perform query\n"); \
		    return 1;						\
		}							\
	} while (0)

#define MORE_QUERY(x) MORE_QUERY_N((x), VM_STRING_MAX)

int vm_intfn_sql_query(void)
{
	int nargs = stack_get(vm_current, 1);
	int len = stack_get(vm_current, nargs+1);
	char *fmt = stack_getstr(vm_current, len, nargs+1);
	char buf1[VM_STRING_MAX];
	char buf2[VM_STRING_MAX];
	char buf[VM_STRING_MAX];
	int off = 0;
	int result;
	int next;
	char *p, *p1;

	if (len > VM_STRING_MAX) {
		printf("Excessive string length - can't perform query\n");
		return 1;
	}
	strncpy(buf1, fmt, len);
	buf1[len] = '\0';
	
	p = buf1;
	while (p1 = strchr(p, '%') {
		if (p1) {
		    *p1 = 0;
		}
		if (p != buf1) {
			switch(*p) {
			case '%':
			    MORE_QUERY("%");
			    break;
			case 's':
			    next = (len + sizeof(stkentry)-1) / sizeof(stkentry) + 1;

			    MORE_QUERY_N(stack_getstr(vm_current, slen, nargs));
			}
		}
		MORE_QUERY(p);
		if (p1)
		    p = p1;
		else
		    break;
	}

	sql_query(buffer, len, &result);
	/* XXX what to do with an error here? */
	stack_poke(vm_current, 0, result); /* return value */
	return 1;
}
#else
int vm_intfn_sql_query(void)
{
	int len = stack_get(vm_current, 1);
	char *query = stack_getstr(vm_current, len, 1);
	int result;

	sql_query(query, len, &result);
	/* XXX what to do with an error here? */
	stack_poke(vm_current, 0, result); /* return value */
	return 1;
}

int vm_intfn_sql_query_1s(void)
{
	int len1, len2;
	char buf1[VM_STRING_MAX];
	char buf2[VM_STRING_MAX];
	char query[VM_STRING_MAX];
	char *arg1;
	char *arg2;
	int result;
	int next;

	next = 1;

	len2 = stack_get(vm_current, next);
	arg2 = stack_getstr(vm_current, len2, next);

	next += (len2 + sizeof(stkentry)-1) / sizeof(stkentry) + 1;

	len1 = stack_get(vm_current, next);
	arg1 = stack_getstr(vm_current, len1, next);

	if ((len1 > VM_STRING_MAX) || (len2 > VM_STRING_MAX)) {
		printf("Excessive string length - can't perform query\n");
		return 1;
	}
	strncpy(buf1, arg1, len1);
	buf1[len1] = '\0';
	strncpy(buf2, arg2, len2);
	buf2[len2] = '\0';

	snprintf(query, VM_STRING_MAX, buf1, buf2);

	sql_query(query, strlen(query), &result);

	/* XXX what to do with an error here? */
	stack_poke(vm_current, 0, result); /* return value */
	return 1;
}

int vm_intfn_sql_getvar(void)
{
	int len;
	char buf[VM_STRING_MAX];
	char query[VM_STRING_MAX];
	int result;

	len = stack_get(vm_current, 1);

	if (len >= VM_STRING_MAX) {
		printf("Excessive string length - can't perform query\n");
		return 1;
	}

	snprintf(buf, len+1, stack_getstr(vm_current, len, 1));
	snprintf(query, VM_STRING_MAX, "SELECT value FROM vars WHERE name=\"%s\"", buf);

	if (sql_query(query, strlen(query), &result) != 2)
		result = -1;

	/* XXX what to do with an error here? */
	stack_poke(vm_current, 0, result); /* return value */
	return 1;
}

int vm_intfn_sql_getvar_array(void)
{
	int len, off;
	char buf[VM_STRING_MAX];
	char query[VM_STRING_MAX];
	int result;

	off = stack_get(vm_current, 1);
	len = stack_get(vm_current, 2);

	if (len >= VM_STRING_MAX) {
		printf("Excessive string length - can't perform query\n");
		return 1;
	}

	snprintf(buf, len+1, stack_getstr(vm_current, len, 2));
	snprintf(query, VM_STRING_MAX, "SELECT value FROM vars WHERE name=\"%s[%d]\"", buf, off);

	if (sql_query(query, strlen(query), &result) != 2)
		result = -1;

	/* XXX what to do with an error here? */
	stack_poke(vm_current, 0, result); /* return value */
	return 1;
}

int vm_intfn_sql_setvar(void)
{
	int len, val;
	char buf[VM_STRING_MAX];
	char query[VM_STRING_MAX];
	int result;

	val = stack_get(vm_current, 1);
	len = stack_get(vm_current, 2);

	if (len >= VM_STRING_MAX) {
		printf("Excessive string length - can't perform query\n");
		return 1;
	}

	snprintf(buf, len+1, stack_getstr(vm_current, len, 2));
	snprintf(query, VM_STRING_MAX, "INSERT OR REPLACE INTO vars VALUES(\"%s\", %d)", buf, val);

	sql_query(query, strlen(query), &result);

	/* XXX what to do with an error here? */
	stack_poke(vm_current, 0, result); /* return value */
	return 1;
}

int vm_intfn_sql_setvar_array(void)
{
	int len, off, val;
	char buf[VM_STRING_MAX];
	char query[VM_STRING_MAX];
	int result;

	val = stack_get(vm_current, 1);
	off = stack_get(vm_current, 2);
	len = stack_get(vm_current, 3);

	if (len >= VM_STRING_MAX) {
		printf("Excessive string length - can't perform query\n");
		return 1;
	}

	snprintf(buf, len+1, stack_getstr(vm_current, len, 3));
	snprintf(query, VM_STRING_MAX, "INSERT OR REPLACE INTO vars VALUES(\"%s[%d]\", %d)", buf, off, val);

	sql_query(query, strlen(query), &result);

	/* XXX what to do with an error here? */
	stack_poke(vm_current, 0, result); /* return value */
	return 1;
}
#endif

int vm_intfn_beatdetect_read(void)
{
	if (!beatdetect_read()) {
		vm_queue(vm_current, VM_BEATQ);
		vm_current = NULL;
		return 0;
	}
	return 1;
}

int vm_intfn_beatdetect_phase(void)
{
	stack_pokereal(vm_current, 0, (float)beatdetect_getphase());
	return 1;
}

int vm_intfn_beatdetect_confidence(void)
{
	stack_pokereal(vm_current, 0, (float)beatdetect_getconfidence());
	return 1;
}

int vm_intfn_realtoint(void)
{
	stack_poke(vm_current, 0, (int)stack_getreal(vm_current, 1));
	return 1;
}

int vm_intfn_inttoreal(void)
{
	stack_pokereal(vm_current, 0, (float)stack_get(vm_current, 1));
	return 1;
}

int vm_intfn_map3d_setcal(void)
{
	map3d_setcal(stack_get(vm_current, 7), /* light */
	    stack_get(vm_current, 6), /* n */
	    (double)stack_getreal(vm_current, 5), /* x */
	    (double)stack_getreal(vm_current, 4), /* y */
	    (double)stack_getreal(vm_current, 3), /* v */
	    stack_get(vm_current, 2), /* pan */
	    stack_get(vm_current, 1)); /* tilt */
	return 1;
}

int vm_intfn_map3d_calibrate(void)
{
	stack_poke(vm_current, 0, map3d_calibrate(stack_get(vm_current, 1)));
	return 1;
}

int vm_intfn_map3d_transform(void)
{
	int pan, tilt;

	map3d_transform(stack_get(vm_current, 4), /* light */
	    (double)stack_getreal(vm_current, 3), /* x */
	    (double)stack_getreal(vm_current, 2), /* y */
	    (double)stack_getreal(vm_current, 1), /* z */
	    &pan, &tilt);
	stack_poke(vm_current, 4, pan);	/* arg1 */
	stack_poke(vm_current, 0, tilt); /* return value */
	return 1;
}

int vm_intfn_map3d_setparams(void)
{
	map3d_setparams(stack_get(vm_current, 6), /* light */
	    (double)stack_get(vm_current, 5), /* opan */
	    (double)stack_get(vm_current, 4), /* otilt */
	    (double)stack_getreal(vm_current, 3), /* lpan */
	    (double)stack_getreal(vm_current, 2), /* ltilt */
	    (double)stack_getreal(vm_current, 1)); /* dist */
	return 1;
}

int vm_intfn_map3d_load(void)
{
	stack_poke(vm_current, 0, map3d_load());
	return 1;
}

int vm_intfn_map3d_save(void)
{
	map3d_save();
	return 1;
}

int vm_intfn_sin(void)
{
	stack_pokereal(vm_current, 0, sinf(stack_getreal(vm_current, 1)));
	return 1;
}

int vm_intfn_cos(void)
{
	stack_pokereal(vm_current, 0, cosf(stack_getreal(vm_current, 1)));
	return 1;
}

int vm_intfn_random(void)
{
	stack_poke(vm_current, 0, random() % stack_get(vm_current, 1));
	return 1;
}

void vm_sighandler(int signal)
{
	vm_caughtsignal = 1;
}

void vm_wakeupafterpoll(void)
{
	int i;

	for (i = 0; i < vm_npollfds; i++)
		if (vm_pollfd[i].revents & POLLRDNORM) {
			vm_wakeup(vm_pollfdqueue[i]);
		}
}

void vm_handle_signal(void)
{
	int rv;

	vm_caughtsignal = 0;

	fflush(stdout);
	rv = poll(vm_pollfd, vm_npollfds, 0);
	if (rv == -1) {
		printf("poll() returned an error\n");
		exit(1);
	}
	if (rv == 0)
		return;

	vm_wakeupafterpoll();
}

void vm_init(void)
{
	vm_codesize = VM_CODESIZE;
	vm_codearea = safe_malloc(vm_codesize);
	bzero(vm_queues, sizeof(struct vm_thread *) * VM_MAXQUEUES);
	vm_current = NULL;
	hash_init(fnhash);
	hash_init(globhash);
	hash_init(arrayhash);
	vm_npollfds = 0;
	vm_caughtsignal = 0;
	signal(SIGIO, vm_sighandler);
	signal(SIGPIPE, SIG_IGN);
}

void vm_load_file(char *filename)
{
	FILE *fh;
	char *ptr;

	fh = fopen(filename, "r");
	assert(fh != NULL);

	for (ptr = vm_codearea; ptr < vm_codearea+vm_codesize; ptr++) {
		int c;
		c = fgetc(fh);
		if (c == EOF)
			break;
		*ptr = c;
	}

	fclose(fh);
}

#define GETINT(p, o) (((p[o] & 0xff) << 24) | \
			((p[o+1] & 0xff) << 16) | \
			((p[o+2] & 0xff) << 8) | \
			(p[o+3] & 0xff))

void vm_init_functions(void)
{
	int t;
	int len, type, fn, nargs;
	char *name;
	struct hashentry *ptr;

	/* First, let's check the magic */
	assert(vm_codearea[0] == MAGIC1);
	assert(vm_codearea[1] == MAGIC2);
	assert(vm_codearea[2] == VERSION1);
	assert(vm_codearea[3] == VERSION2);

	assert(GETINT(vm_codearea, 8) == vm_abiversion1);
	assert(GETINT(vm_codearea, 12) == vm_abiversion2);
	assert(GETINT(vm_codearea, 16) == vm_abiversion3);
	assert(GETINT(vm_codearea, 20) == vm_abiversion4);
	assert(GETINT(vm_codearea, 24) == vm_abiversion5);

	/* Now, get the function table pointer */
	t = GETINT(vm_codearea, 4);

	while ((len = GETINT(vm_codearea, t)) != 0) {
		type = GETINT(vm_codearea, t+4);
		if (type == 0) {
			fn = GETINT(vm_codearea, t+8);
			nargs = GETINT(vm_codearea, t+12);
			name = vm_codearea + t + 16;
			ptr = hash_lookup(fnhash, name, 1);

			printf("Function %s\n", name);
			fflush(stdout);

			if (ptr->name)
				err(1, "Function already exists");
			ptr->name = name;
			ptr->value = fn;
			ptr->flags = nargs;
		}
		t += len;
	}
}

void vm_load(char *filename)
{
	vm_load_file(filename);
	vm_init_functions();
}

void vm_unqueue(struct vm_thread *thread)
{
	if (thread->prev == NULL) {
		vm_queues[thread->queue] = thread->next;
	} else {
		thread->prev->next = thread->next;
	}

	if (thread->next)
		thread->next->prev = thread->prev;

	thread->next = NULL;
	thread->prev = NULL;
	thread->queue = VM_NOQUEUE;
}


/* Thread must not be queued anywhere else before calling this */
void vm_queue(struct vm_thread *thread, int queue)
{
	assert(thread != NULL);
	assert(thread->queue == VM_NOQUEUE);
	thread->prev = NULL;
	thread->next = vm_queues[queue];
	thread->queue = queue;
	vm_queues[queue] = thread;
}

int vm_spawn_args(char *fn, int n, ...)
{
	struct vm_thread *newt;
	struct hashentry *ptr;
	va_list ap;

	ptr = hash_lookup(fnhash, fn, 0);
	if (ptr == NULL) {
		printf("%s: No such function\n", fn);
		return 0;
	}

	printf("Spawning %s\n", fn);

	newt = malloc(sizeof(struct vm_thread));
	if (newt == NULL) {
		printf("malloc failed while spawning\n");
		return 0;
	}

	newt->pc = vm_codearea + ptr->value;
	printf("Starting execution at %p, %p, %x\n", newt->pc, vm_codearea, ptr->value);
	newt->stacksize = VM_STACKSIZE;
	newt->stackbase = malloc(newt->stacksize * sizeof(stkentry));
	if (newt->stackbase == NULL) {
		printf("malloc failed while spawning\n");
		free(newt);
		return 0;
	}
	newt->sp = newt->stackbase;

	stack_push(newt, 0); 	/* Return value */

	/* Push optional arguments */
	va_start(ap, n);
	while (n--) {
	    int a = va_arg(ap, int);
//	    printf("arg is %d\n", a);
	    stack_push(newt, a);
	}
	va_end(ap);

	/* Push return address here, to point to some special thread exit
	   routine */
	stack_push(newt, 0);	/* Return address */

	/* Insert into head of run queue */
	newt->prev = NULL;
	newt->queue = VM_NOQUEUE;
	vm_queue(newt, VM_RUNQ);
	vm_threads++;

	return 1;
}

int vm_spawn(char *fn)
{
	return vm_spawn_args(fn, 0);
}

void vm_destroy(struct vm_thread *thread)
{
	vm_unqueue(thread);
	free(thread->stackbase);
	free(thread);
	vm_threads--;
	if (vm_threads == 0) {
		printf("No threads left\n");
		exit(0);
	}
}

int vm_runnable(struct vm_thread *thread)
{
	struct timeval tv;
	if (gettimeofday(&tv, NULL))
		err(1, "gettimeofday() failed");
	//printf("Runnable? %ld, %ld <= %ld, %ld\n", thread->time.tv_sec, thread->time.tv_usec, tv.tv_sec, tv.tv_usec);
	return timercmp(&thread->time, &tv, <=);
}

void vm_sched(void)
{
	if (vm_current) {
		vm_queue(vm_current, VM_RUNQ); /* XXX insert at tail */
		vm_current = NULL;
	}

	/* Search for waiting thread to move to runq */
	if (vm_queues[VM_TIMEQ] && vm_runnable(vm_queues[VM_TIMEQ])) {
		struct vm_thread *thread;
		thread = vm_queues[VM_TIMEQ];
		vm_unqueue(thread);
		vm_queue(thread, VM_RUNQ);
	}

	/* Go for next runnable thread */

	if (vm_queues[VM_RUNQ]) {
		vm_current = vm_queues[VM_RUNQ];
		vm_unqueue(vm_current);
	} else {
		struct timeval tv;
		struct timespec ts;
		int rv;

//		printf("No runnable thread - sleeping\n");
		if (vm_queues[VM_TIMEQ]) {
		    gettimeofday(&tv, NULL);
		    timersub(&vm_queues[VM_TIMEQ]->time, &tv, &tv);
		    if ((tv.tv_sec < 0) || (tv.tv_usec < 0)) {
			    tv.tv_sec = 0;
			    tv.tv_usec = 0;
		    }
		    TIMEVAL_TO_TIMESPEC(&tv, &ts);
		} else {
		    ts.tv_sec = SLEEPTIME_SEC;
		    ts.tv_nsec = SLEEPTIME_NSEC;
		}
//		nanosleep(&ts, NULL);
		rv = pollts(vm_pollfd, vm_npollfds, &ts, NULL);
		if ((rv == -1) && (errno != EINTR))
			err(1, "pollts() error");
		if (rv > 0)
			vm_wakeupafterpoll();
	}
}

void vm_register_signal_fd(int fd, int queue)
{
	int rv;

	if (vm_npollfds >= VM_MAXPOLLFD)
		err(1, "Too many fds registered");
	vm_pollfd[vm_npollfds].fd = fd;
	vm_pollfd[vm_npollfds].events = POLLRDNORM;
	vm_pollfdqueue[vm_npollfds] = queue;
	vm_npollfds++;
	rv = fcntl(fd, F_SETFL, O_NONBLOCK | O_ASYNC);
}

void vm_unregister_signal_fd(int fd)
{
	int i;

	for (i = 0; i < vm_npollfds; i++) {
	    if (fd == vm_pollfd[i].fd) {
		memmove(&vm_pollfd[i], &vm_pollfd[i+1],
		    sizeof(struct pollfd) * (vm_npollfds-i-1));
		vm_npollfds--;
		return;
	    }
	}
}

void stack_push(struct vm_thread *thread, stkentry e)
{
	thread->sp++;
	if (thread->sp >= (thread->stackbase + thread->stacksize)) {
		printf("Stack overflow\n");
		exit(1);
		/* XXX what to do here? */
		thread->sp--;
		return;
	}
	*thread->sp = e;
}

void stack_pushstr(struct vm_thread *thread, int len, char *string)
{
	stkentry *strbase = thread->sp + 1;

	thread->sp += 1+((len+sizeof(stkentry)-1)/sizeof(stkentry));

	if (thread->sp >= (thread->stackbase + thread->stacksize)) {
		printf("Stack overflow\n");
		/* XXX what to do here? */
		thread->sp = thread->stackbase + thread->stacksize - 1;
		return;
	}
	strncpy((char *)strbase, string, len);
	*thread->sp = len;
}

char *stack_getstr(struct vm_thread *thread, int len, int count)
{
	stkentry *ptr;

	ptr = thread->sp - count -
	    ((len+sizeof(stkentry)-1)/sizeof(stkentry));
	if (ptr < thread->stackbase) {
		printf("Stack underflow\n");
		/* XXX what to do here? */
		ptr = thread->stackbase;
	}
	return (char *)ptr;
}

void stack_pop(struct vm_thread *thread, int count)
{
	thread->sp -= count;
	if (thread->sp < thread->stackbase) {
		printf("Stack underflow\n");
		/* XXX what to do here? */
		thread->sp = thread->stackbase;
	}
}

void stack_alloc(struct vm_thread *thread, int count)
{
	thread->sp += count;
	if (thread->sp >= (thread->stackbase + thread->stacksize)) {
		printf("Stack overflow\n");
		/* XXX what to do here? */
		thread->sp = thread->stackbase + thread->stacksize - 1;
	}
}

void stack_load(struct vm_thread *thread, int count)
{
	stkentry *ptr;

	ptr = thread->sp - count;
	if (ptr < thread->stackbase) {
		printf("Stack underflow\n");
		/* XXX what to do here? */
		ptr = thread->stackbase;
	}
	stack_push(thread, *ptr);
}

void stack_store(struct vm_thread *thread, int count)
{
	stkentry *ptr;

	ptr = thread->sp - count - 1;
	if (ptr < thread->stackbase) {
		printf("Stack underflow\n");
		/* XXX what to do here? */
		ptr = thread->stackbase;
	}
	*ptr = *thread->sp;
	stack_pop(thread, 1);
}

stkentry stack_get(struct vm_thread *thread, int count)
{
	stkentry *ptr;

	ptr = thread->sp - count;
	if (ptr < thread->stackbase) {
		printf("Stack underflow\n");
		/* XXX what to do here? */
		ptr = thread->stackbase;
	}
	return *ptr;
}

float stack_getreal(struct vm_thread *thread, int count)
{
	union {
		stkentry e;
		float f;
	} conv;
	conv.e = stack_get(thread, count);
	return conv.f;	/* XXX Not exactly MI */
}

void stack_poke(struct vm_thread *thread, int count, stkentry value)
{
	stkentry *ptr;

	ptr = thread->sp - count;
	if (ptr < thread->stackbase) {
		printf("Stack underflow\n");
		/* XXX what to do here? */
		ptr = thread->stackbase;
	}
	*ptr = value;
}

void stack_pokereal(struct vm_thread *thread, int count, float value)
{
	union {
		stkentry e;
		float f;
	} conv;
	conv.f = value;
	stack_poke(thread, count, conv.e);
}

#define INTOP	intop = 0;						\
		while (bytecount--)					\
			intop = (intop << 8) | ((*vm_current->pc++) & 0xff);

/*
 * RESTART can only be used to restart things that don't change the stack.
 * It is intended for use to restart internal functions if they need to sleep.
 */
#define RESTART	savedthread->pc = savedpc;

#define NOTIMPL		printf("Not implemented\n");

#define STACKTOP	(*vm_current->sp)
#define STACKTOPFL	(*(float *)(vm_current->sp))

void vm_run(void)
{
	int opcode;
	int bytecount;
	int intop;
	int val;
	float flval;
	instr *savedpc;
	struct vm_thread *savedthread;

	while (1) {
		if (vm_caughtsignal) {
			if (vm_current) {
				vm_queue(vm_current, VM_RUNQ);
				vm_current = NULL;
			}
			vm_handle_signal();
		}
		while (vm_current == NULL)
			vm_sched();
		if ((vm_current->pc < vm_codearea) ||
		    (vm_current->pc > (vm_codearea + vm_codesize))) {
			printf("Execution outside code area\n");
			vm_destroy(vm_current);
			vm_current = NULL;
			continue;
		}
		savedpc = vm_current->pc;
		savedthread = vm_current;
		opcode = *vm_current->pc++;
		bytecount = BYTECOUNT(opcode);
		opcode = OPCODE_MASK(opcode);
#if DEBUG
		printf("executing opcode %s (%d) at %p (bytecount %d)\n", instr_names[opcode], opcode, vm_current->pc-1, bytecount);
		printf("sp = %p\n", vm_current->sp);
#endif
		switch(opcode) {
		case OP_NOP:	/* No-op */
			break;
		case OP_PUSH:	/* Push immediate */
			INTOP
			stack_push(vm_current, intop);
			break;
		case OP_PUSHSTR:
			INTOP
			stack_pushstr(vm_current, intop, vm_current->pc);
			vm_current->pc += intop;
			break;
		case OP_POP:
			INTOP
			stack_pop(vm_current, intop);
			break;
		case OP_ALLOC:
			INTOP
			stack_alloc(vm_current, intop);
			break;
		case OP_LOAD:
			INTOP
			stack_load(vm_current, intop);
			break;
		case OP_STORE:
			INTOP
			stack_store(vm_current, intop);
			break;

		case OP_B:
		case OP_BL:
			INTOP
			if (opcode == OP_BL)
				stack_push(vm_current,
				    (stkentry)vm_current->pc);
			vm_current->pc += intop;
			break;
		case OP_BI:
		case OP_BIL:
			val = STACKTOP;
			if (opcode == OP_BIL)
				stack_push(vm_current,
				    (stkentry)vm_current->pc);
			vm_current->pc += val;
			stack_pop(vm_current, 1);
			break;
		case OP_BZ:
		case OP_BZL:
			INTOP
			val = STACKTOP;
			stack_pop(vm_current, 1);
			if (val == 0) {
				if (opcode == OP_BZL)
					stack_push(vm_current,
					    (stkentry)vm_current->pc);
				vm_current->pc += intop;
			}
			break;
		case OP_BNZ:
		case OP_BNZL:
			INTOP
			val = STACKTOP;
			stack_pop(vm_current, 1);
			if (val != 0) {
				if (opcode == OP_BNZL)
					stack_push(vm_current,
					    (stkentry)vm_current->pc);
				vm_current->pc += intop;
			}
			break;
		case OP_RET:
			vm_current->pc = (instr *)STACKTOP;
			stack_pop(vm_current, 1);
			break;

		case OP_CALLNUM:
			INTOP
			if ((intop >=0) && (intop < vm_intfn_size)) {
				if (!vm_intfn_table[intop]()) {
					RESTART
				}
			} else
				printf("Internal function out of range\n");
			break;
		case OP_CALLSTR:
			INTOP
			vm_current->pc += intop;
			NOTIMPL
			break;

		case OP_ADD:
			val = STACKTOP;
			stack_pop(vm_current, 1);
			STACKTOP += val;
			break;
		case OP_SUB:
			val = STACKTOP;
			stack_pop(vm_current, 1);
			STACKTOP -= val;
			break;
		case OP_MUL:
			val = STACKTOP;
			stack_pop(vm_current, 1);
			STACKTOP *= val;
			break;
		case OP_DIV:
			val = STACKTOP;
			stack_pop(vm_current, 1);
			STACKTOP /= val;
			break;
		case OP_EQ:
			val = STACKTOP;
			stack_pop(vm_current, 1);
			STACKTOP = (STACKTOP == val);
			break;
		case OP_NE:
			val = STACKTOP;
			stack_pop(vm_current, 1);
			STACKTOP = (STACKTOP != val);
			break;
		case OP_LT:
			val = STACKTOP;
			stack_pop(vm_current, 1);
			STACKTOP = (STACKTOP < val);
			break;
		case OP_GT:
			val = STACKTOP;
			stack_pop(vm_current, 1);
			STACKTOP = (STACKTOP > val);
			break;
		case OP_LE:
			val = STACKTOP;
			stack_pop(vm_current, 1);
			STACKTOP = (STACKTOP <= val);
			break;
		case OP_GE:
			val = STACKTOP;
			stack_pop(vm_current, 1);
			STACKTOP = (STACKTOP >= val);
			break;

		case FLOP_ADD:
			flval = STACKTOPFL;
			stack_pop(vm_current, 1);
			STACKTOPFL += flval;
			break;
		case FLOP_SUB:
			flval = STACKTOPFL;
			stack_pop(vm_current, 1);
			STACKTOPFL -= flval;
			break;
		case FLOP_MUL:
			flval = STACKTOPFL;
			stack_pop(vm_current, 1);
			STACKTOPFL *= flval;
			break;
		case FLOP_DIV:
			flval = STACKTOPFL;
			stack_pop(vm_current, 1);
			STACKTOPFL /= flval;
			break;
		case FLOP_EQ:
			flval = STACKTOPFL;
			stack_pop(vm_current, 1);
			STACKTOP = (STACKTOPFL == flval);
			break;
		case FLOP_NE:
			flval = STACKTOPFL;
			stack_pop(vm_current, 1);
			STACKTOP = (STACKTOPFL != flval);
			break;
		case FLOP_LT:
			flval = STACKTOPFL;
			stack_pop(vm_current, 1);
			STACKTOP = (STACKTOPFL < flval);
			break;
		case FLOP_GT:
			flval = STACKTOPFL;
			stack_pop(vm_current, 1);
			STACKTOP = (STACKTOPFL > flval);
			break;
		case FLOP_LE:
			flval = STACKTOPFL;
			stack_pop(vm_current, 1);
			STACKTOP = (STACKTOPFL <= flval);
			break;
		case FLOP_GE:
			flval = STACKTOPFL;
			stack_pop(vm_current, 1);
			STACKTOP = (STACKTOPFL >= flval);
			break;

		default:
			printf("Unrecognised opcode\n");
			break;
		}
	}
}