/*
             LUFA Library
     Copyright (C) Dean Camera, 2011.

  dean [at] fourwalledcubicle [dot] com
           www.lufa-lib.org
*/

/*
  Copyright 2011  Dean Camera (dean [at] fourwalledcubicle [dot] com)

  Permission to use, copy, modify, distribute, and sell this
  software and its documentation for any purpose is hereby granted
  without fee, provided that the above copyright notice appear in
  all copies and that both that the copyright notice and this
  permission notice and warranty disclaimer appear in supporting
  documentation, and that the name of the author not be used in
  advertising or publicity pertaining to distribution of the
  software without specific, written prior permission.

  The author disclaim all warranties with regard to this
  software, including all implied warranties of merchantability
  and fitness.  In no event shall the author be liable for any
  special, indirect or consequential damages or any damages
  whatsoever resulting from loss of use, data or profits, whether
  in an action of contract, negligence or other tortious action,
  arising out of or in connection with the use or performance of
  this software.
*/

/** \file
 *
 *  Main source file for the RNDISEthernet demo. This file contains the main tasks of
 *  the demo and is responsible for the initial application hardware configuration.
 */

#include "usb-rfm.h"

//#include <avr/pgmspace.h>
#include <alloca.h>
#include "rfm22b_regs.h"

#define MAX_PACKET_LENGTH 48
//unit is *0.5db above noise TODO base on received SNR instead of static
#define CCA_SNR 9
#define RX_BUFSIZE 80
#define BINARY_COUNT 20
#define ADAPTER_MAC_ADDRESS              {0x02, 0x00, 0x02, 0x00, 0x02, 0x00}
#define ETHERNET_RX_TIMEOUT 400
//minimum time betwen sending packets
//#define TX_RATE_LIMIT 40
#define TX_RATE_LIMIT tx_rate_limit
#define BRIDGE_TABLE_SIZE 15
#define BRIDGE_MAX_AGE 30

typedef struct {
	unsigned tx_data_set :1; //packet is in transmit buffer for transmit
	unsigned transmitter_active :1; //don't enter rx mode or modify tx data
	unsigned queue_packet_set :1; //for retransmitting normal data packets
	unsigned immediate_packet_set :1; //for control packets
	unsigned tx_data_is_queue_packet :1; //one or the other
	uint8_t tx_destination; //data in transmit buffer
	uint8_t tx_sequence;
	uint8_t tx_type;
	uint8_t sequence; //for single-packet data, increment or random
	uint8_t rx_sequence; //ack and ignore duplicates
	uint8_t rx_sequence_src; //todo track multiple?
	uint8_t queue_timeout_ms; //time until packet is [re]sent
	uint8_t tx_timeout_ms; //time until packet is [re]sent
	uint8_t queue_retransmit_count;
	uint8_t tx_cca_failure_count;
	uint8_t tx_ethernet_sequence;
	uint8_t tx_ethernet_retransmit_count;
	uint8_t rx_ethernet_src; //clear when frame received
	uint8_t rx_ethernet_sequence; //last
	//uint8_t rx_have_seq;
	//uint8_t tx_seq_sent;
	uint8_t address; //this node
} radio_status;

typedef struct {
	uint8_t destination;
	uint8_t source;
	uint8_t type;
	uint8_t sequence;	//for ethernet or other multi-packet, a multiple of 32 bytes
				//for single-packet, circular counter for duplicate detection
	uint8_t length;
	uint8_t *data;
} rf_packet;

typedef struct {
	uint8_t mac[6];
	uint8_t node;
	uint8_t age_s;
} bridge_entry;

void console_sendbyte(char byte);
void console_processbyte(char c);
void console_processline(char *line);
uint8_t parse_ascii(char *word, uint8_t *store, uint8_t max);

int8_t rfm22b_init(void);
uint8_t spi_xfer(uint8_t send);
void rfm22b_writem(uint8_t reg, uint8_t *data, uint8_t count);
void rfm22b_readm(uint8_t reg, uint8_t *data, uint8_t count);
void rfm22b_write(uint8_t reg, uint8_t data);
uint8_t rfm22b_read(uint8_t reg);
void console_break(void);
void rfm22b_reset(void);
void rfm22b_interrupt(void);
void rfm22b_cbi(uint8_t reg, uint8_t bv);
void rfm22b_sbi(uint8_t reg, uint8_t bv);
void rfm22b_cb(uint8_t reg, uint8_t mask);
void rfm22b_sb(uint8_t reg, uint8_t mask);
void tick_1khz(void);
void tick_1hz(void);
void process_rx_packet(rf_packet packet);
void rfm_transmit(void);
int8_t transmit_packet(rf_packet packet);
void send_test_string(uint8_t destination, char *data);
void send_ethernet(Ethernet_Frame_Info_t *frame);
void send_ack(rf_packet packet);
void send_nak(rf_packet packet);
void bridge_learn(Ethernet_Frame_Info_t *frame, uint8_t srcnode);
uint8_t bridge_lookup(Ethernet_Frame_Info_t *frame);
void bridge_ageing(void);
void print_ethernet_frame(Ethernet_Frame_Info_t *frame);
void set_tx_data(void);

/** LUFA RNDIS Class driver interface configuration and state information. This structure is
 *  passed to all RNDIS Class driver functions, so that multiple instances of the same class
 *  within a device can be differentiated from one another.
 */
USB_ClassInfo_RNDIS_Device_t Ethernet_RNDIS_Interface =
	{
		.Config =
			{
				.ControlInterfaceNumber         = 0,

				.DataINEndpointNumber           = CDC_TX_EPNUM,
				.DataINEndpointSize             = CDC_TXRX_EPSIZE,
				.DataINEndpointDoubleBank       = false,

				.DataOUTEndpointNumber          = CDC_RX_EPNUM,
				.DataOUTEndpointSize            = CDC_TXRX_EPSIZE,
				.DataOUTEndpointDoubleBank      = false,

				.NotificationEndpointNumber     = CDC_NOTIFICATION_EPNUM,
				.NotificationEndpointSize       = CDC_NOTIFICATION_EPSIZE,
				.NotificationEndpointDoubleBank = false,

				.AdapterVendorDescription       = "LUFA RNDIS Demo Adapter",
				.AdapterMACAddress              = {ADAPTER_MAC_ADDRESS},
			},
	};

/** LUFA CDC Class driver interface configuration and state information. This structure is
 *  passed to all CDC Class driver functions, so that multiple instances of the same class
 *  within a device can be differentiated from one another. This is for the second CDC interface,
 *  which echos back all received data from the host.
 */
USB_ClassInfo_CDC_Device_t VirtualSerial2_CDC_Interface =
	{
		.Config =
			{
				.ControlInterfaceNumber           = 2,

				.DataINEndpointNumber             = CDC2_TX_EPNUM,
				.DataINEndpointSize               = CDC2_TXRX_EPSIZE,
				.DataINEndpointDoubleBank         = false,

				.DataOUTEndpointNumber            = CDC2_RX_EPNUM,
				.DataOUTEndpointSize              = CDC2_TXRX_EPSIZE,
				.DataOUTEndpointDoubleBank        = false,

				.NotificationEndpointNumber       = CDC2_NOTIFICATION_EPNUM,
				.NotificationEndpointSize         = CDC_NOTIFICATION_EPSIZE,
				.NotificationEndpointDoubleBank   = false,
			},
	};


/** Global to store the incoming frame from the host before it is processed by the device. */
static Ethernet_Frame_Info_t FrameIN;
static uint16_t framein_remaining_size; //set to frame size when packet received, counts down

/** Global to store the outgoing frame created in the device before it is sent to the host. */
static Ethernet_Frame_Info_t FrameOUT;
static uint16_t frameout_ready_size; //counts up to frame size with received data

static FILE USBSerialStream;

