#include <stdint.h>
#include <avr/wdt.h>
#include <avr/sleep.h>
#include <avr/interrupt.h>
#include <util/delay.h>

#include "fpm.h"
#include "uart.h"

#define SERVO_PIN          PB1
#define SERVO_DDR          DDRB

#define PWM_MIN            500
#define PWM_MID            1600
#define PWM_MAX            2550
#define PWM_TOP            19999

#define LED_PIN            PB5
#define LED_DDR            DDRB
#define LED_PORT           PORTB

#define PWR_SERVO          PC4
#define PWR_FPM            PC5
#define PWR_DDR            DDRC
#define PWR_PORT           PORTC

#define FRONT_UNLOCK_PIN   PD2
#define FRONT_LOCK_PIN     PD3
#define ENROLL_PIN         PD4
#define BACK_LOCK_PIN      PD5
#define BACK_UNLOCK_PIN    PD6

#define INPUT_DDR          DDRD
#define INPUT_PORT         PORTD

#define FPM_UNLOCK_INT     INT0
#define FPM_INT_VEC        INT0_vect

#define FRONT_LOCK_INT     PCINT19
#define ENROLL_INT         PCINT20
#define BACK_LOCK_INT      PCINT21
#define BACK_UNLOCK_INT    PCINT22
#define BTN_INT_VEC        PCINT2_vect

enum CTRL { 
	NONE = 0, 
	LOCK_FRONT = 1, 
	LOCK_BACK = 2, 
	UNLOCK_FRONT = 3, 
	UNLOCK_BACK = 4, 
	ENROLL = 5 
}; 

static volatile enum CTRL cmd = NONE;

static inline void lock(void)
{
	OCR1A = PWM_MID;
	_delay_ms(500);
}

static inline void unlock(void)
{
	OCR1A = PWM_MAX;
	_delay_ms(500);
}

static inline void flash_led(void)
{
	for (int i = 0; i < 4; i++) {
		LED_PORT |= (1 << LED_PIN);
		_delay_ms(70);
		LED_PORT &= ~(1 << LED_PIN);
		_delay_ms(70);
	}
}

static inline void pwr_fpm_servo_on(void)
{
	PWR_PORT |= (1 << PWR_FPM) | (1 << PWR_SERVO);
}

static inline void pwr_fpm_servo_off(void)
{
	PWR_PORT &= ~((1 << PWR_FPM) | (1 << PWR_SERVO));
}

int main(void)
{
	uint16_t id;

	/* disable watchdog timer */
	cli();
	wdt_reset();
	MCUSR &= ~(1 << WDRF);
	WDTCSR |= (1 << WDCE) | (1 << WDE);
	WDTCSR = 0x00;

	uart_init();

	/* power on the FPM and the servo */
	PWR_DDR |= (1 << PWR_FPM) | (1 << PWR_SERVO);
	pwr_fpm_servo_on();

	/* servo */
	TCCR1A |= (1 << WGM11);
	TCCR1B |= (1 << WGM12) | (1 << WGM13);
	TCCR1B |= (1 << CS11);
	ICR1 = PWM_TOP;
	TCCR1A |= (1 << COM1A1);
	SERVO_DDR |= (1 << SERVO_PIN);

	/* battery check */
	LED_DDR |= (1 << LED_PIN);
	LED_PORT &= ~(1 << LED_PIN);

	/* input ports */
	INPUT_DDR &= ~((1 << BACK_LOCK_PIN) | (1 << BACK_UNLOCK_PIN) | 
	               (1 << FRONT_LOCK_PIN) | (1 << FRONT_UNLOCK_PIN) | 
	               (1 << ENROLL_PIN));

	INPUT_PORT |= ((1 << BACK_LOCK_PIN) | (1 << BACK_UNLOCK_PIN) | 
	               (1 << FRONT_LOCK_PIN) | (1 << FRONT_UNLOCK_PIN) | 
	               (1 << ENROLL_PIN));

	EICRA = 0b00000000;
	EIMSK = (1 << FPM_UNLOCK_INT);

	PCICR |= (1 << PCIE2);
	PCMSK2 |= ((1 << FRONT_LOCK_INT) | (1 << ENROLL_INT) | 
	           (1 << BACK_LOCK_INT) | (1 << BACK_UNLOCK_INT));

	flash_led();

	for (;;) {
		switch(cmd) {
		case LOCK_FRONT:
			lock();
			fpm_led(FLASH, RED, 1);
			break;
		case LOCK_BACK:
			lock();
			break;
		case UNLOCK_FRONT:
			if (fpm_match()) {
				fpm_led(BREATHE, BLUE, 1);
				unlock();
			} else {
				fpm_led(BREATHE, RED, 1);
			}
			break;
		case UNLOCK_BACK:
			unlock();
			fpm_led(FLASH, BLUE, 1);
			break;
		case ENROLL:
			id = fpm_match();
			if (id == 1 || id == 2) {
				fpm_led(BREATHE, BLUE, 1);
				_delay_ms(1000);
				if (fpm_enroll())
					fpm_led(BREATHE, BLUE, 1);
				else
					fpm_led(BREATHE, RED, 1);
			}
			break;
		default:
			break;
		}

		cmd = NONE;
		_delay_ms(500);

		pwr_fpm_servo_off();
		set_sleep_mode(SLEEP_MODE_PWR_DOWN);
		sleep_enable();
		sleep_bod_disable();
		sei();
		sleep_cpu();
		cli();
		sleep_disable();
		pwr_fpm_servo_on();
	}

	return 0;
}

static inline int is_pressed(uint8_t btn)
{
	if (!((PIND >> btn) & 0x01)) {
		_delay_ms(50);
		return !((PIND >> btn) & 0x01);
	}
	return 0;
}

ISR(FPM_INT_VEC)
{
	cmd = UNLOCK_FRONT;
}

ISR(BTN_INT_VEC)
{
	cli();

	if (is_pressed(FRONT_LOCK_PIN))
		cmd = LOCK_FRONT;
	else if (is_pressed(BACK_LOCK_PIN))
		cmd = LOCK_BACK;
	else if (is_pressed(BACK_UNLOCK_PIN))
		cmd = UNLOCK_BACK;
	else if (is_pressed(ENROLL_PIN))
		cmd = ENROLL;

	sei();
}