#include "xbee.h"

#include "usart.h"
#include "mutex.h"

Mutex xbee_mutex;

void xbee_send(uint16_t type, int len, const uint8_t* buf) {
	xbee_mutex.lock();
	
	// Start and length.
	usart_send(0x7e);
	usart_send(((len + 16) >> 8) & 0xff);
	usart_send((len + 16) & 0xff);
	
	// Frame type and ID.
	usart_send(0x10);
	usart_send(0x01);
	
	// Destination address.
	usart_send(0x00);
	usart_send(0x13);
	usart_send(0xa2);
	usart_send(0x00);
	usart_send(0x40);
	usart_send(0x6f);
	usart_send(0x19);
	usart_send(0xf1);
	
	usart_send(0xff);
	usart_send(0xfe);
	usart_send(0x00);
	usart_send(0x00);
	
	uint8_t chsum = 0x83;
	
	usart_send(type & 0xff);
	chsum -= type & 0xff;
	usart_send(type >> 8);
	chsum -= type >> 8;
	
	// Payload
	for(int i = 0; i < len; i++) {
		usart_send(buf[i]);
		chsum -= buf[i];
	}
	
	usart_send(chsum);
	
	xbee_mutex.unlock();
}