static uint8_t noisy=0;
static uint16_t count_1khz=0;
static uint16_t rssi_decimate;
static uint8_t rssi_decimate_count;
static uint16_t rssi;
static radio_status rfm_status;
static uint16_t ethernet_rx_timeout_ms;
static uint8_t tx_rate_limit_ms;
static bridge_entry bridge_table[BRIDGE_TABLE_SIZE];
static rf_packet queue_packet;
static uint8_t queue_packet_data[MAX_PACKET_LENGTH];
static rf_packet immediate_packet;
static uint8_t tx_rate_limit=10;

static uint16_t data_rssi;
static uint8_t data_rssi_last;
static uint16_t data_rssi_decimate;
static uint8_t data_rssi_count;
static uint8_t preamble_rssi;


/** Main program entry point. This routine contains the overall program flow, including initial
 *  setup of all components and the main program loop.
 */
int main(void)
{
	SetupHardware();

	//TCP_Init();
	//Webserver_Init();

	LEDs_SetAllLEDs(LEDMASK_USB_NOTREADY);
	sei();

	for (;;)
	{
		int16_t ReceivedByte = CDC_Device_ReceiveByte(&VirtualSerial2_CDC_Interface);
		if (!(ReceivedByte < 0))
		{
			console_processbyte((uint8_t)ReceivedByte);
			//CDC_Device_SendByte(&VirtualSerial2_CDC_Interface, (uint8_t)ReceivedByte);
		}

		if (framein_remaining_size==0 && RNDIS_Device_IsPacketReceived(&Ethernet_RNDIS_Interface))
		{
			LEDs_SetAllLEDs(LEDMASK_USB_BUSY);
			RNDIS_Device_ReadPacket(&Ethernet_RNDIS_Interface, &FrameIN.FrameData, &FrameIN.FrameLength);
			framein_remaining_size=FrameIN.FrameLength;
			rfm_status.tx_ethernet_sequence=0;
			rfm_status.tx_ethernet_retransmit_count=0;
			//Ethernet_ProcessPacket(&FrameIN, &FrameOUT);
			if(noisy>=6)
			{
				printf_P(PSTR("Received an ethernet packet from host for processing\r\n"));
				print_ethernet_frame(&FrameIN);
			}
			LEDs_SetAllLEDs(LEDMASK_USB_READY);
		}
		if (FrameOUT.FrameLength && frameout_ready_size==FrameOUT.FrameLength)
		{
			LEDs_SetAllLEDs(LEDMASK_USB_BUSY);
			if(noisy>=6)
			{
				printf_P(PSTR("Sending an ethernet packet to the host\r\n"));
				print_ethernet_frame(&FrameOUT);
			}
			RNDIS_Device_SendPacket(&Ethernet_RNDIS_Interface, &FrameOUT.FrameData, FrameOUT.FrameLength);
			bridge_learn(&FrameOUT, rfm_status.rx_ethernet_src);
			FrameOUT.FrameLength = 0;
			//frameout_ready_size=0;
			rfm_status.rx_ethernet_src=0;
			LEDs_SetAllLEDs(LEDMASK_USB_READY);
		}

		//TCP_TCPTask(&Ethernet_RNDIS_Interface, &FrameOUT);

		RNDIS_Device_USBTask(&Ethernet_RNDIS_Interface);
		CDC_Device_USBTask(&VirtualSerial2_CDC_Interface);
		USB_USBTask();

		if(!(PIND&(1<<0)))
		{
			rfm22b_interrupt();
		}
		if(count_1khz>0)
		{
			count_1khz--;
			tick_1khz();
		}
		if(!rfm_status.transmitter_active && rfm_status.tx_data_set && rfm_status.tx_timeout_ms==0 && tx_rate_limit_ms==0)
		{
			rfm_transmit();
		}
		if(framein_remaining_size && !rfm_status.queue_packet_set)
		{
			send_ethernet(&FrameIN);
		}
		if(!rfm_status.tx_data_set)
		{
			set_tx_data();
		}
		if(ethernet_rx_timeout_ms==0 && rfm_status.rx_ethernet_src)
		{
			//abort frame
			rfm_status.rx_ethernet_src=0;
		}

	}
}

void console_sendbyte(char byte)
{
//	if(byte==10) CDC_Device_SendByte(&VirtualSerial2_CDC_Interface, 13);
	CDC_Device_SendByte(&VirtualSerial2_CDC_Interface, byte);
}

/*
 * ----------------------------------------------------------------------------
 * "THE BEER-WARE LICENSE" (Revision 42):
 * <joerg@FreeBSD.ORG> wrote this file.  As long as you retain this notice you
 * can do whatever you want with this stuff. If we meet some day, and you think
 * this stuff is worth it, you can buy me a beer in return.        Joerg Wunsch
 * ----------------------------------------------------------------------------
 *
 * Stdio demo
 *
 */


void console_processbyte(char c)
{
//	console_sendbyte(byte+1);
  //uint8_t c;
  char *cp2;
  static char b[RX_BUFSIZE];
  static char *cp = b;
  //static char *rxp;
	//static char *cp3;

	/* behaviour similar to Unix stty ICRNL */
	if (c == '\r')
	  c = '\n';
	if (c == '\n') //enter
	  {
	    console_sendbyte('\r');
	    console_sendbyte('\n');
	    *cp='\0';
            console_processline(b);
	    cp=b;
	    //break;
	  }
	else if (c == '\t') //tab
	  c = ' ';

	if ((c >= (uint8_t)' ' && c <= (uint8_t)'\x7e') ||
	    c >= (uint8_t)'\xa0')
	  {
	    if (cp == b + RX_BUFSIZE - 1)
	      console_sendbyte('\a');
	    else
	      {
		*cp++ = c;
		console_sendbyte(c);
	      }
	    //continue;
	  }

	switch (c)
	  {
	  case 'c' & 0x1f:
	    console_break();
            break;

	  case '\b': //backspace
	  case '\x7f':
	    if (cp > b)
	      {
		console_sendbyte('\b');
		console_sendbyte(' ');
		console_sendbyte('\b');
		cp--;
	      }
	    break;

	  case 'r' & 0x1f: //redraw
	    console_sendbyte('\r');
	    for (cp2 = b; cp2 < cp; cp2++)
	      console_sendbyte(*cp2);
	    break;

	  case 'u' & 0x1f: //erase line
	    while (cp > b)
	      {
		console_sendbyte('\b');
		console_sendbyte(' ');
		console_sendbyte('\b');
		cp--;
	      }
	    break;

	  case 'w' & 0x1f: //erase word
	    while (cp > b && cp[-1] != ' ')
	      {
		console_sendbyte('\b');
		console_sendbyte(' ');
		console_sendbyte('\b');
		cp--;
	      }
	    break;
	  }
}

uint8_t cmdcmp(char *line, char *cmd)
{
	int len;
	len=strlen(cmd);
	if(!strncmp(cmd,line,len))
		return len;
	else
		return 0;
}
uint8_t cmdcmp_P(char *line, PGM_P cmd)
{
	char tempcmd[15];
	uint8_t i;
	i=0;
	while(1)
	{
		tempcmd[i]=pgm_read_byte(cmd+i);
		if(tempcmd[i]==0) break;
		if(i>=15) return 0;
		i++;
	}
	return cmdcmp(line,tempcmd);
}

