/************************************************************************/
/** 
 * \brief	YARM_Lib rel 1.1
 *			8510 ON/OFF
 *				PA14 /EVENT8510
 *				PA27 /NPWRON
 *				PA15 NRESET8510
 *			8510 SPI Master
 *				PA04D (PAD0)
 *				PA05D (PAD1)
 *				PA06D (PAD2)
 *				PA07D (PAD3)
 *
*/
/************************************************************************/
#include <compiler.h>
#include <board.h>
#include <conf_board.h>
#include <port.h>
#include <asf.h>
#include <stdbool.h>

#include <extint.h>
#include <extint_callback.h>

//
#include "terminal_lib.h"

#include "YARM_lib.h"

#include <serial.h>
#include <stdio_serial.h>
#include <sam0_usart/usart_serial.h>

// PA14 /EVENT8510
#define EVENT8510_PIN               PIN_PA14
#define EVENT8510_ACTIVE            false
#define EVENT8510_INACTIVE          !EVENT8510_ACTIVE
// PA27 /NPWRON
#define NPWRON_PIN                  PIN_PA27
#define NPWRON_ACTIVE               false
#define NPWRON_INACTIVE             !NPWRON_ACTIVE
// PA15 NRESET8510
#define NRESET8510_PIN              PIN_PA15
#define NRESET8510_ACTIVE           false
#define NRESET8510_INACTIVE         !NRESET8510_ACTIVE

#define SW2_PIN                   PIN_PA02
#define SW2_ACTIVE                false
#define SW2_INACTIVE              !SW0_ACTIVE
#define SW2_EIC_PIN               PIN_PA02A_EIC_EXTINT2
#define SW2_EIC_MUX               MUX_PA02A_EIC_EXTINT2
#define SW2_EIC_PINMUX            PINMUX_PA02A_EIC_EXTINT2
#define SW2_EIC_LINE              2

#define EVENT_PIN                   PIN_PA14
#define EVENT_ACTIVE                false
#define EVENT_INACTIVE              !EVENT_ACTIVE
#define EVENT_EIC_PIN               PIN_PA14A_EIC_EXTINT14
#define EVENT_EIC_MUX               MUX_PA14A_EIC_EXTINT14
#define EVENT_EIC_PINMUX            PINMUX_PA14A_EIC_EXTINT14
#define EVENT_EIC_LINE              14

volatile uint32_t button_state;
volatile uint32_t event_radio;
uint32_t rxError;


void Button_ExtIntChannel(void);
void Button_ExtIntCallbacks(void);
void Button_Callback(void);

//
void Event8510_ExtIntChannel(void);
void Event8510_ExtIntCallbacks(void);
void Event8510_Callback(void);

void readFEPACs(void);

void setPower(uint8_t, uint8_t);


uint8_t SPI_Delay[]={
	0,			// 0x00
	0,			// 0x01 Read Fill Level Rx FIFO
	0,			// 0x02 Read Fill Level Tx FIFO
	0,			// 0x03 Read Fill Level RSSI FIFO
	0,			// 0x04 Get Event Bytes
	120,		// 0x05 Read RSSI FIFO
	120,		// 0x06 Read Rx FIFO
	110,		// 0x07 Write SRAM Reg
	120,		// 0x08 Read SRAM Reg
	55,			// 0x09 Write EEPROM
	0,			// 0x0A Read EEPROM
	110,		// 0x0B Write Tx FIFO
	110,		// 0x0C Write Tx Preamble FIFO
	55,			// 0x0D Set System Mode
	50,			// 0x0E Calibrate and Check
	0,			// 0x0F Patch SPI
	0,			// 0x10 Get RESET ROM
	0,			// 0x11
	0,			// 0x12
	0,			// 0x13 Get Version Flash
	0,			// 0x14 Customer Configurable Command
	0,			// 0x15 Systems RESET
	65,			// 0x16 Trigger EEPROM Secure Write
	85,			// 0x17 Set Voltage Monitor
	0,			// 0x18 OFF Command
	0,			// 0x19 Read Temperature Value
	50,			// 0x1A Init SRAM Service
	55,			// 0x1B Start RSSI Measurement
	0,			// 0x1C Get RSSI Value
	70,			// 0x1D Read Rx FIFO Byte Interrupt
	70,			// 0x1E Read RSSI FIFO Byte Interrupt
};


#define WEATHER_TX_CHNL	0		// 0, Chn 0, Serv 0
//#define WEATHER_TX_CHNL	11		// 9, Chn 0, Serv 3

