#include "ss/ms.h"
#include <map>
#include "C12832.h"

// LCD
C12832 lcd(D11, D13, D12, D7, D10);
// Joystick: up, down, left, right, press
DigitalIn joystick[] = {A2, A3, A4, A5, D4};

namespace ms {

#define CHECK_INT(v) __extension__({ \
	if ((v)->type != Value::INTEGER) \
		return unshield().new_error("expected integer"); \
	reinterpret_cast<intptr_t>(v->p1); \
})

#define CHECK_PAIR(v) __extension__({ \
	if ((v)->type != Value::PAIR) \
		return unshield().new_error("expected pair"); \
	v; \
})

#define CHECK_STR(v) __extension__({ \
	if ((v)->type != Value::STRING) \
		return unshield().new_error("expected string"); \
	*static_cast<const std::string *>(v->p1); \
})


static Value *mbed_wait_ms(int argc, Value **argv) {
	wait(CHECK_INT(argv[0]) / 1000.0);
	return unshield().new_nil();
}

static Value *c12832_cls(int argc, Value **argv) {
	lcd.cls();
	return unshield().new_nil();
}

static Value *c12832_pixel(int argc, Value **argv) {
	intptr_t a = CHECK_INT(argv[0]),
		 b = CHECK_INT(argv[1]),
		 c = CHECK_INT(argv[2]);
	lcd.pixel((int)a, (int)b, (int)c);
	return unshield().new_nil();
}

static Value *c12832_print(int argc, Value **argv) {
	const Value *pair = CHECK_PAIR(argv[0]);
	intptr_t x = CHECK_INT(car(pair)), y = CHECK_INT(cdr(pair));
	const std::string &str = CHECK_STR(argv[1]);

	lcd.locate(x, y);
	lcd.printf(str.c_str());
	return unshield().new_nil();
}

static Value *c12832_fillcircle(int argc, Value **argv) {
	intptr_t x = CHECK_INT(argv[0]),
		 y = CHECK_INT(argv[1]),
		 r = CHECK_INT(argv[2]),
		 c = CHECK_INT(argv[3]);
	lcd.fillcircle((int)x, (int)y, (int)r, (int)c);
	return unshield().new_nil();
}

static Value *joystick_active_p(int argc, Value **argv) {
	intptr_t n = CHECK_INT(argv[0]);
	if (n < 0 || n > (intptr_t)(sizeof(joystick)/sizeof(joystick[0])))
		return unshield().new_error("invalid joystick number");
	return unshield().new_bool(joystick[n]);
}

void builtin_add_mbed(std::map<const std::string, Value *> &map) {
	static Memory::Shield shield(get_memory()); // FIXME

	map["mbed-wait-ms"] = shield.new_cfunc(mbed_wait_ms, 1);

	map["c12832-cls"] = shield.new_cfunc(c12832_cls, 0);
	map["c12832-pixel"] = shield.new_cfunc(c12832_pixel, 3);
	map["c12832-print"] = shield.new_cfunc(c12832_print, 2);
	map["c12832-fillcircle"] = shield.new_cfunc(c12832_fillcircle, 4);

	map["joystick-active?"] = shield.new_cfunc(joystick_active_p, 1);
}

}