#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 SERVO_PIN          PB1
#define SERVO_DDR          DDRB

#define PWM_MIN            500
#define PWM_MID            1500
#define PWM_MAX            2500
#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 volatile int is_lock = 0;
static volatile int is_unlock = 0;
static volatile int is_sig_led = 0;

int main(void)
{
	/* disable WDT */
	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(80);
	}

	/* PD2: FPM unlock, PD3: outside lock, PD4: FPM enroll */
	DDRD &= ~((1 << PD2) | (1 << PD3) | (1 << PD4));
	PORTD |= (1 << PD2) | (1 << PD3) | (1 << PD4);

	/* internal lock btn */
	DDRB &= ~(1 << PB2);
	PORTB |= (1 << PB2);

	/* internal unlock btn */
	DDRC &= ~(1 << PC5);
	PORTC |= (1 << PC5);
	
	/* INT0 and INT1 from PD2 and PD3 */
	EICRA = 0b00000000;
	EIMSK = (1 << INT0) | (1 << INT1);

	PCICR |= (1 << PCIE0) | (1 << PCIE1) | (1 << PCIE2);

	PCMSK0 |= (1 << PCINT2);   /* internal lock int */
	PCMSK1 |= (1 << PCINT13);  /* internal unlock int */
	PCMSK2 |= (1 << PCINT20);  /* FPM enroll int */

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

	SERVO_DDR |= (1 << SERVO_PIN);

	fpm_init();

	sei();

	for (;;) {
		if (getvcc() < VCC_MIN)
			LED_PORT |= (1 << LED_PIN);

		sleep_bod_disable();
		set_sleep_mode(SLEEP_MODE_PWR_DOWN);
		sleep_mode();
	}

	return 0;
}

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

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

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

ISR(INT0_vect)
{
	cli();

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

	sei();
}

ISR(INT1_vect)
{
	cli();

	if (is_pressed(PIND, PD3)) {
		lock();
		fpm_led(FLASH, RED, 1);
	}

	sei();
}

ISR(PCINT0_vect)
{
	cli();

	if (is_pressed(PINB, PB2))
		lock();

	sei();
}

ISR(PCINT1_vect)
{
	cli();

	if (is_pressed(PINC, PC5))
		unlock();

	sei();
}

ISR(PCINT2_vect)
{
	uint16_t id;

	cli();

	if (is_pressed(PIND, PD4)) {
		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();
}