/*
 * ATxmega256A3B.c
 *
 * Created: 29-Jul-19 1:58:28 PM
 * Author : M19892
 */ 

#define F_CPU 32768UL

#include <avr/io.h>
#include <util/delay.h>
#include <avr/fuse.h>
#include <avr/xmega.h>
#include "eeprom.h"

FUSES =
{
	.FUSEBYTE0 = 0xFF,
	.FUSEBYTE1 = 0x00,
	.FUSEBYTE2 = 0xDF,
	.FUSEBYTE4 = 0xF2,
	.FUSEBYTE5 = 0xFF,
};


uint8_t debounce (PORT_t btn_port, uint8_t pin)
{
	uint8_t ret = 1;
	for (uint8_t i = 0; i < 100; i++)
	{
		if(btn_port.IN & pin){
			ret = 0;
			break;
		}
		_delay_ms(1);
	}
	return ret;
}

void disable_32k_osc()
{
	// Div to 62500 Hz so delays for debounce are almost static
	_PROTECTED_WRITE(CLK.PSCTRL, CLK_PSADIV_32_gc);
	_PROTECTED_WRITE(CLK.CTRL, CLK_SCLKSEL_RC2M_gc);
	OSC.CTRL &= ~OSC_XOSCEN_bm;
}

void enable_32k_osc()
{
	OSC.XOSCCTRL = (OSC.XOSCCTRL & ~OSC_XOSCSEL_gm) | OSC_XOSCSEL_32KHz_gc;
	OSC.CTRL |= OSC_XOSCEN_bm;
	// VBAT fix
	VBAT.CTRL |= VBAT_ACCEN_bm;
}

void set_xtal_mode()
{
	uint8_t mode_byte = EEPROM_ReadByte(0, 0x00);
	// Set appropriate XTAL mode
	if ((OSC.XOSCCTRL & OSC_X32KLPM_bm) && !(mode_byte & 0x01))
	{
		disable_32k_osc();
		OSC.XOSCCTRL &= ~OSC_X32KLPM_bm;
		enable_32k_osc();
	}
	if (!(OSC.XOSCCTRL & OSC_X32KLPM_bm) && (mode_byte & 0x01))
	{
		disable_32k_osc();
		OSC.XOSCCTRL |= OSC_X32KLPM_bm;
		enable_32k_osc();
	}
	if ((VBAT.CTRL & VBAT_HIGHESR_bm) && !(mode_byte & 0x02))
	{
		disable_32k_osc();
		_PROTECTED_WRITE(VBAT.CTRL, VBAT_RESET_bm);
		VBAT.CTRL = VBAT_ACCEN_bm;
		VBAT.CTRL = VBAT_ACCEN_bm | VBAT_XOSCEN_bm;
		enable_32k_osc();
	}
	if (!(VBAT.CTRL & VBAT_HIGHESR_bm) && (mode_byte & 0x02))
	{
		disable_32k_osc();
		_PROTECTED_WRITE(VBAT.CTRL, VBAT_RESET_bm);
		VBAT.CTRL = VBAT_ACCEN_bm;
		VBAT.CTRL = VBAT_HIGHESR_bm | VBAT_ACCEN_bm | VBAT_XOSCEN_bm;
		enable_32k_osc();
	}
}

void set_LED_mode()
{
	// Set LED according to setting
	if (OSC.XOSCCTRL & OSC_X32KLPM_bm)
	{
		PORTA.OUTCLR = PIN3_bm;
		}else{
		PORTA.OUTSET = PIN3_bm;
	}
		
	if (VBAT.CTRL & VBAT_HIGHESR_bm)
	{
		PORTA.OUTCLR = PIN4_bm;
		}else{
		PORTA.OUTSET = PIN4_bm;
	}
}

void poll_btn_press()
{
	if (debounce(PORTA, PIN1_bm))
	{
		uint8_t mode_byte = EEPROM_ReadByte(0, 0x00);
		mode_byte ^= 0x01;
		EEPROM_WriteByte(0, 0x00, mode_byte);
		while(!(PORTA.IN & PIN1_bm));
	}
	if (debounce(PORTA, PIN2_bm))
	{
		uint8_t mode_byte = EEPROM_ReadByte(0, 0x00);
		mode_byte ^= 0x02;
		EEPROM_WriteByte(0, 0x00, mode_byte);
		while(!(PORTA.IN & PIN2_bm));
	}
}

void port_init()
{
	// Init LEDs (init LEDs off)
	PORTA.DIRSET = PIN3_bm | PIN4_bm;
	PORTA.OUTSET = PIN3_bm | PIN4_bm;
	
	// Init btns
	PORTA.DIRCLR = PIN1_bm | PIN2_bm;
	
	// Crystal clock will be output on PORTC7
	PORTC.DIRSET = PIN7_bm;
	PORTCFG.CLKEVOUT = PORTCFG_CLKOUT_PC7_gc;
}

void select_32k_osc()
{
	// Div to 31250 Hz so delays for debounce are almost static
	// Select 32K crystal if it's ready and not already selected
	if ((!(CLK.CTRL & CLK_SCLKSEL_XOSC_gc)) && (OSC.STATUS & OSC_XOSCRDY_bm))
	{
		_PROTECTED_WRITE(CLK.PSCTRL, CLK_PSADIV_1_gc);
		_PROTECTED_WRITE(CLK.CTRL, CLK_SCLKSEL_XOSC_gc);
	}
}

int main(void)
{
	// Div to 62500 Hz so delays for debounce are almost static
	_PROTECTED_WRITE(CLK.PSCTRL, CLK_PSADIV_32_gc);
	port_init();
	enable_32k_osc();
	
    while (1)
	{
		set_xtal_mode();
		set_LED_mode();
		poll_btn_press();
		select_32k_osc();
	}
}

