#include <inttypes.h>
#include <stdbool.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include <util/delay.h>
#include <can.h>
#include <messages.h>
#include "display.h"
#include "button.h"

static bool report_change = false;

static void init();
static void send_status();
static void loop();

/* I/O map version 1 (non-working):
	PB2 BUTTON0
	PB3 BUTTON1
	PB4 BUTTON2
	PB5 BUTTON3
	PB6 BUTTON4
	PC0 BUTTON5
	PC1 BUTTON6
	PC2 CANTX
	PC3 CANRX
	PC4 BUTTON7
	PC5 VFDRS
	PC6 VFDRW
	PC7 VFDE
	PD0 VFDDB0
	PD1 VFDDB1
	PD2 VFDDB2
	PD3 VFDDB3
	PD4 VFDDB4
	PD5 VFDDB5
	PD6 VFDDB6
	PD7 VFDDB7
*/

/* I/O map version 1 (patched):
	PB0 VFDDB2
	PB1 VFDDB3
	PB2 BUTTON0
	PB3 BUTTON1
	PB4 BUTTON2
	PB5 BUTTON3
	PB6 BUTTON4
	PB7 VFDDB4
	PC0 BUTTON5
	PC1 BUTTON6
	PC2 CANTX
	PC3 CANRX
	PC4 BUTTON7
	PC5 VFDRS
	PC6 VFDRW
	PC7 VFDE
	PD0 VFDDB0
	PD1 VFDDB1
	PD2 -
	PD3 -
	PD4 -
	PD5 VFDDB5
	PD6 VFDDB6
	PD7 VFDDB7
*/

/* I/O map version 2:
	PB0 VFDDB0
	PB1 VFDDB1
	PB2 VFDDB2
	PB3 VFDDB3
	PB4 VFDDB4
	PB5 VFDDB5
	PB6 VFDDB6
	PB7 VFDDB7
	PC0 BUTTON5
	PC1 BUTTON7
	PC2 CANTX
	PC3 CANRX
	PC4 BUTTON6
	PC5 VFDRS
	PC6 VFDRW
	PC7 VFDE
	PD0 BUTTON0
	PD1 BUTTON1
	PD2 -
	PD3 -
	PD4 -
	PD5 BUTTON2
	PD6 BUTTON3
	PD7 BUTTON4
*/

static void init() {
	// initialize buttons
	button_init();

	// initialize VFD
	display_init();

	// Initialize CAN interface
	can_init(BITRATE_125_KBPS);
	// Regular receive/transmit mode
	can_set_mode(NORMAL_MODE);

	// Filter for messages addressed to us (RTR on)
	can_filter_t us = {
		.id = CAN_HOSTID_DISPLAY,
		.mask = CAN_HOSTID_MASK,
		.flags = {
			.rtr = 1,
			.extended = 0,
		},
	};
	can_set_filter(0, &us);
	// Filter for public messages (RTR off)
	can_filter_t pub = {
		.id = CAN_HOSTID_BROADCAST,
		.mask = CAN_HOSTID_MASK,
		.flags = {
			.rtr = 0,
			.extended = 0,
		},
	};
	can_set_filter(1, &pub);

	// Enable interrupts to start reception
	sei();
}

static void send_status() {
	uint8_t status = button_get();

	if (can_check_free_buffer()) {
		can_t msg = {
			.id = CAN_MSG_DISPLAY_STATUS,
			.flags = {
				.rtr = 0,
			},
			.length = 1,
			.data = { (uint8_t) status, },
		};
		can_send_message(&msg);
	}
}

static void loop() {
	// Check for new messages
	if (can_check_message()) {
		can_t msg = { 0 };
		uint8_t status = can_get_message(&msg);
		if (status != 0) {
			switch (msg.id) {
			case CAN_MSG_DISPLAY_STATUS:
				send_status();
				break;
			case CAN_MSG_DISPLAY_CMD:
				if (msg.length >= 1) {
					display_write(msg.data[0], &msg.data[1], msg.length - 1);
				}
				break;
			case CAN_MSG_AUTO_STATUS:
				if (msg.length == 1) {
					report_change = msg.data[0] ? true : false;
				}
				break;
			}
		}
	}
}

static void test() {
	// clear
	display_write(0x01, 0, 0);
	// display on, no cursor
	display_write(0x0c, 0, 0);
	display_write(0x80, (const uint8_t *) "Hello, world", 12);
	_delay_ms(1000);
}

int main() {
	init();
	while (1) {
		//loop();
		test();
	}
}