uint8_t TxSequence[]={
	0x40,		// 0, Chn 0, Serv 0
	0x50,		// 1, Chn 1, Serv 0
	0x60,		// 2, Chn 2, Serv 0
	0x41,		// 3, Chn 0, Serv 1
	0x51,		// 4, Chn 1, Serv 1
	0x61,		// 5, Chn 2, Serv 1
	0x42,		// 6, Chn 0, Serv 2
	0x52,		// 7, Chn 1, Serv 2
	0x62,		// 8, Chn 2, Serv 2
	0x43,		// 9, Chn 0, Serv 3
	0x53,		// 10, Chn 1, Serv 3
	0x63,		// 11, Chn 2, Serv 3
	0x44,		// 12, Chn 0, Serv 4
	0x54,		// 13, Chn 1, Serv 4
	0x64		// 14, Chn 2, Serv 4
};
uint8_t seq_idx = WEATHER_TX_CHNL;

/* buffer used for all transactions */
#define BUF_LENGTH	64
static uint8_t bufferYARM[BUF_LENGTH];


static int32_t	YARM_InitDone = false;
YARM_State_t	YARM_CurrState = YARM_IDLE;
YARM_Cmd_t		YARM_Cmd = GETVERSIONFLASH;

uint8_t YARM_Events[2];
uint8_t SysError;
uint8_t YARM_Verbose = 0;

uint8_t TxPreambleBuffer[]={0x04, 0x70, 0x8E, 0x0A, 0x55, 0x55, 0x10, 0x55, 0x56};


struct spi_module spi_master_instance;
struct spi_slave_inst slave;
status_code_genare_t spi_ret;
volatile bool transrev_complete_spi_master = false;

/* Private function */
//static void configure_usart(void);
void configure_spi_master_callbacks(void);
void configure_spi_master(void);
unsigned char checksum (unsigned char *ptr, size_t sz);
/* ************************************************************************ */

static void callback_spi_master( struct spi_module *const module)
{
	transrev_complete_spi_master = true;
}


void configure_spi_master_callbacks(void)
{
	spi_register_callback(&spi_master_instance, callback_spi_master, SPI_CALLBACK_BUFFER_TRANSCEIVED);
	spi_enable_callback(&spi_master_instance, SPI_CALLBACK_BUFFER_TRANSCEIVED);
}

#define YARM_SPI_MODULE              SERCOM0
#define YARM_SPI_SERCOM_MUX_SETTING  SPI_SIGNAL_MUX_SETTING_E
#define YARM_SPI_SERCOM_PINMUX_PAD0  PINMUX_PA04D_SERCOM0_PAD0
#define YARM_SPI_SERCOM_PINMUX_PAD1  PINMUX_PA05D_SERCOM0_PAD1
#define YARM_SPI_SERCOM_PINMUX_PAD2  PINMUX_PA06D_SERCOM0_PAD2
#define YARM_SPI_SERCOM_PINMUX_PAD3  PINMUX_PA07D_SERCOM0_PAD3
#define YARM_SPI_SELECT_PIN PIN_PA05


void configure_spi_master(void)
{

	struct spi_config config_spi_master;
	struct spi_slave_inst_config slave_dev_config;
	/* Configure and initialize software device instance of peripheral slave */
	spi_slave_inst_get_config_defaults(&slave_dev_config);
	slave_dev_config.ss_pin = YARM_SPI_SELECT_PIN;
	spi_attach_slave(&slave, &slave_dev_config);
	/* Configure, initialize and enable SERCOM SPI module */
	spi_get_config_defaults(&config_spi_master);
	config_spi_master.mux_setting = YARM_SPI_SERCOM_MUX_SETTING;
	/* Configure pad 0 for data in */
	config_spi_master.pinmux_pad0 = YARM_SPI_SERCOM_PINMUX_PAD0;
	/* Configure pad 1 as unused */
	config_spi_master.pinmux_pad1 = PINMUX_UNUSED;
	/* Configure pad 2 for data out */
	config_spi_master.pinmux_pad2 = YARM_SPI_SERCOM_PINMUX_PAD2;
	/* Configure pad 3 for SCK */
	config_spi_master.pinmux_pad3 = YARM_SPI_SERCOM_PINMUX_PAD3;
	spi_init(&spi_master_instance, YARM_SPI_MODULE, &config_spi_master);

	spi_enable(&spi_master_instance);

}

void YARM_SPIDisable( void)
{
	spi_disable( &spi_master_instance);
}

void YARM_SPIEnable( void)
{
	spi_enable( &spi_master_instance);
}