void console_write(char *word)
{
	char hex[3]="00";
	uint8_t i;
	int b;
	uint8_t a;
	i=0;
	while(word[i]!=' ')
	{
		if(word[i]==0) return;
		i++;
	}
	word[i]=0;
	if(sscanf(word,"%x",&b)<=0)
		return;
	PORTB&=~(1<<0);
	if(noisy>=8) printf_P(PSTR("CS-\r\n"));
	spi_xfer(b | (1<<7));
	word+=i+1;
	while(word[0]!=0)
	{
		hex[0]=word[0];
		if(word[1]==0)
		{
			hex[1]=word[0];
			hex[0]='0';
			word-=1; //hack to make word+2=0;
		}else
		{
			hex[1]=word[1];
		}
		if(sscanf(hex,"%x",&b)>0)
		{
			a=b;
		}else
		{
			a=0;
			if(noisy>=4) printf_P(PSTR("Could not parse \"%s\"\r\n"),hex);
		}
		//printf_P(PSTR("Value: %d\r\n"),a);
		spi_xfer(a);
		word+=2;
	}
	PORTB|=(1<<0);
	if(noisy>=8) printf_P(PSTR("CS+\r\n"));
}
void ee_write(char *word)
{
	char hex[3]="00";
	uint8_t i;
	int b;
	uint8_t a;
	int addr;
	i=0;
	while(word[i]!=' ')
	{
		if(word[i]==0) return;
		i++;
	}
	word[i]=0;
	if(sscanf(word,"%x",&b)<=0)
		return;

	//write to addr b
	word+=i+1;
	addr=b;
	while(word[0]!=0)
	{
		hex[0]=word[0];
		if(word[1]==0)
		{
			hex[1]=word[0];
			hex[0]='0';
			word-=1; //hack to make word+2=0;
		}else
		{
			hex[1]=word[1];
		}
		if(sscanf(hex,"%x",&b)>0)
		{
			a=b;
		}else
		{
			a=0;
			if(noisy>=4) printf_P(PSTR("Could not parse \"%s\"\r\n"),hex);
		}
		//printf_P(PSTR("Value: %d\r\n"),a);
		eeprom_write_byte((void*)(addr++),a);
		word+=2;
	}
	printf_P(PSTR("Done writing\r\n"));
}
void console_read(char *word)
{
	uint8_t a;
	int b;
	int length=1;
	a=0;
	while(word[a]!=' ')
	{
		if(word[a]==0) break;
		a++;
	}
	if(word[a]==' ' && word[a+1]!=0)
	{
		if(sscanf(word+a+1,"%d",&b)>0)
			length=b;
		word[a]=0;
	}
	if(sscanf(word,"%x",&b)<=0)
		return;
	printf_P(PSTR("Read(%02x): "),b);
	PORTB&=~(1<<0);
	if(noisy>=8) printf_P(PSTR("CS-\r\n"));
	spi_xfer(b);
	while(length>0)
	{
		a=spi_xfer(0);
		printf_P(PSTR("%02x "),a);
		length--;
	}
	PORTB|=(1<<0);
	if(noisy>=8) printf_P(PSTR("CS+\r\n"));
	printf_P(PSTR("\r\n"),a);
}
void ee_read(char *word)
{
	uint8_t a;
	int i;
	int b;
	int length=1;
	a=0;
	while(word[a]!=' ')
	{
		if(word[a]==0) break;
		a++;
	}
	if(word[a]==' ' && word[a+1]!=0)
	{
		if(sscanf(word+a+1,"%d",&b)>0)
			length=b;
		word[a]=0;
	}
	if(sscanf(word,"%x",&b)<=0)
		return;
	printf_P(PSTR("EERead(%02x): "),b);
	for(i=0;i<length;i++)
	{
		a=eeprom_read_byte((void*)b+i);
		printf_P(PSTR("%02x "),a);
	}
	printf_P(PSTR("\r\n"),a);
}


