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

#include <sys/errno.h>
#include <sys/spi.h>
#include <sys/mutex.h>
#include <sys/cpu.h>
#include <sys/systick.h>

struct __spi_state
{
	uint8_t		ptr,
				len;
	uint8_t		*tx,
				*rx;

	void(*selector)(uint8_t slave);

	MUTEX		mutex;

} _spi_state = { };

int spi_set_select(void(*select)(uint8_t chip)) {
	_spi_state.selector = select;
	return 0;
};

int spi_configure(uint32_t	clk, uint32_t flags)
{
	MUTEXED(&_spi_state.mutex);

	uint8_t	cfg = _BV(SPIE) | _BV(SPE);

	if (flags & SPI_MASTER)
		cfg |= _BV(MSTR);
	if (flags & SPI_LSBFIRST)
		cfg |= _BV(DORD);
	if (flags & SPI_IDLE_CLKHIGH)
		cfg |= _BV(CPOL);
	if (flags & SPI_PHASE_TRAILING)
		cfg |= _BV(CPHA);

	if (clk <= (__freq_cpu / 128)) {
		cfg |= 3;
	} else if (clk <= (__freq_cpu / 64)) {
		cfg |= 2;
	} else if (clk <= (__freq_cpu / 32)) {
		cfg |= 2;
		SPSR |= _BV(SPI2X);
	} else if (clk <= (__freq_cpu / 16)) {
		cfg |= 1;
	} else if (clk <= (__freq_cpu / 8)) {
		cfg |= 1;
		SPSR |= _BV(SPI2X);
	} else if (clk <= (__freq_cpu / 4)) {
		cfg |= 0;
	} else {
		cfg |= 0;
		SPSR |= _BV(SPI2X);
	};

	SPCR = cfg;

	return ESUCCESS;
};

int spi_deconfigure(void)
{
	MUTEXED(&_spi_state.mutex);

	SPCR &= ~_BV(SPE);
	return ESUCCESS;
};

int	spi_transmit_ex(uint8_t chip,void *tx,void *rx,uint8_t len) {
	int8_t r = -EFAIL;

	MUTEXED(&_spi_state.mutex);

	_spi_state.selector(chip);
	r = spi_transmit(tx,rx,len);
	_spi_state.selector(-1);

	return r;
};

int spi_transmit(void* tx,void* rx,uint8_t len)
{
	MUTEXED(&_spi_state.mutex);

	_spi_state.ptr = 0;
	_spi_state.len = len;
	_spi_state.tx = tx;
	_spi_state.rx = rx;

	if (_spi_state.tx)
		SPDR = _spi_state.tx[0];
	else
		SPDR = 0;

	while (_spi_state.ptr < _spi_state.len){
		yield();
	};

	return ESUCCESS;
};

VECT(SPI_STC_vect)
{
	if (_spi_state.rx)
		_spi_state.rx[ _spi_state.ptr ] = SPDR;
	else
		(void)SPDR;

	_spi_state.ptr++;

	if (_spi_state.ptr < _spi_state.len)
	{
		if (_spi_state.tx)
			SPDR = _spi_state.tx[ _spi_state.ptr ];
		else
			SPDR = 0x00;
	};
};