void Event8510_ExtIntChannel(void)
{
//	return 0;
	struct extint_chan_conf config_extint_chan;
	extint_chan_get_config_defaults( &config_extint_chan);

	config_extint_chan.gpio_pin           = EVENT_EIC_PIN;
	config_extint_chan.gpio_pin_mux       = EVENT_EIC_MUX;
	config_extint_chan.gpio_pin_pull      = EXTINT_PULL_NONE;  // provo con pull_none per eliminare i 40uA di consumo quando viene chiamata questa
	config_extint_chan.detection_criteria = EXTINT_DETECT_FALLING;
	extint_chan_set_config( EVENT_EIC_LINE, &config_extint_chan);
}

void Event8510_ExtIntCallbacks(void)
{
	extint_register_callback( Event8510_Callback, EVENT_EIC_LINE, EXTINT_CALLBACK_TYPE_DETECT);
	extint_chan_enable_callback( EVENT_EIC_LINE, EXTINT_CALLBACK_TYPE_DETECT);
}

void Event8510_Callback(void)
{
	event_radio = 1;
	// event_radio = port_pin_get_input_level( EVENT_PIN);
	// port_pin_set_output_level(LED_0_PIN, pin_state);
}


void Button_ExtIntChannel(void)
{
	struct extint_chan_conf config_extint_chan;
	extint_chan_get_config_defaults( &config_extint_chan);

	config_extint_chan.gpio_pin           = SW2_EIC_PIN;
	config_extint_chan.gpio_pin_mux       = SW2_EIC_MUX;
	config_extint_chan.gpio_pin_pull      = EXTINT_PULL_UP;
	config_extint_chan.detection_criteria = EXTINT_DETECT_FALLING;
	extint_chan_set_config( SW2_EIC_LINE, &config_extint_chan);
}

void Button_ExtIntCallbacks(void)
{
	extint_register_callback( Button_Callback, SW2_EIC_LINE, EXTINT_CALLBACK_TYPE_DETECT);
	extint_chan_enable_callback( SW2_EIC_LINE, EXTINT_CALLBACK_TYPE_DETECT);
}

void Button_Callback(void)
{
	button_state = 1;
	// buttom_state = port_pin_get_input_level(BUTTON_0_PIN);
	// port_pin_set_output_level(LED_0_PIN, pin_state);
}


int32_t YARM_Init( void)
{
	struct port_config pin_conf;
	
	/* Enable External Interrupt for Button0 trap */
	// Button_ExtIntChannel();
	// Button_ExtIntCallbacks();
	Event8510_ExtIntChannel();

	Event8510_ExtIntCallbacks();
	system_interrupt_enable_global();

	port_get_config_defaults(&pin_conf);

	/*
	 *		PA14 /EVENT8510	(Inout)
	 *		PA27 /NPWRON	(Output)
	 *		PA15 NRESET8510	(Output)	
	*/
	/* Do not initialize the EVENT pin if you are configuring the External Interrupt */
#if 0
	/* Set EVENT8510 as inputs */
	pin_conf.direction  = PORT_PIN_DIR_INPUT;
	pin_conf.input_pull = PORT_PIN_PULL_UP;
	port_pin_set_config(EVENT8510_PIN, &pin_conf);
#endif

	/* Configure /NPWRON as outputs, turn active */
	pin_conf.direction  = PORT_PIN_DIR_OUTPUT;
	port_pin_set_output_level(NPWRON_PIN, NPWRON_INACTIVE);
	port_pin_set_config(NPWRON_PIN, &pin_conf);
	

	/* Configure NRESET8510 as outputs, turn active */
	pin_conf.direction  = PORT_PIN_DIR_OUTPUT;
	port_pin_set_output_level(NRESET8510_PIN, NRESET8510_INACTIVE);
	port_pin_set_config(NRESET8510_PIN, &pin_conf);

	// Configure SPI master and callback
	configure_spi_master();
	configure_spi_master_callbacks();
	
	if ( YARM_Verbose)
		printf("YARM Init Done...\r\n");
	
	YARM_PowerOn();
	
	return 0;
}

int32_t YARM_PowerOn( void)
{
	port_pin_set_output_level(NRESET8510_PIN, NRESET8510_ACTIVE);
	port_pin_set_output_level(NRESET8510_PIN, NRESET8510_ACTIVE);
	port_pin_set_output_level(NRESET8510_PIN, NRESET8510_ACTIVE);
	port_pin_set_output_level(NRESET8510_PIN, NRESET8510_ACTIVE);
	port_pin_set_output_level(NRESET8510_PIN, NRESET8510_INACTIVE);
	port_pin_set_output_level(NRESET8510_PIN, NRESET8510_INACTIVE);
	port_pin_set_output_level(NRESET8510_PIN, NRESET8510_INACTIVE);
	port_pin_set_output_level(NRESET8510_PIN, NRESET8510_INACTIVE);
	port_pin_set_output_level(NPWRON_PIN, NPWRON_ACTIVE);
	
	delay_ms(2);
	
	if ( YARM_Verbose)
		printf("YARM PowerOn Done...\r\n");
	YARM_InitDone=true;
	
	return 0;
}

