#include <limits.h>
#include <assert.h>
#include <stdio.h>
#include "DueTimer.h"
#include "dagon.h"

bool serial_vprintf(const char *fmt, va_list va) {
	char buf[4096];

	if (vsnprintf(buf, sizeof buf, fmt, va) >= sizeof buf)
		return false;
	Serial.print(buf);
	return true;
}

bool serial_printf(const char *fmt, ...) {
	va_list va;

	va_start(va, fmt);
	bool r = serial_vprintf(fmt, va);
	va_end(va);
	return r;
}

bool print_start(struct libscomp_line *l, const char *fmt, ...) {
	va_list va;
	va_start(va, fmt);

	if (l->name) {
		Serial.print(":");
		Serial.print(l->buf[0]);
		Serial.print("\t");
	}

	bool b = serial_vprintf(fmt, va);
	va_end(va);
	return b;
}

static long pstr(const char *s, long max) {
	long v;
	char *p;

	v = strtol(s, &p, 0);
	if (*p || v < 0 || v >= max)
		return -1;
	return v;
}
#define inl __attribute__((always_inline))
                                                                          \
#define mkhandler(nam, cond) static int nam(struct libscomp_line *) inl;  \
int dagon_##nam(struct libscomp_line *l, void *p unused) {                \
	if (!([](size_t len) { return (cond); })(l->len - l->name - 1))  \
		return 1;                                                 \
	return nam(l);                                                    \
} static int nam(struct libscomp_line *l)

volatile struct dagon dg;

mkhandler(idn, len == 0) {
	print_start(l, "IDN\tDagon\t0\n");
	return LIBSCOMP_CMD_OK;
}

#define V "%" PRIu32
mkhandler(vlt, len == 0) {
	print_start(l, "VLT\t" V "\t" V "\t" V "\t" V "\t",
	            dg.ch[0].volt, dg.ch[1].volt, dg.ch[2].volt,
	            dg.ch[3].volt);
	return LIBSCOMP_CMD_OK;
}
#undef V

// GNU extension (statement expression)
#define PARSE_INT(v, m, typ) ({                                   \
	long parse__v = pstr(v, m);                                \
	if (parse__v < 0) {                                        \
		print_start(l, "ERROR\t%s invalid %s\n", v, typ); \
		return 1;                                          \
	}                                                          \
	parse__v;                                                  \
})

/* [:??] SET CH V */
mkhandler(set, len == 2) {
	long ch, v;
	char *s;
	const size_t arg1 = l->name + 1;

	ch = PARSE_INT(l->buf[arg1], DACS, "Channel");
	v = PARSE_INT(l->buf[arg1 + 1], V_MAX + 1, "Voltage");

	dg.ch[ch].volt = v;
	print_start(l, "SET\n");
	spi_set(v, ch);
	return LIBSCOMP_CMD_OK;
}

static DueTimer timers[DACS] = {Timer0,Timer1,Timer2,Timer3};

template<size_t CH>
static void irq() {
	uint32_t n = dg.ch[CH].volt + dg.ch[CH].step;

	if (n <= dg.ch[CH].volt || n >= dg.ch[CH].end) {
		n = dg.ch[CH].end;
		timers[CH].stop().detachInterrupt();
	}
	dg.ch[CH].volt = n;
	spi_set(n, CH);
}

static void (*const irqs[DACS])() = {
	irq<0>, irq<1>, irq<2>, irq<3>
};

/* [:??] RMP CH END START INTERVAL STEP [...] */

mkhandler(rmp, len % 5 == 0 && len >= 5) {
	long ch, start, end, inter, step;
	size_t arg = l->name + 1;

	while (arg < l->len) {
		ch = PARSE_INT(l->buf[arg], DACS, "Channel");
		if (dg.ch[ch].used) {
			print_start(l, "ERROR\tChannel %ld used\n", ch);
			return 1;
		}
		end = PARSE_INT(l->buf[arg + 1], V_MAX + 1, "End voltage");
		start = PARSE_INT(l->buf[arg + 2], end, "Start voltage");
		inter = PARSE_INT(l->buf[arg + 3], LONG_MAX, "Time");
		if (inter == 0) {
			print_start(l, "ERROR\tZero interval\n");
			return 1;
		}
		step = PARSE_INT(l->buf[arg + 4], V_MAX + 1, "Step voltage");
		if (step == 0) {
			print_start(l, "ERROR\tZero step\n");
			return 1;
		}
		arg += 5;
    dg.ch[ch].used = true;
    dg.ch[ch].end = end;
    dg.ch[ch].step = step;

		spi_write(start, ch);
		timers[ch].attachInterrupt(irqs[ch]).start(inter);
	}
	return LIBSCOMP_CMD_OK;
}


#define b2s(b) ((b) ? "true" : "false")
mkhandler(con, len == 0) {
	print_start(l, "CON\t%s\t%s\t%s\t%s\n",
		b2s(dg.conn[0]), b2s(dg.conn[1]),
		b2s(dg.conn[2]), b2s(dg.conn[3])
	);
	return LIBSCOMP_CMD_OK;
}

mkhandler(rst, len == 0) {
	spi_reset();
}