void console_processline(char *line)
{
	uint8_t a;
	int b;
	char *param;
	char *pa;
	char *pb;
	//char hex[3]="00";
	uint8_t i;
	uint8_t c;
	uint8_t binary[BINARY_COUNT];
	float afloat;
	//printf_P(PSTR("Line: \"%s\"\r\n"),line);
	if( (a=cmdcmp_P(line,PSTR("hello"))) )
	{
		param=line+a;
		if(param[0]!=' ')
		{
			printf_P(PSTR("Hello.\r\n"));
		}else
		{
			printf_P(PSTR("Hello: \"%s\"\r\n"),param+1);
		}
	}else if( (a=cmdcmp_P(line,PSTR("nothing"))) )
	{
		param=line+a;
                if(param[0]!=' ')
                {
                        printf_P(PSTR("Usage\r\n"));
                }else
                {
                        param++;
                }
	}else if( (a=cmdcmp_P(line,PSTR("tx_rate"))) )
	{
		param=line+a;
                if(param[0]==' ')
                {
                        param++;
			if(sscanf(param,"%d",&b)>0) tx_rate_limit=b;
		}
                printf_P(PSTR("tx_rate_limit: %d\r\n"),tx_rate_limit);
	}else if( (a=cmdcmp_P(line,PSTR("showmacs"))) )
	{
		param=line+a;
                if(param[0]!=' ')
                {
                        printf_P(PSTR("Node\tAge\tMac\r\n"));
			for(i=0;i<BRIDGE_TABLE_SIZE;i++)
			{
				printf_P(PSTR("%02x\t%d\t"),bridge_table[i].node,bridge_table[i].age_s);
				for(a=0;a<6;a++)
				{
					c=bridge_table[i].mac[a];
					printf_P(PSTR("%02x"),c);
				}
				printf_P(PSTR("\r\n"));
			}
                }else
                {
                        param++;
                }
	}else if( (a=cmdcmp_P(line,PSTR("status"))) )
	{
		param=line+a;
                if(param[0]!=' ')
                {
                        printf_P(PSTR("rfm_status{\r\n\
tx_data_set: (%d) \r\n\
transmitter_active: (%d) \r\n\
tx_destination: (%02x) \r\n\
tx_sequence: (%d) \r\n\
tx_type: (%02x) \r\n\
sequence: (%d) \r\n\
rx_sequence: (%d) \r\n\
rx_sequence_src: (%d) \r\n\
tx_timeout_ms: (%d) \r\n\
tx_retransmit_count: (%d) \r\n\
tx_ethernet_sequence: (%d) \r\n\
rx_ethernet_src: (%02x) \r\n\
rx_ethernet_sequence: (%d) \r\n\
address: (%02x) \r\n\
} \r\n\
noisy: (%d) \r\n\
count_1khz: (%d) \r\n\
rssi_decimate: (%d) \r\n\
rssi_decimate_count: (%d) \r\n\
rssi: (%d) \r\n\
ethernet_rx_timeout_ms: (%d) \r\n\
FrameIN.FrameLength: (%d) \r\n\
framein_remaining_size: (%d) \r\n\
FrameOUT.FrameLength: (%d) \r\n\
frameout_ready_size: (%d)\r\n"),rfm_status.tx_data_set ,rfm_status.transmitter_active ,rfm_status.tx_destination ,
rfm_status.tx_sequence ,rfm_status.tx_type ,rfm_status.sequence ,rfm_status.rx_sequence ,rfm_status.rx_sequence_src,
rfm_status.tx_timeout_ms ,rfm_status.queue_retransmit_count ,rfm_status.tx_ethernet_sequence ,rfm_status.rx_ethernet_src ,rfm_status.rx_ethernet_sequence ,rfm_status.address, noisy, count_1khz, rssi_decimate, rssi_decimate_count, rssi, ethernet_rx_timeout_ms,
FrameIN.FrameLength, framein_remaining_size, FrameOUT.FrameLength, frameout_ready_size);
                }else
                {
                        param++;
                }
	}else if( (a=cmdcmp_P(line,PSTR("ee_read"))) )
	{
		param=line+a;
                if(param[0]!=' ')
                {
                        printf_P(PSTR("Usage\r\n"));
                }else
                {
                        param++;
			ee_read(param);
                }
	}else if( (a=cmdcmp_P(line,PSTR("ee_write"))) )
	{
		param=line+a;
                if(param[0]!=' ')
                {
                        printf_P(PSTR("Usage\r\n"));
                }else
                {
                        param++;
			ee_write(param);
                }
	}else if( (a=cmdcmp_P(line,PSTR("senda"))) )
	{
		param=line+a;
                if(param[0]!=' ')
                {
                        printf_P(PSTR("Usage: senda destination_hex string...\r\n"));
                }else
                {
                        param++;
			a=0;
			while(param[a]!=' ' && param[a]!=0) a++;
			if(param[a]==' ' && param[a+1]!=0)
			{
				param[a]=0;
				sscanf(param,"%x",&b);
				send_test_string(b,param+a+1);
			}else
			{
				printf_P(PSTR("no data specified\r\n"));
			}
		}
	}else if( (a=cmdcmp_P(line,PSTR("rssi"))) )
	{
		param=line+a;
                if(param[0]!=' ')
                {
			//yeehaw
			afloat=(rssi/((float)256.0));
                        printf_P(PSTR("RSSI: %f (%fdBm)\r\n"), afloat, afloat/2-120);
			afloat=(data_rssi/((float)256.0));
                        printf_P(PSTR("Data RSSI: last %d avg %f (%fdBm)\r\n"), data_rssi_last, afloat, afloat/2-120);
                }else
                {
                        param++;
			sscanf(param,"%f",&afloat);
			printf_P(PSTR("%f is maybe %fdBm\r\n"),afloat,afloat/2-120);
                }
	}else if( (a=cmdcmp_P(line,PSTR("senda_old"))) )
	{
		param=line+a;
                if(param[0]!=' ')
                {
                        printf_P(PSTR("Usage\r\n"));
                }else
                {
                        param++;
			a=strlen(param);
			rfm22b_write(0x3e,a); //packet length
			//clear tx fifo
			rfm22b_sbi(8,0);
			rfm22b_cbi(8,0);

			rfm22b_writem(0x7f,(void*)param,a);
			rfm22b_sbi(7,3); //tx TODO not rx at same time
                }
	}else if( (a=cmdcmp_P(line,PSTR("noisy"))) )
	{
		param=line+a;
                if(param[0]==' ' && param[1]!=0)
                {
                        param++;
			sscanf(param,"%d",&b);
			noisy=b;
                }
		printf_P(PSTR("Debug level: %d\r\n"),noisy);
	}else if( (a=cmdcmp_P(line,PSTR("write"))) )
	{
		param=line+a;
                if(param[0]!=' ')
                {
                        printf_P(PSTR("Usage\r\n"));
                }else
                {
                        param++;
			console_write(param);
                }
	}else if( (a=cmdcmp_P(line,PSTR("read"))) )
	{
		param=line+a;
                if(param[0]!=' ')
                {
                        printf_P(PSTR("Usage\r\n"));
                }else
                {
                        param++;
			console_read(param);
                }
	}else if( (a=cmdcmp_P(line,PSTR("rfm22b_init"))) )
	{
		param=line+a;
                if(param[0]!=' ')
                {
			if(rfm22b_init())
			{
				printf_P(PSTR("Fail\r\n"));
			}else
			{
				printf_P(PSTR("Ok\r\n"));
			}

                }else
                {
                        param++;
                }
	}else if( (a=cmdcmp_P(line,PSTR("rfm22b_reset"))) )
	{
		param=line+a;
                if(param[0]!=' ')
                {
			rfm22b_reset();
                }else
                {
                        param++;
                }
	}else if( (a=cmdcmp_P(line,PSTR("parse"))) )
        {
                param=line+a;
                if(param[0]!=' ')
                {
                        printf_P(PSTR("Usage\r\n"));
                }else
                {
                        param++;
			pa=strtok_r(param," ",&pb);
			c=0;
			while(pa != NULL && c<BINARY_COUNT)
			{
				c+=parse_ascii(pa,binary+c,BINARY_COUNT-c);
				pa=strtok_r(NULL," ",&pb);
			}
			printf_P(PSTR("Result (%d): "),c);
			for(i=0;i<c;i++)
				printf_P(PSTR("%d "),binary[i]);
			printf_P(PSTR("\r\n"),c);
                }
	}else if( (a=cmdcmp_P(line,PSTR("regdump"))) )
	{
		param=line+a;
                if(param[0]!=' ')
                {
                        for(i=0;i<=0x7E;i++)
			{
				//this code is dumb because all the registers can be read at once
				PORTB&=~(1<<0);
				if(noisy>=8) printf_P(PSTR("CS-\r\n"));
				SPDR=i;
				while(!(SPSR & (1<<SPIF)));
				SPDR=0;
				while(!(SPSR & (1<<SPIF)));
				printf_P(PSTR("Add %02x Value %02x\r\n"),i,SPDR);
				PORTB|=(1<<0);
				if(noisy>=8) printf_P(PSTR("CS+\r\n"));
			}
                }else
                {
                        param++;
                }
	}else if( (a=cmdcmp_P(line,PSTR("spi"))) )
        {
                param=line+a;
                if(param[0]!=' ')
                {
                        printf_P(PSTR("Usage\r\n"));
                }else
                {
                        param++;
			pa=strtok_r(param," ",&pb);
			c=0;
			while(pa != NULL && c<BINARY_COUNT)
			{
				c+=parse_ascii(pa,binary+c,BINARY_COUNT-c);
				pa=strtok_r(NULL," ",&pb);
			}
			printf_P(PSTR("Sending (%d): "),c);
			PORTB&=~(1<<0);
			if(noisy>=8) printf_P(PSTR("CS-\r\n"));
			for(i=0;i<c;i++)
			{
				SPDR=binary[i];
				printf_P(PSTR("%x-"),binary[i]);
				while(!(SPSR & (1<<SPIF)));
				printf_P(PSTR("%x "),SPDR);
			}
			PORTB|=(1<<0);
			if(noisy>=8) printf_P(PSTR("CS+\r\n"));
			printf_P(PSTR("\r\n"),c);
                }
	}
}

uint8_t parse_ascii(char *word, uint8_t *store, uint8_t max)
{
	uint8_t count=0;
	int b;
	uint8_t a;
	uint8_t c;
	uint8_t i;
	char hex[3]="00";

	//printf_P(PSTR("parse_ascii(%s,%d,%d)\r\n"),word,store,max);

	if(word[0]=='0' && word[1]=='x' && word[2]!=0)
	{
		word+=2;
		while(word[0]!=0)
		{
			hex[0]=word[0];
			if(word[1]==0)
			{
				hex[1]=word[0];
				hex[0]='0';
				word-=1; //hack to make word+2=0;
			}else
			{
				hex[1]=word[1];
			}
			if(sscanf(hex,"%x",&b)>0)
			{
				a=b;
			}else
			{
				a=0;
				if(noisy>=4) printf_P(PSTR("Could not parse \"%s\"\r\n"),hex);
			}
			//printf_P(PSTR("Value: %d\r\n"),a);
			store[count++]=a;
			if(count>=max) return count;
			word+=2;
		}
	}else if(word[0]=='0' && word[1]=='b' && word[2]!=0)
	{
		word+=2;
		c=0;
		while(1)
		{
			a=0;
			for(i=0;i<8;i++)
			{
				//printf_P(PSTR("B(%d,%x) "),i,word[i]);
				if(word[i]==0)
				{
					c=1;
				 	break;
				}
				if(word[i]!='0') a|=1<<(7-i);
				//a |= (word[i]?1:0) << (7-i);
			}
			if(c==1) break;
			word+=8;
			//printf_P(PSTR("Value: %d\r\n"),a);
			store[count++]=a;
			if(count>=max) return count;
		}
	}else if(word[0]=='0' && word[1]=='a' && word[2]!=0)
	{
		word+=2;
		while(*word!=0)
		{
			//printf_P(PSTR("Value: %d\r\n"),*word);
			store[count++]=*word;
			if(count>=max) return count;
			word++;
		}
	}else
	{
		if(sscanf(word,"%d",&b)>0)
		{
			a=b;
		}else
		{
			a=0;
			if(noisy>=4) printf_P(PSTR("Could not parse \"%s\"\r\n"),word);
			}
		//printf_P(PSTR("Value: %d\r\n"),a);
		store[count++]=a;
		if(count>=max) return count;
	}
	return count;
}

