#pragma once

#include <sys/threads.h>

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

#define LOW(a) (a&0xFF)
#define HIGH(a) ((a>>8)&0xff)

#define zeromem(a) memset(&a,0x00,sizeof(a));
#define RETFAIL(a)	if (a) return -1;

#define FAIL(a) { int8_t r = (a); if (r) { return r; }; }
#define FAILDBG(a) { int8_t r = (a); if (r) { lasterror = (r&0xffff) | (((uint32_t)getPC())<<16); return r; }; }

#define IFAIL(a) { int8_t r = (a); if (!r) { return -127; }; }
#define IFAILDBG(a) { int8_t r = (a); if (!r) { lasterror = (((uint32_t)getPC())<<16); return -127; }; }

extern uint32_t __freq_cpu;
extern uint8_t* __brkval;
extern uint8_t*	__stack_limit;
extern thread_t* _thread_current;

extern uint16_t	dbgPtr;
extern uint32_t	lasterror;

#define MALLOC_CLEANUP(type,var,size)	\
	inline void __freefree(type **pptr) { free((void*)*pptr); }; \
	type* var __attribute__((__cleanup__(__freefree))) = (type*)malloc(size);

#define WAITFOR( var ) { while (!var) yield(); }

struct _smoother
{
	float sum,
		inv_n;
};
typedef struct _smoother SMOOTHER;


float smooth(SMOOTHER *s,float input);
void smoother_set(SMOOTHER *s,float sum,float n);

#define DBG_SAVE_MEMLIMITS()	eeprom_write_word( (void*)0x00, (int16_t)__brkval ); \
								eeprom_write_word( (void*)0x02, (int16_t) __stack_limit ); \
								eeprom_write_word( (void*)0x04, SP ); \
								eeprom_write_word( (void*)0x06, (int16_t)_thread_current->stack->base );

int wcslen(wchar_t* str);

int64_t ifact(int n);

static inline void i32_xch(void *ptr)
{
	uint8_t *b = ptr;
	uint8_t a;
	
	a = b[0];
	b[0] = b[3];
	b[3] = a;
	a = b[1];
	b[1] = b[2];
	b[2] = a;
};