int32_t YARM_PowerOff( void)
{
	port_pin_set_output_level(NPWRON_PIN, NPWRON_INACTIVE);			// true
	port_pin_set_output_level(NRESET8510_PIN, NRESET8510_ACTIVE);	// false
	port_pin_set_output_level(NRESET8510_PIN, NRESET8510_INACTIVE /*NRESET8510_ACTIVE*/);	// false
	
	return 0;
}

int32_t YARM_GetInitDone( void)
{
	return YARM_InitDone;
}

int32_t YARM_GetEvent( void)
{
	return port_pin_get_input_level( EVENT8510_PIN);
}

/**
 * \brief Switch to polling mode of the RF receiver
 *
 * Switch to polling mode with VCO tuning enabled and starting with polling configuration 0.
 */
int32_t YARM_SetPollingMode( void)
{
	int32_t ret;
	// set polling mode 
	uint8_t command[3]={ YARM_CMD_SETSYSTEMMODE, YARM_CMD_SETPOLLINGMODE, 0x00};
	uint8_t dummy[3];

	ret=YARM_SendCmd( command, dummy, YARM_CMD_SETPOLLINGMODE_LEN);
	
	return ret;
}

/**
 * \brief Switch to idle mode of the RF receiver
 *
 * Switch to idle mode.
 */
int32_t YARM_SetIdleMode(void)
{
	int32_t ret;
	
	// set idle mode
	uint8_t command[3]={ YARM_CMD_SETSYSTEMMODE, YARM_RF_IDLEMODE, YARM_RF_RXSERVICE};
	uint8_t dummy[3];
	
	ret=YARM_SendCmd( command, dummy, YARM_CMD_SETIDLEMODE_LEN);

	return ret;
}

/**
 * \brief Read the fill level of the RX buffer of the RF receiver
 *
 * Read the buffer level of the receiver data buffer.
 */
int32_t YARM_ReadRxFillLevel( uint8_t*d)
{	
	int32_t ret;
	
	uint8_t command[3]={ YARM_CMD_READRXFILLLEVEL, 0x00, 0x00};
	uint8_t data[3];
	
	ret=YARM_SendCmd( command, data, YARM_CMD_READRXFILLLEVEL_LEN);

	// fill level is in last received data byte 
	*d=data[2];
	
	return ret;
}

/**
 * \brief Read the fill level of the TX buffer of the RF receiver
 *
 * Read the buffer level of the transmitter buffer.
 */
int32_t YARM_ReadTxFillLevel( uint8_t*d)
{	
	int32_t ret;
	
	uint8_t command[3]={YARM_CMD_READTXFILLLEVEL,0x00,0x00};
	uint8_t data[3];
	
	ret=YARM_SendCmd( command, data, YARM_CMD_READTXFILLLEVEL_LEN);

	// fill level is in last received data byte 
	*d=data[2];
	
	return ret;
}

/**
 * \brief Read the fill level of the RSSI buffer of the RF receiver
 *
 * Read the buffer level of the RSSI buffer.
 */
int32_t YARM_ReadRssiFillLevel( uint8_t*d)
{	
	int32_t ret;
	
	uint8_t command[3]={YARM_CMD_READRSSIFILLLEVEL, 0x00, 0x00};
	uint8_t data[3];
	
	ret=YARM_SendCmd( command, data, YARM_CMD_READTXFILLLEVEL_LEN);

	// fill level is in last received data byte 
	*d=data[2];
	
	return ret;
}

/**
 * \brief Start the RSSI measurement
 *
 * 
 */
int32_t YARM_StartRSSIMeasurement( void)
{	
	int32_t ret;
	
	uint8_t command[2]={YARM_CMD_STARTRSSIMSRMNT, 0x00};
	uint8_t data[2];
	
	ret=YARM_SendCmd( command, data, YARM_CMD_STARTRSSIMSRMNT_LEN);
	
	return ret;
}

/**
 * \brief Get the RSSI measurement
 *
 * 
 */
