#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#include <avr/sleep.h>
#include <avr/interrupt.h>
#include <avr/wdt.h>


#include "data_manager.h"
#include "display.h"
#include "timer.h"
#include "barometer.h"

#ifdef WDT_DISABLE
# warning "WDT disabled in this build"
# define WDT_SETUP_MAYBE(x)
# define WDT_PAT_MAYBE()
#else
# define WDT_SETUP_MAYBE(x) do{wdt_enable(x);}while(0)
# define WDT_PAT_MAYBE()    do{wdt_reset();}while(0)
#endif

#define DEBUG_ALL_VAL ((1 << 1) | (1 << 2) | (1 << 3) | (1 << 4) | (1 << 5) | (1 << 6))
#define DEBUG_INIT         do{DDRB |= DEBUG_ALL_VAL;}while(0)
#define DEBUG_MARK_SET_ALL do{PORTB |= DEBUG_ALL_VAL;}while(0)
#define DEBUG_MARK_1_SET   do{PORTB |= 1 << 1;}while(0)
#define DEBUG_MARK_1_CLEAR do{PORTB &= ~(1 << 1);}while(0)
#define DEBUG_MARK_2_SET   do{PORTB |= 1 << 3;}while(0)
#define DEBUG_MARK_2_CLEAR do{PORTB &= ~(1 << 3);}while(0)
#define DEBUG_MARK_3_SET   do{PORTB |= 1 << 2;}while(0)
#define DEBUG_MARK_3_CLEAR do{PORTB &= ~(1 << 2);}while(0)
#define DEBUG_MARK_4_SET   do{PORTB |= 1 << 6;}while(0)
#define DEBUG_MARK_4_CLEAR do{PORTB &= ~(1 << 6);}while(0)
#define DEBUG_MARK_5_SET   do{PORTB |= 1 << 5;}while(0)
#define DEBUG_MARK_5_CLEAR do{PORTB &= ~(1 << 5);}while(0)

static struct data_ctx dctx = {};

/* ISR for collecting and displaying pressure, altitude data etc */
ISR(TIMER1_COMPA_vect)
{
	data_manager_tick(&dctx);
	//ui_tick();

	WDT_PAT_MAYBE();
}

int main(void)
{
	struct display display;
	struct barometer barometer;
	struct timer timer;

	/* Initialise display before enabling interrupts */
	get_system_display(&display);
	display.init();
	display.clear();

	/* get descriptors for system default peripherals */
	get_system_barometer(&barometer);
	get_system_timer(&timer);

	timer.init();
	barometer.init();

	data_manager_init(&dctx, &timer, &barometer);

	DEBUG_INIT;

#ifdef USBCON
	/* Disable USB controller if one is present - this spams (latches?) USB_GEN
	 * interrupt which we're not handling for now */
	USBCON &= ~(1 << USBE);
#endif
	// FIXME measure current saving here if any
	//ACSR |= (1 << ACD);

	/* main body should take no longer than 1 second to run */
	WDT_SETUP_MAYBE(WDTO_2S);

	sei();
	while(1) {
		set_sleep_mode(SLEEP_MODE_IDLE);
		sleep_mode();
	}
}