#include <inttypes.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 uint16_t saved_status;
static uint16_t status_hysteresis[2];

static void init() {
	// Set PB as input, pull-up off
	DDRB = 0;
	// Set PC0/4/6 as input, pull-up off
	DDRC &= ~(_BV(DDC0) | _BV(DDC4) | _BV(DDC6));
	
	// 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_FEEDBACK,
		.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 uint16_t get_status() {
	// Get GPIO state
	// PB -> invert -> Dend..Hend, Dempty..Fempty
	uint8_t statusl = ~PINB;
	// PC0,6,4 -> invert -> Gempty, Hempty, Reset
	uint8_t pinc = ~PINC;
	uint8_t gempty = (pinc & _BV(0)) ? _BV(0) : 0;
	uint8_t hempty = (pinc & _BV(6)) ? _BV(1) : 0;
	uint8_t swres = (pinc & _BV(4)) ? _BV(2) : 0;
	uint8_t statush = gempty | hempty | swres;

	return ((uint16_t) statush << 8) | ((uint16_t) statusl);
}

static uint16_t graded_status() {
	uint16_t status = get_status();

	// combine the bits of the last 3 states into one: "at least 2 of 3"
	uint16_t graded01 = status_hysteresis[0] & status_hysteresis[1];
	uint16_t graded1n = status_hysteresis[1] & status;
	uint16_t gradedn0 = status & status_hysteresis[0];
	uint16_t graded = graded01 | graded1n | gradedn0;

	status_hysteresis[0] = status_hysteresis[1];
	status_hysteresis[1] = status;

	return graded;
}

static void send_status(uint16_t status) {
	if (can_check_free_buffer()) {
		can_t msg = {
			.id = CAN_MSG_FEEDBACK_STATUS,
			.flags = {
				.rtr = 0,
			},
			.length = 2,
			.data = { (uint8_t) (status >> 8), status & 0xff, },
		};
		can_send_message(&msg);
	}
}

static void loop() {
	// Check for status changes
	uint16_t new_status = graded_status();
	if (new_status != saved_status) {
		if (report_change) {
			send_status(new_status);
		}
		saved_status = new_status;
	}

	// 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_FEEDBACK_STATUS:
				send_status(new_status);
				break;
			case CAN_MSG_AUTO_STATUS:
				if (msg.length == 1) {
					report_change = msg.data[0] ? true : false;
				}
				break;
			}
		}
	}

	_delay_us(100);
}

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