/** Configures the board hardware and chip peripherals for the demo's functionality. */
void SetupHardware(void)
{
	/* Disable watchdog if enabled by bootloader/fuses */
	MCUSR &= ~(1 << WDRF);
	wdt_disable();

	/* Disable clock division */
	clock_prescale_set(clock_div_2);

/*	DDRD|=(1<<6);
	PORTD|=(1<<6);
	_delay_ms(1000);
	PORTD&=~(1<<6);
	_delay_ms(1000);
	PORTD|=(1<<6);
	_delay_ms(200);
	PORTD&=~(1<<6);
	_delay_ms(200);
*/
	DDRA=0;
	PORTA=0xFF;
	DDRB=(1<<0)|(1<<1)|(1<<2)|(1<<4); //spi, reset
	PORTB=(1<<0)|(1<<4)|(1<<5)|(1<<6)|(1<<7);
	DDRC=0;
	PORTC=0xFF;
	DDRD=0;
	PORTD=0b01111111;
	DDRE=0;
	PORTE=0xFF;
	DDRF=0;
	PORTF=0xFF;

	SPCR=(1<<SPE)|(1<<MSTR);
	//SPCR=(1<<SPE)|(1<<MSTR)|(1<<SPR1)|(1<<SPR0);
	SPSR=(1<<SPI2X);

	OCR0A=124; // /125
	TCCR0A=(1<<WGM01); //ctc for OCR0A
	TCCR0B=(1<<CS01)|(1<<CS00); // /64
	TIMSK0=(1<<OCIE0A); //compare match interrupt

	rfm_status.address=eeprom_read_byte((void*)0x10);
	eeprom_read_block(&Ethernet_RNDIS_Interface.Config.AdapterMACAddress,(const void*)0x11,6);

	/* Hardware Initialization */
	LEDs_Init();
	//Serial_Init(9600, false);
	USB_Init();

	/* Create a stdio stream for the serial port for stdin and stdout */
	//Serial_CreateStream(NULL);
	CDC_Device_CreateStream(&VirtualSerial2_CDC_Interface, &USBSerialStream);
	stdin=stdout=stderr=&USBSerialStream;

	//printf_P(PSTR("Hello, World!\r\n"));

	rfm22b_reset();
	//if(rfm22b_init())
	//{
	//	printf_P(PSTR("rfm22b init failure\r\n"));
	//}
}

/** Event handler for the library USB Connection event. */
void EVENT_USB_Device_Connect(void)
{
	LEDs_SetAllLEDs(LEDMASK_USB_ENUMERATING);
}

/** Event handler for the library USB Disconnection event. */
void EVENT_USB_Device_Disconnect(void)
{
	LEDs_SetAllLEDs(LEDMASK_USB_NOTREADY);
}

/** Event handler for the library USB Configuration Changed event. */
void EVENT_USB_Device_ConfigurationChanged(void)
{
	bool ConfigSuccess = true;

	ConfigSuccess &= RNDIS_Device_ConfigureEndpoints(&Ethernet_RNDIS_Interface);
	ConfigSuccess &= CDC_Device_ConfigureEndpoints(&VirtualSerial2_CDC_Interface);

	LEDs_SetAllLEDs(ConfigSuccess ? LEDMASK_USB_READY : LEDMASK_USB_ERROR);
}

/** Event handler for the library USB Control Request reception event. */
void EVENT_USB_Device_ControlRequest(void)
{
	RNDIS_Device_ProcessControlRequest(&Ethernet_RNDIS_Interface);
	CDC_Device_ProcessControlRequest(&VirtualSerial2_CDC_Interface);
}

void rfm22b_reset()
{
	PORTB|=(1<<4); //hardware reset
	_delay_ms(16);
	PORTB&=~(1<<4);
	_delay_ms(16);
	rfm22b_write(0x07,0x80); //software reset
	_delay_ms(16);
}

int8_t rfm22b_init()
{
	uint8_t i;
	uint8_t addr;
	uint8_t val;
	uint8_t a;

	i=0;
	if(rfm22b_read(0)!=0x08 || rfm22b_read(1)!=0x06)
	{
		if(noisy>=2) printf_P(PSTR("communication error\r\n"));
		return 3;
	}
	/*i=0;
	while(!(rfm22b_read(0x04)&(1<<1)))
	{
		i++;
		if(noisy>=2) printf_P(PSTR("chip not ready\r\n"));
		_delay_ms(100);
		if(i>5) return 2;
	}*/
	//_delay_ms(16); //todo
	for(i=0;i<RFM22B_REGS_COUNT;i++)
	{
		addr=pgm_read_byte(&rfm22b_regs[i*2]);
		val=pgm_read_byte(&rfm22b_regs[i*2+1]);
		if(noisy>=7) printf_P(PSTR("num %d addr %02x val %02x\r\n"),i,addr,val);
		rfm22b_writem(addr,&val,1);
		rfm22b_readm(addr,&a,1);
		if(a!=val)
		{
			printf_P(PSTR("error writing register\r\n"));
			return 1;
		}
	}
	rfm22b_write(0x3f,rfm_status.address); //set packet filter to receive unicast for node
	rfm22b_write(0x3b,rfm_status.address); //set default packet source address
	rfm22b_sbi(7,2); //enable rx
	return 0;
}

void rfm22b_writem(uint8_t reg, uint8_t *data, uint8_t count)
{
	uint8_t i;
	if(noisy>=8) printf_P(PSTR("rfm22b_writem(%02x,%d,%d)\r\n"),reg,data,count);
	PORTB&=~(1<<0);
	if(noisy>=8) printf_P(PSTR("CS-\r\n"));
	spi_xfer(reg | (1<<7));
	for(i=0;i<count;i++)
		spi_xfer(data[i]);
	PORTB|=(1<<0);
	if(noisy>=8) printf_P(PSTR("CS+\r\n"));
}

void rfm22b_readm(uint8_t reg, uint8_t *data, uint8_t count)
{
	uint8_t i;
	if(noisy>=8) printf_P(PSTR("rfm22b_readm(%02x,%d,%d)\r\n"),reg,data,count);
        PORTB&=~(1<<0);
	if(noisy>=8) printf_P(PSTR("CS-\r\n"));
        spi_xfer(reg);
        for(i=0;i<count;i++)
                data[i]=spi_xfer(0);
        PORTB|=(1<<0);
	if(noisy>=8) printf_P(PSTR("CS+\r\n"));
}

uint8_t rfm22b_read(uint8_t reg)
{
	uint8_t a;
	if(noisy>=8) printf_P(PSTR("rfm22b_read(%02x)\r\n"),reg);
        PORTB&=~(1<<0);
	if(noisy>=8) printf_P(PSTR("CS-\r\n"));
        spi_xfer(reg);
	a=spi_xfer(0);
        PORTB|=(1<<0);
	if(noisy>=8) printf_P(PSTR("CS+\r\n"));
	return a;
}

void rfm22b_write(uint8_t reg, uint8_t data)
{
	if(noisy>=8) printf_P(PSTR("rfm22b_write(%02x,%02x)\r\n"),reg,data);
	PORTB&=~(1<<0);
	if(noisy>=8) printf_P(PSTR("CS-\r\n"));
	spi_xfer(reg | (1<<7));
	spi_xfer(data);
	PORTB|=(1<<0);
	if(noisy>=8) printf_P(PSTR("CS+\r\n"));
}

uint8_t spi_xfer(uint8_t send)
{
	if(noisy>=8) printf_P(PSTR("spi_xfer(%02x)..."),send);
	//while(!(SPSR & (1<<SPIF)));
	SPDR=send;
	while(!(SPSR & (1<<SPIF)));
	if(noisy>=8) printf_P(PSTR("%02x\r\n"),SPDR);
	return SPDR;
}

void console_break()
{
	printf_P(PSTR("Break\r\n"));
}