int32_t YARM_GetRSSIMeasurement( uint8_t*d)
{	
	int32_t ret;
	
	uint8_t command[4]={YARM_CMD_GETRSSIMSRMNT, 0x00, 0x00, 0x00};
	uint8_t data[4];
	
	ret=YARM_SendCmd( command, data, YARM_CMD_GETRSSIMSRMNT_LEN);
	
	d[0]=data[2];	// Last measured RSSI average value
	d[1]=data[3];	// Last measured RSSI peak value
	return ret;
}

/**
 * \brief Read the config event byte of the RF receiver
 *
 * Read the event bytes of the receiver and return the config event byte.
 */
uint8_t YARM_GetConfigEventByte( uint8_t*d)
{
	int32_t ret;
	uint8_t command[4]={YARM_CMD_GETCONFIGEVENTBYTE,0x00,0x00,0x00};
	uint8_t data[4];
	
	ret=YARM_SendCmd( command, data, YARM_CMD_GETCONFIGEVENTBYTE_LEN);

	// return config event byte with channel, service and path information
	*d=data[3];
	
	return ret;
}

/**
 * \brief Read the event bytes of the RF receiver
 *
 * Read the event bytes of the receiver and return these bytes.
 */
int32_t YARM_GetEventBytes(uint8_t *data)
{
	int32_t ret;
	uint8_t command[4]={YARM_CMD_GETEVENTBYTE, 0x00, 0x00, 0x00};
	
	ret=YARM_SendCmd( command, data, YARM_CMD_GETEVENTBYTE_LEN);
	
	return ret;

}

/**
 * \brief Read RSSI buffer of the RF receiver
 *
 * Read the RSSI buffer of the receiver.
 */
int32_t YARM_ReadRssiFifo(uint8_t data_size, uint8_t *data)
{
	int32_t ret;
	uint8_t command[35]={YARM_CMD_READRSSIFIFO,data_size};
	
	ret=YARM_SendCmd( command, data, data_size+3);

	return ret;
}

/**
 * \brief Read RX buffer of the RF receiver
 *
 * Read the RX buffer of the receiver.
 */
int32_t YARM_ReadRxFifo(uint8_t data_size, uint8_t *data)
{
	int32_t ret;
	uint8_t command[35]={YARM_CMD_READRXFIFO,data_size};
	
	ret=YARM_SendCmd( command, data, data_size+3);
	
	return ret;

}

/**
 * \brief Write SRAM/REG of the RF receiver
 *
 * Write content to SRAM/Reg of the receiver.
 */
int32_t YARM_WriteSram( uint8_t data_size, uint8_t addr_h, uint8_t addr_l, uint8_t *data)
{
	int32_t ret;
	uint8_t command[35]={YARM_CMD_WRITESRAM, data_size, addr_h, addr_l};
	uint8_t index;
	
	for ( index=0; index < data_size; index++)
	{
		command[4 + index]= data[index];
	}
	
	ret=YARM_SendCmd( command, data, data_size+YARM_CMD_WRITESRAM_LEN);

	return ret;
}

/**
 * \brief Read SRAM/REG of the RF receiver
 *
 * Read content from SRAM/Reg of the receiver.
 */
int32_t YARM_ReadSram(uint8_t data_size, uint8_t addr_h, uint8_t addr_l, uint8_t *data)
{
	int32_t ret;
	uint8_t command[35]={YARM_CMD_READSRAM, data_size, addr_h, addr_l, 0x00};
	
	ret=YARM_SendCmd( command, data, data_size+YARM_CMD_READSRAM_LEN);

	return ret;
}

/**
 * \brief Write byte to EEPROM
 *
 * Write one byte to the EEPROM of the receiver.
 */
int32_t YARM_WriteEeprom(uint8_t ee_addr_h, uint8_t ee_addr_l, uint8_t data)
{
	int32_t ret;
	uint8_t command[4]={YARM_CMD_WRITEEEPROM,ee_addr_h, ee_addr_l, data};
	uint8_t dummy[4];
	
	ret=YARM_SendCmd( command, dummy, YARM_CMD_WRITEEEPROM_LEN);

	return ret;
}

/**
 * \brief Read byte from EEPROM
 *
 * Read one byte from the EEPROM of the receiver.
 */
int32_t YARM_ReadEeprom(uint8_t ee_addr_h, uint8_t ee_addr_l, uint8_t*d)
{
	int32_t ret;
	uint8_t command[5]={YARM_CMD_READEEPROM, ee_addr_h, ee_addr_l, 0x00, 0x00};
	uint8_t data[5];
	
	ret=YARM_SendCmd( command, data, YARM_CMD_READEEPROM_LEN);
	
	*d=data[5];
	return ret;
}

