#include <hwo/logcsr.h>
#include <avr/wdt.h>
#include <hwo/utils.h>

#include <avr/io.h>
#include <avr/interrupt.h>


#include <stdint.h>
#include <stdlib.h>

#ifndef MCUCSR
	#define MCUCSR MCUSR
#endif

extern struct avr_eeprom_desc __ee_desc;
uint8_t _mcucsr;


void log_csr(void)
{
	LogCSR *log = malloc(sizeof(LogCSR));

	wdt_reset();

	if (log)
	{
		eeprom_load(__ee_desc.num_banks-1,&log->eeprom);
		if (log->wp >= 0x10)
			log->wp = 0x00;
		_mcucsr = log->mcucsr[log->wp] = MCUCSR;
		if (!MCUCSR)
			_mcucsr = log->mcucsr[log->wp] |= 0x80;
		log->wp++;

		MCUCSR = 0x00;

		wdt_reset();
		eeprom_save(__ee_desc.num_banks-1,&log->eeprom);

		free(log);

		if (_mcucsr & 0x80)
		{
			ATOMIC
			wdt_enable(WDTO_250MS);
			while (1);
		};

	};

};

#if defined(DEBUG_BADIRQ)

void __bad_irq(uint16_t ret){
	LogCSR *log = malloc(sizeof(LogCSR));

	wdt_reset();

	eeprom_load(__ee_desc.num_banks-1,&log->eeprom);
	if (log->wp >= 0x10)
		log->wp = 0x00;

	_mcucsr = log->mcucsr[log->wp] = 0xAA;
	log->wp++;
	_mcucsr = log->mcucsr[log->wp] = 0x55;
	log->wp++;
	_mcucsr = log->mcucsr[log->wp] = SREG;
	log->wp++;
	_mcucsr = log->mcucsr[log->wp] = ret & 0xFF;
	log->wp++;
	_mcucsr = log->mcucsr[log->wp] = (ret >> 8) & 0xff;
	log->wp++;
	_mcucsr = log->mcucsr[log->wp] = 0xAA;
	log->wp++;

	wdt_reset();

	eeprom_save(__ee_desc.num_banks-1,&log->eeprom);

	free(log);

	{
		ATOMIC
		wdt_enable(WDTO_250MS);
		while (1);
	};
}

ISR(BADISR_vect)
{
	intptr_t	sp = SP;
	uint16_t	r;

	sp += 4;
	r = ((*(char*)sp)<<8)|((*(char*)sp+1));
	r <<= 1;

	__bad_irq(r);
};

#endif