void rfm22b_interrupt()
{
	uint8_t status,flags1,flags2;
	uint8_t a;
	status=rfm22b_read(2);
	flags1=rfm22b_read(3);
	flags2=rfm22b_read(4);
	rf_packet packet;
	uint8_t data_length=0;
	if(noisy>=7) printf_P(PSTR("Interrupt status: %02x %02x %02x\r\n"),status,flags1,flags2);
	if(flags2&(1<<0))
	{
		if(noisy>=3) printf_P(PSTR("Power on, running init\r\n"));
		rfm22b_init();
		return;
	}
	if(status&0xC0 || flags1&(1<<7))
	{
		if(noisy>=1) printf_P(PSTR("FIFO%s%s error, resetting fifos\r\n"),(status&(1<<7))?" overflow":"",(status&(1<<6))?" underflow":"");
		a=rfm22b_read(8);
		a|=0x03;
		rfm22b_write(8,a);
		a&=~(0x03);
		rfm22b_write(8,a);
	}
	if(flags1&(1<<2))
	{
		if(noisy>=6) printf_P(PSTR("Packet Sent\r\n"));
		rfm_status.transmitter_active=0;
		rfm_status.tx_data_set=0; //don't modify tx registers while transmitting, TODO test
		if(!rfm_status.tx_data_is_queue_packet)
		{
			rfm_status.immediate_packet_set=0;
		}
		tx_rate_limit_ms=TX_RATE_LIMIT;
		rfm22b_sbi(7,2); //enable rx
	}
	if(flags1&(1<<1))
	{
		rfm22b_cbi(7,2); //rx off
		if(noisy>=6) printf_P(PSTR("Packet Received\r\n"));
		data_rssi_last=preamble_rssi;
		data_rssi_decimate+=preamble_rssi;
		data_rssi_count++;
		if(data_rssi_count==255)
		{
			data_rssi=data_rssi_decimate;
			data_rssi_decimate=0;
		}
		//while( !(rfm22b_read(2)&(1<<5)) )
		if( !(rfm22b_read(2)&(1<<5)) ) //rx fifo not empty
		{
			rfm22b_readm(0x7f,(void*)&packet,5);
			if(packet.length>data_length)
			{
				if(packet.length>MAX_PACKET_LENGTH)
				{
					if(noisy>=2) printf_P(PSTR("Length invalid\r\n"));
					//clear rx because we won't be reading it
					//rfm22b_sbi(8,1);
					//rfm22b_cbi(8,1);

				}else
				{
					packet.data=alloca(packet.length); //two packets recieved is bad
					data_length=packet.length;
				}
			}
			if(packet.length<=data_length) //don't continue on failure from above
			{
				if(packet.length) rfm22b_readm(0x7f,packet.data,packet.length);
				if(noisy>=6)
				{
					printf_P(PSTR("Header: To %02x From %02x type %02x seq %02x Len %d\r\n"),
					packet.destination, packet.source, packet.type, packet.sequence, packet.length);
					if(packet.length>0) printf_P(PSTR("Data: "));
					for(a=0;a<packet.length;a++)
					{
						printf_P(PSTR("%02x"),packet.data[a]);
					}
					if(packet.length>0) printf_P(PSTR("\r\n"));
				}
				process_rx_packet(packet);
			}
		}
		//clear rx to avoid offset errors
		rfm22b_sbi(8,1);
		rfm22b_cbi(8,1);
		if(!rfm_status.transmitter_active) rfm22b_sbi(7,2); //rx on
	}
	if(flags1&(1<<0))
	{
		if(noisy>=3) printf_P(PSTR("CRC Error\r\n"));
	}
	if(flags2&(1<<7))
	{
		a=rfm22b_read(0x26);
		if(noisy>=6) printf_P(PSTR("Sync RSSI: %d\r\n"),a);
	}
	if(flags2&(1<<6))
	{
		a=rfm22b_read(0x26);
		if(noisy>=7) printf_P(PSTR("Preamble RSSI: %d\r\n"),a);
		preamble_rssi=a;
	}
}
inline void rfm22b_cbi(uint8_t reg, uint8_t bv)
{
	rfm22b_cb(reg,(1<<bv));
}
inline void rfm22b_sbi(uint8_t reg, uint8_t bv)
{
	rfm22b_sb(reg,(1<<bv));
}
void rfm22b_cb(uint8_t reg, uint8_t mask)
{
	uint8_t a;
	a=rfm22b_read(reg);
	a&=~mask;
	rfm22b_write(reg,a);
}
void rfm22b_sb(uint8_t reg, uint8_t mask)
{
	uint8_t a;
	a=rfm22b_read(reg);
	a|=mask;
	rfm22b_write(reg,a);
}

ISR(TIMER0_COMPA_vect)
{
	count_1khz++;
}

void tick_1khz(void)
{
	static uint16_t count_1hz;
	//TODO don't randomly sample rssi for noise level indication
	uint8_t a;
	//don't update when transmitting or when receiving a packet
	if(!rfm_status.transmitter_active && !(rfm22b_read(4)&(1<<7)) )
	{
		a=rfm22b_read(0x26);
		rssi_decimate+=a;
		rssi_decimate_count++;
		if(rssi_decimate_count==255)
		{
			rssi=rssi_decimate;
			rssi_decimate=0;
			rfm22b_write(0x27,(rssi>>8)+CCA_SNR); //cca threshold
		}
	}
	if(rfm_status.tx_timeout_ms>0) rfm_status.tx_timeout_ms--;
	if(rfm_status.queue_timeout_ms>0) rfm_status.queue_timeout_ms--;
	if(ethernet_rx_timeout_ms>0) ethernet_rx_timeout_ms--;
	if(tx_rate_limit_ms>0) tx_rate_limit_ms--;

	count_1hz++;
	if(count_1hz>=1000)
	{
		count_1hz-=1000;
		tick_1hz();
	}
}
void tick_1hz(void)
{
	if(noisy>=8) printf_P(PSTR("Tick\r\n"));
	bridge_ageing();
}