/**
 * \brief Write TX buffer of the RF receiver
 *
 * Write data to the TX buffer of the receiver.
 */
int32_t YARM_WriteTxFifo( uint8_t data_size, uint8_t *data)
{
	int32_t ret;
	uint8_t command[35]={YARM_CMD_WRITETXFIFO, data_size};
	uint8_t dummy[35];
	uint8_t index;
	
	for (index=0; index <data_size; index++)
	{
		command[2 + index]= data[index];
	}
	
	ret=YARM_SendCmd( command, dummy, data_size+2);
	
	return ret;

}

/**
 * \brief Write TX preamble buffer of the RF receiver
 *
 * Write data to the TX preamble buffer of the receiver.
 */
int32_t YARM_WriteTxPreamble(uint8_t data_size, uint8_t *data)
{
	int32_t ret;
	uint8_t command[35]={YARM_CMD_WRITETXPREAMBLE, data_size};
	uint8_t dummy[35];
	uint8_t index;
	
	for (index=0; index <data_size; index++)
	{
		command[2 + index]= data[index];
	}
	
	ret=YARM_SendCmd( command, dummy, data_size+2);

	return ret;
}

/**
 * \brief Set system mode of the RF receiver
 *
 * Switch the transceiver to the wanted system mode.
 */
int32_t YARM_SetSystemMode(uint8_t mode_config, uint8_t service_channel)
{
	int32_t ret;
	uint8_t command[3]={YARM_CMD_SETSYSTEMMODE, mode_config, service_channel};
	uint8_t dummy[3];
	
	ret=YARM_SendCmd( command, dummy, YARM_CMD_SETSYSTEMMODE_LEN);
	
	return ret;
}

/**
 * \brief Read the ROM, Flash and customer version of the RF receiver
 *
 * Read the ROM, Flash and customer version of the receiver and return the ROM version.
 */
int32_t YARM_GetVersionFlash( uint8_t*d)
{
	int32_t ret;
	uint8_t command[6]={YARM_CMD_GETVERSIONFLASH,0x00,0x00,0x00,0x00,0x00};
	uint8_t data[6];
	
	ret=YARM_SendCmd( command, data, YARM_CMD_GETVERSIONFLASH_LEN);
	
	// return the ROM version code
	*d=data[2];
	return ret;
}

/**
 * \brief Perform a reset of the RF receiver
 *
 * Perform a reset of the RF receiver.
 */
int32_t YARM_Reset(void)
{
	int32_t ret;
	uint8_t command[2]={YARM_CMD_RESET, 0x00};
	uint8_t data[6];
	
	ret=YARM_SendCmd( command, data, YARM_CMD_RESET_LEN);
	
	return ret;
}

uint32_t YARM_PrepareTxData( uint8_t*b, uint8_t*d, uint32_t l)
{
	uint32_t i;
	
	b[0]=YARM_DATA_HEADER;
	for ( i=0; i<l; i++) {
		b[i+YARM_DATA_HEADER_LEN]=d[i];
	}
	i+=YARM_DATA_HEADER_LEN;
	b[i]=checksum( b, i);
	
	return l+YARM_DATA_HEADER_LEN+YARM_CHECKSUM_LEN;
}

/**
 * \brief Get the Sys Error last result
 *
 * 
 */
int32_t YARM_GetSysError( void)
{	
	return (int32_t)SysError;
}

unsigned char checksum (unsigned char *ptr, size_t sz)
{
	unsigned char chk = 0;

	while (sz-- != 0)
		chk -= *ptr++;

	return chk;
}

int32_t YARM_SendCmd( uint8_t*txb, uint8_t*rxb, uint8_t len)
{
	uint32_t tmout;
	
//	uint8_t *ptr;
//	uint32_t len;
//	
//	switch( cmd) {
//		case GETVERSIONFLASH:
//			ptr = &CmdGetVersionFlash_buffer[0];
//			len = CMD_GETVERSIONFLASH_LEN;
//		break;
//		case SETIDLEMODE:
//			ptr = &CmdSetIdleMode_buffer[0];
//			len = CMD_SETIDLEMODE_LEN;
//		break;
//		case READTEMPERATURE:
//			ptr=&CmdReadTemperature_buffer[0];
//			len = CMD_READTEMPERATURE_LEN;
//		break;
//		case GETEVENTBYTES:
//			ptr = &CmdGetEventBytes_buffer[0];
//			len = CMD_GETEVENTBYTES_LEN;
//		break;
//		default:
//			ptr = &CmdSetIdleMode_buffer[0];
//			len = CMD_SETIDLEMODE_LEN;		
//		break;
//	}
	
	spi_select_slave(&spi_master_instance, &slave, true);
	delay_us( 45);
	
	spi_ret=spi_transceive_buffer_job(&spi_master_instance, txb, rxb, len);
	
	//
	if ( spi_ret != 0) {
		// Error
		spi_select_slave(&spi_master_instance, &slave, false);
		return -spi_ret;
	}
	
	tmout=0;
	while (!transrev_complete_spi_master) {
		/////* Wait for write and read complete */
		delay_us(1);
		tmout++;
		if ( tmout>3000) {
			// Error...
			spi_select_slave(&spi_master_instance, &slave, false);
			return -STATUS_ERR_TIMEOUT;
		}
			
	}
	transrev_complete_spi_master = false;
	if ( SPI_Delay[ txb[0]])
		delay_us( SPI_Delay[ txb[0]]);
	spi_select_slave(&spi_master_instance, &slave, false);
	
	YARM_Events[0] = rxb[0];
	YARM_Events[1] = rxb[1];
	SysError=YARM_Events[0]&0x80;

	return len;
}

