#include "Charliplexing.h"
#include "kbd.h"

#include <avr/io.h>
#include <avr/interrupt.h>
#include <util/delay.h>
#include <stdbool.h>


typedef struct _point_t {
  uint8_t x, y;
} point_t;


typedef enum _dir_t {
	NORTH = 0,
	WEST,
	SOUTH,
	EAST
} dir_t;


static volatile uint8_t kbdir;
static volatile bool start;
static volatile uint16_t lfsr = 0xace1;

static uint8_t dir;
static point_t tail = {7, 4};

#define HISTORY_MAX 128

static uint8_t history[HISTORY_MAX >> 2];
static uint8_t history_len = 0;
static uint8_t history_pos = 0;

static point_t q;

static uint16_t rand(void) {
	unsigned bit;

	/* taps: 16 14 13 11; feedback polynomial: x^16 + x^14 + x^13 + x^11 + 1 */
	bit  = ((lfsr >> 0) ^ (lfsr >> 2) ^ (lfsr >> 3) ^ (lfsr >> 5)) & 1;
	lfsr =  (lfsr >> 1) | (bit << 15);

	return lfsr;
}

static void rand_mix(uint16_t mix) {
	lfsr ^= mix;
	rand();
}

static void rand_q(void) {
	q.x = rand() % 14;
	q.y = rand() % 9;
}

static inline uint8_t rev(uint8_t d) {
	return d ^ 2;
}

static inline bool points_equal(const point_t *p1, const point_t *p2) {
	return p1->x == p2->x && p1->y == p2->y;
}

static void move(uint8_t d, bool grow) {
	uint8_t p = (history_pos+history_len) % HISTORY_MAX;

	uint8_t n = p >> 2, shift = (p & 3) << 1;

	history[n] &= ~(3 << shift);
	history[n] |= d << shift;

	if (grow)
		history_len++;
	else
		history_pos = (history_pos+1) % HISTORY_MAX;
}

static uint8_t get_dir(uint8_t i) {
	uint8_t p = (history_pos+i) % HISTORY_MAX;

	uint8_t n = p >> 2, shift = (p & 3) << 1;

	return (history[n] >> shift) & 3;
}

static void reset(void) {
	LedSignClear(0);

	rand_q();

	kbdir = dir = EAST;
	tail = (point_t){7, 4};
	history_len = 0;
	history_pos = 0;

	unsigned i;
	for (i = 0; i < 2; i++)
		move(EAST, true);
}

static void set_led(uint8_t x, uint8_t y, uint8_t val) {
	LedSignSet(13-x, 8-y, val);
}

void kbd_handle(uint16_t code, bool make) {
	if (!make)
		return;

	switch(code) {
	case KBD_CODE_SPACE:
		start = true;
		break;

	case KBD_CODE_UP:
		kbdir = NORTH;
		break;

	case KBD_CODE_LEFT:
		kbdir = WEST;
		break;

	case KBD_CODE_DOWN:
		kbdir = SOUTH;
		break;

	case KBD_CODE_RIGHT:
		kbdir = EAST;
	}
}

static void go(point_t *point, uint8_t d) {
	switch (d) {
	case NORTH:
		point->y = (point->y+8)%9;
		break;
	case WEST:
		point->x = (point->x+13)%14;
		break;
	case SOUTH:
		point->y = (point->y+1)%9;
		break;
	case EAST:
		point->x = (point->x+1)%14;
	}
}

static bool point_used(const point_t *p, bool head) {
	point_t pt = tail;

	unsigned i;
	for (i = 0; i < history_len - (head ? 0 : 1); i++) {
		go(&pt, get_dir(i));

		if (points_equal(&pt, p))
			return true;
	}

	return false;
}

static bool step(void) {
	if (kbdir != rev(dir))
		dir = kbdir;

	uint8_t d = dir;

	rand_mix(d);

	set_led(tail.x, tail.y, 0);

	int i;
	point_t pt = tail;

	set_led(pt.x, pt.y, 0);

	for (i = 0; i < history_len; i++) {
		go(&pt, get_dir(i));
		set_led(pt.x, pt.y, 2);
	}

	go(&pt, d);
	set_led(pt.x, pt.y, 3);

	if (point_used(&pt, true))
		return false;

	if (points_equal(&pt, &q)) {
		rand_q();
		move(d, true);
	}
	else {
		go(&tail, get_dir(0));
		move(d, false);
	}

	if (point_used(&pt, false))
		return false;

	set_led(q.x, q.y, 7);

	return true;
}

int main() {
	LedSignInit(GRAYSCALE);
	kbd_init();

	sei();

	kbd_send(KBD_CMD_RESET);

	while(true) {
		start = false;
		while (!start) {}
		reset();

		while (step())
			_delay_ms(100);
	}

	return 0;
}