void process_rx_packet(rf_packet packet)
{
	int i;
	uint8_t broadcast=1;
	char *pa;
	if(packet.destination==0xFF)
	{
		broadcast=1;
	}else if(packet.destination==rfm_status.address)
	{
		broadcast=0;
	}else
	{
		if(noisy>=4) printf_P(PSTR("Unhandled packet destination\r\n"));
	}
	if((packet.type&0xF0)==(1<<4)) //test ascii
	{
		if((packet.type&0x0F)==1)
		{
			if(!broadcast) send_ack(packet);
			if(packet.sequence==rfm_status.rx_sequence && packet.source==rfm_status.rx_sequence_src)
			{
				if(noisy>=4) printf_P(PSTR("Ignoring already received packet\r\n"));
			}else
			{
				rfm_status.rx_sequence=packet.sequence;
				rfm_status.rx_sequence_src=packet.source;
				printf_P(PSTR("Test ascii from %02x: \""),packet.source);
				for(i=0;i<packet.length;i++)
				{
					putchar(packet.data[i]);
				}
				printf_P(PSTR("\"\r\n"));
				if(cmdcmp_P(packet.data,PSTR("ping")))
				{
					pa=alloca(48);
					//"pong rssi xxx avg xxx.xx noise xxx.xx"
					sprintf_P(pa,PSTR("pong rssi %d avg %.2f noise %.2f"),data_rssi_last,(data_rssi/((float)256.0)),(rssi/((float)256.0)));
					send_test_string(packet.source,pa);
				}
			}
		}else if((packet.type&0x0F)==2)
		{
			if(rfm_status.queue_packet_set) //this can clear data before it's sent in impossible situations
			{
				if(packet.destination==queue_packet.source &&
					(packet.type&0xF0)==(queue_packet.type&0xF0) &&
					packet.source==queue_packet.destination &&
					packet.sequence==queue_packet.sequence)
				{
					rfm_status.queue_packet_set=0;
					if(noisy>=6) printf_P(PSTR("Ack received\r\n"));
				}else
				{
					if(noisy>=4) printf_P(PSTR("Unmatching ack\r\n"));
				}
			}else
			{
				if(noisy>=4) printf_P(PSTR("Suprious ack\r\n"));
			}
		}else
		{
			if(noisy>=3) printf_P(PSTR("Unknown packet control type\r\n"));
		}
	}else if((packet.type&0xF0)==(2<<4)) //ethernet
	{
		if((packet.type&0x0F)==0x01)
		{
			if(rfm_status.rx_ethernet_src==0 && packet.sequence==0)
			{
				rfm_status.rx_ethernet_src=packet.source;
				rfm_status.rx_ethernet_sequence=packet.sequence;
				frameout_ready_size=0;
			}
			if(packet.source==rfm_status.rx_ethernet_src)
			{
				if(packet.sequence==0 && sizeof(FrameOUT.FrameLength)==packet.length)
				{
					//FrameOUT.FrameLength=*(packet.data); //todo network byte order
					memcpy(&(FrameOUT.FrameLength),packet.data,packet.length); //todo network byte order
					ethernet_rx_timeout_ms=ETHERNET_RX_TIMEOUT;
					if(!broadcast) send_ack(packet);
					if(noisy>=6) printf_P(PSTR("New rx ethernet frame from %02x len %d\r\n"),packet.source,FrameOUT.FrameLength);
				}else if(packet.sequence<=rfm_status.rx_ethernet_sequence+1)
				{
					//if((packet.type&0x0F)==1 && packet.destination==rfm_status.address) send_ack(packet);
					if(packet.sequence==(rfm_status.rx_ethernet_sequence+1))
					{
						rfm_status.rx_ethernet_sequence=packet.sequence;
						if(packet.length+frameout_ready_size<=FrameOUT.FrameLength)
						{
							memcpy((FrameOUT.FrameData+frameout_ready_size),packet.data,packet.length);
							frameout_ready_size+=packet.length;
							ethernet_rx_timeout_ms=ETHERNET_RX_TIMEOUT;
							if(!broadcast) send_ack(packet);
						}else
						{
							if(noisy>=3) printf_P(PSTR("Oversized ethernet frame pktlen %d ready %d framelen %d\r\n"),packet.length,frameout_ready_size,FrameOUT.FrameLength);
							rfm_status.rx_ethernet_src=0;
							if(!broadcast) send_nak(packet);
						}
					}else
					{
						if(noisy>=6) printf_P(PSTR("Duplicate ethernet packet\r\n"));
						if(!broadcast) send_ack(packet);
					}
				}else
				{
					if(noisy>=5) printf_P(PSTR("Missed ethernet packet, cancelling reception\r\n"));
					if(!broadcast) send_nak(packet);
					rfm_status.rx_ethernet_src=0;
				}
			}else
			{
				if(!broadcast) send_nak(packet);
				if(noisy>=5) printf_P(PSTR("Cannot process more than one ethernet packet at once rfm_status.rx_ethernet_src=%02x packet.source:%02x packet.sequence:%d\r\n"),rfm_status.rx_ethernet_src,packet.source,packet.sequence);
			}
		}else if((packet.type&0x0F)==0x02)
		{
                      	if(packet.destination==queue_packet.source &&
                        	(packet.type&0xF0)==(queue_packet.type&0xF0) &&
                                packet.source==queue_packet.destination &&
                        	packet.sequence==queue_packet.sequence)
                        {
	        		rfm_status.queue_packet_set=0;
        			if(noisy>=6) printf_P(PSTR("Ethernet ack received\r\n"));
			}else
			{
				if(noisy>=4) printf_P(PSTR("Ethernet suprious ack\r\n"));
			}
		}else if((packet.type&0x0F)==0x03)
		{
			rfm_status.tx_ethernet_sequence=0;
			rfm_status.tx_ethernet_retransmit_count++;
			if(rfm_status.tx_ethernet_retransmit_count>5)
			{
				framein_remaining_size=0;
				if(noisy>=4) printf_P(PSTR("NAK received, giving up on ethernet frame\r\n"));
			}else
			{
				framein_remaining_size=FrameIN.FrameLength;
				if(noisy>=5) printf_P(PSTR("NAK received, resending ethernet frame\r\n"));
			}
		}else
		{
			if(noisy>=3) printf_P(PSTR("Unknown ethernet control type\r\n"));
		}
	}else
	{
		if(noisy>=3) printf_P(PSTR("Unknown packet data type\r\n"));
	}
}

void rfm_transmit()
{
	/*pre: tx_data_set, !transmitter_active
	wait for cca
	transmit preset data

	sent interrupt will unset tx data
	rx data will unset queue data
	*/
	if( (rfm22b_read(4)&(1<<7)) )
	{
		if(noisy>=5) printf_P(PSTR("Not transmitting while receiving\r\n"));
		return;
	}
	uint8_t drssi=rfm22b_read(4)&(1<<4);
	//broadcast or non-ack-data
	//uint8_t transmit_once=(rfm_status.tx_destination==0xFF || (rfm_status.tx_type&0x0F)!=1 );

	if(noisy>=6) printf_P(
	PSTR("rfm_transmit drssi=%d, retx %d, cca_retry %d, Packet(D %02x T %02x S %02x)\r\n"),
	drssi,rfm_status.queue_retransmit_count,rfm_status.tx_cca_failure_count,rfm_status.tx_destination,rfm_status.tx_type,rfm_status.tx_sequence);

/*	if(rfm_status.tx_retransmit_count>5)
	{
		rfm_status.tx_data_set=0;
		rfm_status.tx_retransmit_count=0;
		rfm_status.tx_timeout_ms=0;
		rfm_status.tx_cca_failure_count=0;
		if(noisy>=3) printf_P(PSTR("Tx retransmit failure, dropping packet\r\n"));
	}*/
	if(rfm_status.tx_cca_failure_count>15)
	{
		drssi=0;
		if(noisy>=3) printf_P(PSTR("Sending packet without regard for clear channel\r\n"));
	}

	if(drssi) //rssi>cca
	{
		rfm_status.tx_timeout_ms=2; //todo random
		rfm_status.tx_cca_failure_count++;
	}else
	{
		rfm_status.tx_cca_failure_count=0;
		rfm22b_cbi(7,2); //rx off
		rfm22b_sbi(7,3); //tx on
		rfm_status.transmitter_active=1;
//		if(transmit_once)
//		{
/*			rfm_status.tx_data_set=0;
			rfm_status.tx_timeout_ms=0;
			rfm_status.tx_retransmit_count=0;*/
/*		}else
		{
			rfm_status.tx_timeout_ms=100; //todo random
			rfm_status.tx_retransmit_count++;
		}*/
	}
}

