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

#include "fpm.h"

#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

#define SERVO_PIN          PB1
#define SERVO_DDR          DDRB

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

#define VCC_MIN            4900

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

/* Measure vcc by measuring known internal 1.1v bandgap
 * reference voltage against AVCC.
 */
uint16_t getvcc(void)
{
	uint16_t vcc;

	ADMUX |= (1 << REFS0);
	ADMUX |= (1 << MUX3) | (1 << MUX2) | (1 << MUX1);
	ADCSRA |= (1 << ADEN) | (1 << ADPS2) | (1 << ADPS0);

	// https://www.sciencetronics.com/greenphotons/?p=1521
	_delay_us(500);

	ADCSRA |= (1 << ADSC);
	while (ADCSRA & (1 << ADSC))
		; 
	vcc = (1100UL * 1023 / ADC);

	ADCSRA &= ~(1 << ADEN);
	return vcc;
}

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

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

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

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

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

	/* init 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));

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

	SERVO_DDR |= (1 << SERVO_PIN);

	fpm_init();
	sei();

	for (;;) {
	}

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

	if (fpm_match()) {
		unlock();
		fpm_led(BREATHE, BLUE, 1);
	} else {
		fpm_led(BREATHE, RED, 1);
	}

	sei();
}

ISR(BTN_INT_VEC)
{
	uint16_t id;

	cli();

	if (is_pressed(FRONT_LOCK_PIN)) {
		lock();
		fpm_led(FLASH, RED, 1);
	} else if (is_pressed(BACK_LOCK_PIN)) {
		lock();
		fpm_led(FLASH, RED, 1);
	} else if (is_pressed(BACK_UNLOCK_PIN)) {
		unlock();
		fpm_led(FLASH, BLUE, 1);
	} else if (is_pressed(ENROLL_PIN)) {
		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);
		} else
			fpm_led(BREATHE, RED, 1);
	}

	sei();
}