void readFEPACs(void) {
	// test read SRAM location for FEPAC (power)
	uint8_t FEPAC[10];

	YARM_ReadSram( 1, 0x02, 0x2A, FEPAC);
	printf("current FEPAC: %d", FEPAC[5]);
	YARM_ReadSram( 1, 0x03, 0x3B, FEPAC);
	printf("; SRVC3 FEPAC: %d", FEPAC[5]);
	YARM_ReadSram( 1, 0x03, 0xC8, FEPAC);
	printf("; SRVC4 FEPAC: %d\r\n", FEPAC[5]);
}

void setPower(uint8_t serv, uint8_t pow) {
	// set the output power for the service passed (only 3 or 4 for now)
	if (serv == 3) {
		YARM_WriteSram( 1, 0x03, 0x3B, &pow);
//		readFEPACs();
	} else { 
		if (serv == 4) {
			YARM_WriteSram( 1, 0x03, 0xC8, &pow);
		}
	}
}


void YARM_PrintSysError( const char*s)
{
	uint8_t TxRxEvent[16];
	//TERM_CURSOR_SAVE;
	if ( YARM_GetSysError())
	{
		//
		YARM_ReadSram( 1, 0x03, 0x00, TxRxEvent);
		TERM_BKGRD_WHITE;
		TERM_TEXT_RED;
		//TERM_CURSOR_POS(1,1);
		printf("Error at %s, ErrorCode: %d\r\n", s, TxRxEvent[5]);
		} else {
		TERM_TEXT_DEFAULT;
		//TERM_CURSOR_POS(1,1);
		printf("Done %s [0x%X 0x%X]\r\n", s, YARM_Events[0], YARM_Events[1]);
	}
	TERM_TEXT_DEFAULT;
	//TERM_CURSOR_RESTORE;
}