int8_t transmit_packet(rf_packet packet)
{
	uint8_t i;
/*	if(rfm_status.tx_data_set || rfm_status.transmitter_active) //wait to be safe as a test
	{
		if(noisy>=2)
		{
			printf_P(PSTR("Attempt to send data while sending data\r\n"));

		}
		return 1;
	}*/
	if(packet.length>MAX_PACKET_LENGTH)
	{
		if(noisy>=2) printf_P(PSTR("Attempt to send oversized packet\r\n"));
		return 2;
	}
	if(noisy>=6)
	{
		printf_P(PSTR("Setting tx packet dest %02x (src %02x/%02x) type %02x sequence %02x length %02x data: ( "),
			packet.destination,packet.source,rfm_status.address,packet.type,packet.sequence,packet.length);
		for(i=0;i<packet.length;i++)
		{
			printf_P(PSTR("%02x "),packet.data[i]);
		}
		printf_P(PSTR(")\r\n"));
	}
	if((packet.type&0x0F)==0x02 || (packet.type&0x0F)==0x03 ) //TODO not hardcoded selection
	{
		if(packet.length>0)
		{
			if(noisy>=3) printf_P(PSTR("Attempt to send control packet with data\r\n"));
			return 3;
		}else
		{
			//can overwrite older send attempts
			immediate_packet=packet;
			rfm_status.immediate_packet_set=1;
		}
	}else
	{
		if(rfm_status.queue_packet_set)
		{
			if(noisy>=3) printf_P(PSTR("Attempt to send multiple data packets simultaneously\r\n"));
			return 4;
		}else
		{
			queue_packet=packet;
			queue_packet.data=queue_packet_data;
			memcpy(queue_packet_data,packet.data,packet.length);
			rfm_status.queue_packet_set=1;
			rfm_status.queue_timeout_ms=0;
			if((packet.type&0x0F)==0x01 && packet.destination!=0xFF)
			{
				rfm_status.queue_retransmit_count=5;
			}else
			{
				rfm_status.queue_retransmit_count=1;
			}
		}
	}
/*	rfm_status.tx_destination=packet.destination;
	rfm22b_write(0x3a,packet.destination);
	//3B source
	rfm_status.tx_type=packet.type;
	rfm22b_write(0x3c,packet.type);
	rfm_status.tx_sequence=packet.sequence;
	rfm22b_write(0x3d,packet.sequence);
	rfm22b_write(0x3e,packet.length);
	rfm22b_sbi(8,0); //clear tx buffer
	rfm22b_cbi(8,0);
	if(packet.length)
	{
		rfm22b_writem(0x7F,packet.data,packet.length);
	}
	rfm_status.tx_data_set=1;
	rfm_status.tx_retransmit_count=0;*/
	return 0;
}
void set_tx_data()
{
	rf_packet packet;
	if(rfm_status.immediate_packet_set)
	{
		packet=immediate_packet;
		rfm_status.tx_data_is_queue_packet=0;
	}else if(rfm_status.queue_packet_set)
	{
		packet=queue_packet;
		if(rfm_status.queue_retransmit_count==0)
		{
			rfm_status.queue_packet_set=0;
			return;
		}else if(rfm_status.queue_timeout_ms>0)
		{
			return;
		}
		rfm_status.queue_retransmit_count--;
		rfm_status.queue_timeout_ms=200;
		rfm_status.tx_data_is_queue_packet=1;
	}else
	{
		return;
	}
	rfm_status.tx_destination=packet.destination;
	rfm22b_write(0x3a,packet.destination);
	//3B source
	rfm_status.tx_type=packet.type;
	rfm22b_write(0x3c,packet.type);
	rfm_status.tx_sequence=packet.sequence;
	rfm22b_write(0x3d,packet.sequence);
	rfm22b_write(0x3e,packet.length);
	rfm22b_sbi(8,0); //clear tx buffer
	rfm22b_cbi(8,0);
	if(packet.length)
	{
		rfm22b_writem(0x7F,packet.data,packet.length);
	}
	rfm_status.tx_data_set=1;
	rfm_status.tx_cca_failure_count=0;
}

void send_test_string(uint8_t destination, char *data)
{
	rf_packet packet;
	packet.destination=destination;
	packet.source=rfm_status.address;
	packet.type=0x11;
	packet.sequence=(rfm_status.sequence++);
	packet.length=strlen(data);
	packet.data=(void*)data;
	transmit_packet(packet);
}
void send_ethernet(Ethernet_Frame_Info_t *frame)
{
	rf_packet packet;
	uint8_t *data=frame->FrameData;
	uint16_t length=frame->FrameLength;

	packet.destination=bridge_lookup(frame);
        packet.source=rfm_status.address;
        packet.type=0x21;
        packet.sequence=rfm_status.tx_ethernet_sequence;
	if(rfm_status.tx_ethernet_sequence==0)
	{
		packet.length=sizeof(length);
		packet.data=(void*)&length; //todo network byte order
	}else
	{
		if(framein_remaining_size>MAX_PACKET_LENGTH)
		{
			packet.length=MAX_PACKET_LENGTH;
		}else
		{
			packet.length=framein_remaining_size;
		}
		packet.data=data+(length-framein_remaining_size);
		framein_remaining_size-=packet.length;
	}
	transmit_packet(packet);
	rfm_status.tx_ethernet_sequence++;
}

void send_ack(rf_packet packet)
{
	rf_packet newpacket;
	if((packet.type&0x0F)!=0x01)
	{
		if(noisy>=2) printf_P(PSTR("Attempt to send ack for no-ack packet\r\n"));
		return;
	}
	if(packet.destination==0xFF)
	{
		if(noisy>=2) printf_P(PSTR("Attempt to send ack for broadcast packet\r\n"));
		return;
	}
	newpacket.destination=packet.source;
	newpacket.source=rfm_status.address;
	newpacket.type=(packet.type&0xF0)|2;
	newpacket.sequence=packet.sequence;
	newpacket.length=0;
	transmit_packet(newpacket);
}

void send_nak(rf_packet packet)
{
	rf_packet newpacket;
	if((packet.type&0x0F)!=0x01)
	{
		if(noisy>=2) printf_P(PSTR("Attempt to send nak for no-ack packet\r\n"));
		return;
	}
	if(packet.destination==0xFF)
	{
		if(noisy>=2) printf_P(PSTR("Attempt to send nak for broadcast packet\r\n"));
		return;
	}
	newpacket.destination=packet.source;
	newpacket.source=rfm_status.address;
	newpacket.type=(packet.type&0xF0)|3;
	newpacket.sequence=packet.sequence;
	newpacket.length=0;
	transmit_packet(newpacket);
}
void bridge_learn(Ethernet_Frame_Info_t *frame, uint8_t srcnode)
{
	uint8_t i;
	uint8_t oldest_age,oldest_index;
	//Ethernet_Frame_Header_t* frameheader = (Ethernet_Frame_Header_t*)(frame->FrameData);
	uint8_t *data=frame->FrameData;

	//find this address
	for(i=0;i<BRIDGE_TABLE_SIZE;i++)
	{
		if(memcmp(data+6,bridge_table[i].mac,6)==0) break;
	}
	//find a blank entry
	if(i>=BRIDGE_TABLE_SIZE)
	{
		for(i=0;i<BRIDGE_TABLE_SIZE;i++)
		{
			if(bridge_table[i].node==0) break;
		}
	}
	//find the oldest entry
	if(i>=BRIDGE_TABLE_SIZE)
	{
		oldest_age=0;
		oldest_index=0;
		for(i=0;i<BRIDGE_TABLE_SIZE;i++)
		{
			if(bridge_table[i].age_s>oldest_age)
			{
				oldest_index=i;
				oldest_age=bridge_table[i].age_s;
			}
		}
		i=oldest_index;
	}
	//bridge_table[i].mac=frameheader->Source;
	memcpy(bridge_table[i].mac,data+6,6);
	bridge_table[i].node=srcnode;
	bridge_table[i].age_s=0;
}
uint8_t bridge_lookup(Ethernet_Frame_Info_t *frame)
{
	uint8_t i;
	uint8_t a;
	//Ethernet_Frame_Header_t* frameheader = (Ethernet_Frame_Header_t*)(frame->FrameData);
	uint8_t *data=frame->FrameData;
	if(noisy>=6)
	{
		printf_P(PSTR("bridge_lookup dest "));
		for(i=0;i<6;i++)
		{
			a=data[i];
			printf_P(PSTR("%02x"),a);
		}
	}
	for(i=0;i<BRIDGE_TABLE_SIZE;i++)
	{
		if( bridge_table[i].node!=0 && memcmp(data,bridge_table[i].mac,6)==0 )
		{
			if(noisy>=6) printf_P(PSTR(" table %d node %d\r\n"),i,bridge_table[i].node);
			return bridge_table[i].node;
		}
	}
	if(noisy>=6) printf_P(PSTR(" failure\r\n"));
	return 0xFF;
}
void bridge_ageing()
{
	uint8_t i;
	for(i=0;i<BRIDGE_TABLE_SIZE;i++)
	{
		if(bridge_table[i].age_s<255)
		{
			bridge_table[i].age_s++;
			if(bridge_table[i].age_s>=BRIDGE_MAX_AGE)
			{
				bridge_table[i].node=0;
			}
		}
	}
}
void print_ethernet_frame(Ethernet_Frame_Info_t *frame)
{
	uint16_t i;
	uint8_t *data=frame->FrameData;
	printf_P(PSTR("Frame (%d) ["),frame->FrameLength);
	for(i=0;i < frame->FrameLength;i++)
	{
		printf_P(PSTR("%02x"),data[i]);
	}
	printf_P(PSTR("]\r\n"));
}