#include "messages.h"

void msg_register(parser_t *parser, int msgid, handler_t handler)
{
	if (msgid < MSG_MAX_ID)
		parser->handler[msgid] = handler;
}

void msg_receive(parser_t *parser, int byte)
{
	//sirq_printf("msg_receive - %02x\r\n", byte);

	// Lookup pointers
	header_t *head = (header_t*)parser->buffer;
	void     *body = (void*)(head+1);
	const int max_length = sizeof(parser->buffer)-sizeof(header_t);

	// Process uart messages
	parser->buffer[parser->index++] = byte;
	switch (parser->state) {
		case 0: // Search
			if (parser->index == sizeof(uint16_t)) {
				if (head->header == MSG_HEADER) {
					parser->state = 1;
				} else {
					parser->buffer[0] = parser->buffer[1];
					parser->index = 1;
				}
			}
			break;
		case 1: // Header
			if (parser->index == sizeof(header_t)) {
				if (head->length <= max_length &&
				    head->msgid  <= MSG_MAX_ID) {
					parser->state = 2;
				} else {
					parser->index = 0;
					parser->state = 0;
				}
			}
			break;
		case 2: // Data
			if (parser->index == (int)sizeof(header_t)+head->length) {
				handler_t handler = parser->handler[head->msgid];
				if (handler)
					handler(head, body);

				parser->index = 0;
				parser->state = 0;
			}
			break;
	}
}