void YARM_Send ( uint8_t txLength, uint8_t *buftx, uint8_t service, uint8_t channel, uint8_t power, bool blocking) {
	// sends a message
	// txLength => length of the message (max 32 for now)
	// sd.data => pointer to data to send
	// service => service to be used (0,1,2,3 or 4)
	// channel => channel to be used (0,1 or 2)
	// power => power of the transmission (0 [-12.9 dBm] to 60 [+14.5dBm]) 
	//          working for now only on service 3 (copy of service 0)
	// blocking => set is as true if blocking behaviour wanted 
	//          otherwise it returns immediately after instructing 
	//          8510 to send the message.
	uint32_t buffer_len;
	uint32_t i;

	YARM_SetIdleMode();
	delay_us( 70);

	if ( YARM_Verbose) { YARM_PrintSysError("YARM_SetIdleMode"); delay_ms( 100); }
#if 1	
	/* print out YARM state */
	buffer_len = YARM_GetEventBytes( &bufferYARM[0]);
	if ( YARM_Verbose ) {
		if ( buffer_len < 0 ) {
			printf( "Errore YARM_GetEventBytes %d\r\n", (buffer_len*-1));
		} else {
			printf( "Reg. GETEVENTBYTES: ");
			for ( i=0; i<buffer_len; i++) {
				printf( "0x%X, ", bufferYARM[i]);
			}
			printf( "\r\n");
		}
	}
#endif	
	TERM_TEXT_RED;
//	setPower(service, power);
	TERM_TEXT_DEFAULT;
		
	// TO BE DONE: use service and channel checking for bounds
	YARM_SetSystemMode( YARM_RF_TXMODE, TxSequence[seq_idx]);
	if ( YARM_Verbose) { YARM_PrintSysError("YARM_SetSystemMode TXMode"); delay_ms( 100); }
		
	YARM_WriteTxPreamble( YARM_WriteTxPreambleBuffer_LEN, &TxPreambleBuffer[0]);
	if ( YARM_Verbose) YARM_PrintSysError("YARM_WriteTxPreamble");
		
	delay_us( 100);

	YARM_WriteTxFifo( txLength, buftx);
	if ( YARM_Verbose) { YARM_PrintSysError("YARM_WriteTxFifo"); delay_ms( 100); }
		
	if ( YARM_Verbose) printf("TXLoop Started\r\n");
		
	// Controlla EVENT_IRQ
	event_radio = 0;
	while( !event_radio);
	event_radio = 0;
		
	//
	i = 0;
	do {
		i++;
		delay_us( 3000);
		YARM_GetEventBytes( &bufferYARM[0]);
		// YARM_PrintSysError( "YARM_GetEventBytes TXLoop");
	} while (((bufferYARM[1] & 0x10) == 0)&&(i <40));

	if ( YARM_Verbose) {
		printf("TXLoop Ended at cycle : %d [%d ms]\r\n", i, i*3);
		printf("TX Ended on service: %d channel: %d\r\n", (TxSequence[seq_idx] & 0x07), (TxSequence[seq_idx]>>4)&0x03);
			
		/* print out YARM state */
		buffer_len = YARM_GetEventBytes( &bufferYARM[0]);
		if ( buffer_len < 0 ) {
			printf( "Error YARM_GetEventBytes %d\r\n", (buffer_len*-1));
		} else {
			printf( "Reg. GETEVENTBYTES: ");
			for ( i=0; i<buffer_len; i++) {
				printf( "0x%X, ", bufferYARM[i]);
			}
			printf( "\r\n");
		}
	}
	delay_us( 70);
		
	YARM_SetIdleMode();
	if (  YARM_Verbose) { YARM_PrintSysError("YARM_SetIdleMode");	delay_ms( 100); }
	delay_us( 70);
}

uint8_t YARM_Receive (uint16_t *rxLength, uint8_t **rxbuffer, uint8_t *serv, uint8_t *chan, uint32_t *rssi) {
	uint32_t buffer_len;
	uint32_t i, ii;
	uint8_t rxError = 0;
	uint8_t serviceChannelReceived;
	uint32_t rssiReceived;
	
	for ( i=0; i<BUF_LENGTH; i++) {
		bufferYARM[i]=0;
	}

	do {
		delay_us( 500);
		//
		buffer_len = YARM_GetEventBytes( &bufferYARM[0]);
		serviceChannelReceived = bufferYARM[3];
		//YARM_YARM_PrintSysError("YARM_GetEventBytes EOTA");
	} while( (bufferYARM[1] & 0x10) == 0);			// EOTA: End of Telegram on path A
	delay_us( 5000);
	
	buffer_len = YARM_ReadRssiFillLevel( &bufferYARM[0]);
	rssiReceived=0;
	i=bufferYARM[0];
	if ( i!=0) {
		YARM_ReadRssiFifo( i, bufferYARM);

		ii=0;
		while( ii<i) {
			rssiReceived += bufferYARM[ii+3];
			ii++;
		}
		//
		rssiReceived/=i;
	} else {
		rxError |= 2;
	}
	*rssi = rssiReceived;	
		
	// verify Rx FIFO
	buffer_len = YARM_ReadRxFillLevel( &bufferYARM[0]);
	i=bufferYARM[0];
	*rxLength = i;

	// check FIFO length
	if ( i>0 && i <= 32) {
		YARM_ReadRxFifo( i, bufferYARM);
		// Checksum test
		if (bufferYARM[i-1+3] != checksum(bufferYARM+3, i-1)) {
			rxError|=1;
		}
	}

	if ( YARM_Verbose) {
		printf("YARM_Libs: Data Received [32]\r\n");
		for ( i=0; i<32;i++)
		printf(" 0x%0X, ", bufferYARM[i+3]);
		printf("\r\n");
	}

	YARM_SetIdleMode();
	if (  YARM_Verbose) YARM_PrintSysError("YARM_SetIdleMode");


	*serv = serviceChannelReceived&0x07;
	*chan = (serviceChannelReceived&0x30>>4); 
//	printf("address of bufferYARM[3] = %x\r\n", &bufferYARM[3]);
//	printf("**rxbuffer = %x\r\n", **rxbuffer);
	
	*rxbuffer = &bufferYARM[3];
//	printf("**rxbuffer = %x\r\n", **rxbuffer);

	return rxError;	
}