#include <stdio.h>
#include <avr/io.h>
#include <util/delay.h>
#include "drivers/uart_rx_async.h"

/* Button matrix between PB3-PB4, PC0-PC3
 * PC0-PC3 are inputs, default pull-up
 * PB3-PB4 are pulled to ground (alternating) while the above are read
 */

uint8_t getButtonState() {
        uint8_t val = 0;

        // Pull upper button line (PB4) down
        PORTB |= (1 << PB3);
        PORTB &= ~(1 << PB4);
	_delay_ms(1);
	val = ((~PINC) << 4);

        // Pull lower button line (PB3) down
        PORTB &= ~(1 << PB3);
        PORTB |= (1 << PB4);
	_delay_ms(1);
	val |= (~PINC) & 0x0F;

	return val;
}

uint8_t getrelay(uint8_t n) {
	uint8_t v = 0;
	switch(n) {
		case 0: v = (PIND >> PD2); break;
		case 1: v = (PIND >> PD3); break;
		case 2: v = (PIND >> PD4); break;
		case 3: v = (PIND >> PD5); break;
		case 4: v = (PIND >> PD6); break;
		case 5: v = (PIND >> PD7); break;
		case 6: v = (PINB >> PB0); break;
		case 7: v = (PINB >> PB1); break;
	}
	return(~v & 1);
}

void setrelay(uint8_t n, uint8_t s) {
	if (s) {
		switch(n) {
                        case 0: PORTD &= ~(1 << PD2); break;
                        case 1: PORTD &= ~(1 << PD3); break;
                        case 2: PORTD &= ~(1 << PD4); break;
                        case 3: PORTD &= ~(1 << PD5); break;
                        case 4: PORTD &= ~(1 << PD6); break;
                        case 5: PORTD &= ~(1 << PD7); break;
                        case 6: PORTB &= ~(1 << PB0); break;
                        case 7: PORTB &= ~(1 << PB1); break;
		}
	} else {
		switch(n) {
                        case 0: PORTD |= (1 << PD2); break;
                        case 1: PORTD |= (1 << PD3); break;
                        case 2: PORTD |= (1 << PD4); break;
                        case 3: PORTD |= (1 << PD5); break;
                        case 4: PORTD |= (1 << PD6); break;
                        case 5: PORTD |= (1 << PD7); break;
                        case 6: PORTB |= (1 << PB0); break;
                        case 7: PORTB |= (1 << PB1); break;
		}
	}
}

char umode = 't';
void uart_process(uint8_t c) {
	if (c == 't' || c == 'e' || c == 'd') {
		umode = c;
	} else if (c >= '0' && c <= '7') {
		c = c - '0';
		if (umode == 'e') {
			setrelay(c,1);
		} else if (umode == 'd') {
			setrelay(c,0);
		} else if (umode == 't') {
			setrelay(c,1 - getrelay(c));
		}
	}
}

int main() {
	DDRC &= ~(0b1111); // This sets PC0 to PC3 to input
	PORTC |= 0b1111; // This makes said pins pull-up per default

	DDRB |= (1 << PB3) | (1 << PB4); // Keypad rows

	DDRD |= 0b11111100; // PD2 to PD7 for relay
	DDRB |= 0b00000011; // PB0 and PB1 for relay

	// Same with PORT, so relays stay off
	PORTD |= 0b11111100;
	PORTB |= 0b00000011;

	// Now handle button inputs
	uint8_t c,p,d,i;
	p = 0;

	while (1) {
		c = getButtonState();
		d = c ^ p;
		p = c;
		for (i = 0; i<8; i++) {
			if (d & c & 0x80) {
				setrelay(i, 1 - getrelay(i));
			}
			d = d << 1;
			c = c << 1;
		}
	};
}