/**
 * Serial communication module.
 */

#include <stdbool.h>
#include "dataTypes.h"

serial_state_t serial_state = SERIAL_UNKNOWN;

static void handle_recv(int r);

static void handle_ctrl(int r) {
	// detect reset procedure
	switch (r) {
	case RESET1:
		// reset request received, ack and wait for RESET2
		Serial.write(ACK1);
		serial_state = SERIAL_INIT;
		return;
	case RESET2:
		if (serial_state == SERIAL_INIT) {
			// Handshake completed!
			Serial.write(ACK2);
			serial_state = SERIAL_READY;
		} else {
			// invalid state, reject it
			Serial.write(OUTOFSYNC);
			serial_state = SERIAL_UNKNOWN;
		}
		return;
	case PONG:
		// TODO mark as alive if PING was previously sent
		return;
	}
	// TODO commands for navigation? Use DATA_LEN, etc?
}

void handle_serial(data_t *data, int changedBits) {
	// first attempt sync
	while (Serial.available()) {
		r = Serial.read();

		if ((r & 0xC0) == 0x80) {
			// handle control packets
			handle_ctrl(r);
		// TODO if DATA_ESCAPE or DATA_LEN1, react appropriately
		} else if (serial_state == SERIAL_READY) {
			// handle normal data only when the handshake has completed.
			handle_recv(r);
		}
	}

	if (serial_state != SERIAL_READY) {
		// Hardware is not ready, do not send data.
		return;
	}

	// then send queued data
}

// writes data of length len
static void serial_write_data(const char *data, unsigned len) {
	len--; // length on wire is one smaller (00 means 1 byte, 01 means 2, etc.)
	if (len >= 0 && len < 56) {
		Serial.write(0x80 + len + 7);
	} else if (len >= 0 && len < 256) {
		Serial.write(0x88 | (len >> 5));
		Serial.write(0xA0 | (len & 0x1F));
	} else {
		// zero length or too long.
		return;
	}
	// write remainder
	for (unsigned i = 0; i < len; i++) {
		char c = data[i];
		if (c & 0x80) {
			Serial.write(DATA_ESCAPE);
			Serial.write(c & ~0x80);
		} else {
			Serial.write(c);
		}
	}
}

void serial_print_debug(const char *str) {
	serial_print_debug(str, strlen(str));
}

static void handle_recv(int r) {
}