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

static bool report_change = false;

static void init() {
	// Enable GPIO: PB2, PB3, PB4 output, low; PB5 input, pull-up on
	PORTB = (PORTB & ~(_BV(PB2) | _BV(PB3) | _BV(PB4))) | _BV(PB5);
	DDRB = (DDRB & ~_BV(PB5)) | (_BV(PB2) | _BV(PB3) | _BV(PB4));

	// 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 = 0;

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

static void write_display(uint8_t command, const uint8_t *data, uint8_t length) {
	
}

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) {
					write_display(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;
			}
		}
	}
}

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