#include <avr/io.h>

#include <stdbool.h>

// The UART is on PD2 (Rx) and PD3 (Tx)
// The gamepads are all connected on port B
#define GRIP_PORT PORTD
#define GRIP_DDR  DDRD
#define GRIP_PIN  PIND
#define GRIP_CLK_A  (1 << 0)
#define GRIP_DAT_A  (1 << 1)
#define GRIP_CLK_B  (1 << 2)
#define GRIP_DAT_B  (1 << 3)
#define GRIP_CLK_C  (1 << 4)
#define GRIP_DAT_C  (1 << 5)
#define GRIP_CLK_D  (1 << 6)
#define GRIP_DAT_D  (1 << 7)

class GrIP
{
public:
	GrIP()
	{
		word = 0;
		count = 0;
		state = 0;
	}

	// Push the next bit and returns true if it completes a word.
	// The whole 24 bit word is stored in "word" and consists of:
	// - One 7-bit sync pattern: always 0111111
	// - Three 6-bit blocks starting with a 0 followed by 5 button bits
	bool PushBit(uint32_t bit)
	{
		word = (word >> 1) | ((bit & 1) << 31);
		count++;

		switch (state) {
			case 0:
				if ((word & 0xFE000000) != 0x7C000000) return false;
				count = 7;
				state = 1;
				break;
			case 1:
				if (count == 24) {
					state = 0;
					count = 0;
					return true;
				}
				break;
		}

		return false;
	}

public:
	uint32_t word;
private:
	uint8_t count;
	uint8_t state; // 0 = looking for sync, 1 = synchronized (we received 0111111)
};


extern "C" void GrIP_Init() {
	// GrIP input
	GRIP_PORT = 0xFF; // Enable pull-ups
	GRIP_DDR = 0;  // Port as input
}

extern "C" uint8_t GrIP_Read(uint32_t* v, uint8_t k2) {
	// Initial value of oldv for edge detection
	uint8_t oldv = GRIP_PIN;
	static uint8_t k = 0;
	if (k2 != 0)
		k = k2 - 1;
	else {
		k++; if (k > 3) k = 0;
	}

		// Because the CPU is too slow, if we try to scan all pads at once we
		// don't manage to decode them (missed transitions). So we scan them
		// one at a time.
		// To scan all 4 gamepads this will take about 4 milliseconds, which
		// is not the best we could do, but is okay. When we have the real
		// hardware (with the CPU running at full 16MHz speed instead of crappy
		// 1MHz here), we can try to interleave them again (one just has to put
		// them all into a single for loop instead of separate loops for each).
		// Then we get to 1ms to scan all 4 pads in parallel, cool! And it
		// also fixes the problem mentionned below that when less pads are
		// connected, it will not slowdown waiting for one of them to timeout.
		//
		// When a gamepad is plugged, the loop seems to complete in around 1000 iterations.
		// When no gamepad is plugged, that's a timeout.

		// Process pad1
		GrIP pad;
		uint16_t i;

		if (k == 0)
		for (i = 0; i < 2048; i++) {
			uint8_t newv = GRIP_PIN;
			if (!(newv & GRIP_CLK_A)) { // clock is down
				if (oldv & GRIP_CLK_A) { // and it was up at previous read
					// Read bit
					uint32_t bit = (newv & GRIP_DAT_A) != 0;
					if (pad.PushBit(bit)) {
						*v = pad.word;
						break;
					}
				}
			}
			oldv = newv;
		}

		// Process pad 2
		if (k == 1)
		for (i = 0; i < 2048; i++) {
			uint8_t newv = GRIP_PIN;
			if (!(newv & GRIP_CLK_B)) { // clock is down
				if (oldv & GRIP_CLK_B) { // and it was up at previous read
					// Read bit
					uint32_t bit = (newv & GRIP_DAT_B) != 0;
					if (pad.PushBit(bit)) {
						*v = pad.word;
						break;
					}
				}
			}
			oldv = newv;
		}

		// Process pad 3
		if (k == 2)
		for (i = 0; i < 2048; i++) {
			uint8_t newv = GRIP_PIN;
			if (!(newv & GRIP_CLK_C)) { // clock is down
				if (oldv & GRIP_CLK_C) { // and it was up at previous read
					// Read bit
					uint32_t bit = (newv & GRIP_DAT_C) != 0;
					if (pad.PushBit(bit)) {
						*v = pad.word;
						break;
					}
				}
			}
			oldv = newv;
		}

		// Process pad 4
		if (k == 3)
		for (i = 0; i < 2048; i++) {
			uint8_t newv = GRIP_PIN;
			if (!(newv & GRIP_CLK_D)) { // clock is down
				if (oldv & GRIP_CLK_D) { // and it was up at previous read
					// Read bit
					uint32_t bit = (newv & GRIP_DAT_D) != 0;
					if (pad.PushBit(bit)) {
						*v = pad.word;
						break;
					}
				}
			}
			oldv = newv;
		}

		return k;
}