/*! \file doc.c
 \brief Distributed On-Demand Cooperation
 
 @version 15
 @author Chris Hunter and Patrick Murphy
 
 
 */
#include "warpmac.h"
#include "warpnet.h"
#include "warpphy.h"
#include "docMac.h"
#include "xparameters.h"
#include "ofdm_txrx_mimo_regMacros.h"
#include "string.h"
#include "errno.h"
#include "stdlib.h"
#include "stdio.h"
#include "ascii_characters.h"
#include "warp_userioboard_util.h"
#include "radio_controller_ext.h"
#include "xtime_l.h"



#define PERLOW	80000
#define PERHIGH	60000
#define DUTYLOW 13107
#define DUTYHIGH 13107

#define htons(A) ((((Xuint16)(A) & 0xff00) >> 8) | (((Xuint16)(A) & 0x00ff) << 8))
#define MAX_NUMNODES 3

#define REQ_DST_ME PHY_AUTORESPONSE_REQ_MATCH0
#define REQ_RLY_ME PHY_AUTORESPONSE_REQ_MATCH1
#define REQ_DATA PHY_AUTORESPONSE_REQ_MATCH2
#define REQ_NACK PHY_AUTORESPONSE_REQ_MATCH3

#define NUM_RX_PKTBUFS 2



unsigned char randomData_Mode;

Macframe templatePkt;
unsigned int autoResp_matchCond;
unsigned int autoResp_action;
unsigned int autoResp_action_reTx;
unsigned int autoResp_action_coopTx;
unsigned short autoRespDelay_NACKTx, autoRespDelay_AFTx, autoRespDelay_ReTx, autoRespDelay_ACKTx;
unsigned int docPreambleScaling,docPayloadScaling;
unsigned int txPower;

unsigned char relayDestID;
unsigned int relayLen;
unsigned char alternating;
unsigned char currentAlt;

XTime startTime;
XTime stopTime;

unsigned char pktBuf_tx_ACK;
unsigned char pktBuf_tx_NACK;
unsigned char pktBuf_tx_DATA;
unsigned char pktBuf_rx_emac;
unsigned char pktBuf_tx_AF;
unsigned char pktBuf_rx;
unsigned char rxPktBufs[NUM_RX_PKTBUFS];
unsigned char pktBuf_rx_ind;
unsigned char respondToNacks;

unsigned char relayMode;
unsigned short relayMode_lastSrcAddr;

unsigned char enableStats;
int pktGen_length;
int pktGen_period;
unsigned char enablePktGen;
unsigned char allowRelay;

unsigned short rx_correThresh;
unsigned char cfoDly;
unsigned char ignoreSeqNums;
unsigned char cfoVar;
unsigned char agcPrintMode;
unsigned int pktDet_rssi;
unsigned int agcGains;
unsigned char antB_preambleShift;
unsigned char debug_sisoMode;
unsigned char userIOBoard_LEDs;
unsigned char charsToPrint[16];
unsigned int pktCount_good, pktCount_bad;

unsigned short int rxSequences[MAX_NUMNODES];
unsigned short int txSequences[MAX_NUMNODES];

unsigned char max_numTransmissions;

///Index to the routing table that identifies this node
unsigned short int myID;

///Full rate modulation selection; QPSK by default
unsigned int pktFullRate;

//Payload code rate selection; must be CODE_RATE_NONE for uncoded PHY
unsigned int pktCodeRate;

#define NUMMODES 2

unsigned int stats_numDataTx[MAX_NUMNODES][MAX_NUMNODES][NUMMODES];
unsigned int stats_numNACKTx[MAX_NUMNODES][MAX_NUMNODES][NUMMODES];
unsigned int stats_numDataRx[MAX_NUMNODES][MAX_NUMNODES][NUMMODES];
unsigned int stats_numNACKRx[MAX_NUMNODES][MAX_NUMNODES][NUMMODES];
unsigned int stats_sumGains[MAX_NUMNODES][MAX_NUMNODES][NUMMODES];
unsigned int stats_sumPacketsRx[MAX_NUMNODES][MAX_NUMNODES][NUMMODES];
unsigned int stats_sumRSSI[MAX_NUMNODES][MAX_NUMNODES][NUMMODES];
unsigned int stats_numBadHeaderRx;

///Buffer for holding a packet-to-xmit across multiple retransmissions
Macframe txBuffer;
///Buffer to hold received packet
Macframe rxBuffer;

///Current antenna selection
unsigned char currentAnt;
///Current 802.11 channel
unsigned char chan;

unsigned char buzzerEnable;

//Define handy macros for MAC packet types
///ACK/NACK packet types
#define ACKPACKET 0
#define NACKPACKET 1
///Data packet types
#define DATAPACKET 2

// Aid for Debugging Purposes, if 1 print from Teraterm
#define PrintDebug 0

warpnetEthernetPktHeader txEthPktHeader;


void resetStats(){
int i,j,k;

for(i=0;i<MAX_NUMNODES;i++){
	for(j=0;j<MAX_NUMNODES;j++){
		for(k=0;k<NUMMODES;k++){
			stats_numDataTx[i][j][k]=0;
			stats_numNACKTx[i][j][k]=0;
			stats_numDataRx[i][j][k]=0;
			stats_numNACKRx[i][j][k]=0;	
			stats_sumGains[i][j][k]=0;
			stats_sumPacketsRx[i][j][k]=0;
			stats_sumRSSI[i][j][k]=0;
		}
	}
}

stats_numBadHeaderRx=0;
}


///@brief Callback for the depression of the left push button
///
///This function is empty by default
void left(){
//	xil_printf("stats_numDataTx      = %d\r\n",stats_numDataTx);
//	xil_printf("stats_numDataRx      = %d\r\n",stats_numDataRx);
//	xil_printf("numRelayTx     = %d\r\n",numRelayTx);
//	xil_printf("stats_numBadHeaderRx = %d\r\n",stats_numBadHeaderRx);
//	xil_printf("stats_numNACKTx      = %d\r\n",stats_numNACKTx);
//	xil_printf("stats_numNACKRx      = %d\r\n",stats_numNACKRx);
}

///@brief Callback for the depression of the right push button
///
///This button switched between different fullrate modulation orders
void right(){
//	stats_numDataTx=0;
//	stats_numDataRx=0;
//	stats_numBadHeaderRx=0;
//	stats_numNACKTx=0;
//	stats_numNACKRx=0;
//	numRelayTx = 0;
}

///@brief Callback for the depression of the up push button
///
///This button increments the 2.4GHz channel being used; only valid channels (in [1,14]) will be used
void up(){
	buzzerEnable = 1;
}

///@brief Callback for the depression of the middle push button
///
///This button decrements the 2.4GHz channel being used; only valid channels (in [1,14]) will be used
void middle(){
	buzzerEnable  = 0;
}

///@brief Updates the LCD
///
///Updates the LCD. This is only for the User I/O daughtercard. If you do not have this card,
///this function will do nothing. This operation takes some time, so it can only be called in a few places so as to
///not adversely affect the performance of the MAC.
void updateLCD()
{

//	warp_userioboard_set_leds(userIOBoard_LEDs);	
//	snprintf(charsToPrint, 16, "Good: %09d ", pktCount_good);
//	warp_userio_lcd_printline(charsToPrint, 16, 4, 1);
//	snprintf(charsToPrint, 16, "Bad:  %09d ", (pktCount_bad)>>1);//bad pkts get counted twice
//	warp_userio_lcd_printline(charsToPrint, 16, 5, 1);
	
	return;
}

///@brief Callback for the reception of UART bytes
///@param uartByte ASCII byte received from UART
///
///Provides the user with the bytes that was received over the serial port. This is useful for configuring
///PHY and MAC parameters in real time on a board.
void uartRecv_callback(unsigned char uartByte)
{
	if(uartByte != 0x0)
	{
		xil_printf("(%c)\t", uartByte);
		
		switch(uartByte)
		{
			case ASCII_W:
				docPreambleScaling+=10;
				mimo_ofdmTx_setTxScaling(docPreambleScaling,docPayloadScaling);
				xil_printf("+Preamble Scaling: %d\r\n",docPreambleScaling);
				break;
				
			case ASCII_w:
				docPreambleScaling-=10;
				mimo_ofdmTx_setTxScaling(docPreambleScaling,docPayloadScaling);
				xil_printf("-Preamble Scaling: %d\r\n",docPreambleScaling);
				break;
				
			case ASCII_E:
				docPayloadScaling+=10;
				mimo_ofdmTx_setTxScaling(docPreambleScaling,docPayloadScaling);
				xil_printf("+Payload Scaling: %d\r\n",docPayloadScaling);
				break;
				
			case ASCII_e:
				docPayloadScaling-=10;
				mimo_ofdmTx_setTxScaling(docPreambleScaling,docPayloadScaling);
				xil_printf("-Payload Scaling: %d\r\n",docPayloadScaling);				
				break;
				
			case ASCII_1:
				pktFullRate = HDR_FULLRATE_BPSK;
				xil_printf("Tx Full Rate = BPSK\r\n");
				break;
				
			case ASCII_2:
				pktFullRate = HDR_FULLRATE_QPSK;
				xil_printf("Tx Full Rate = QPSK\r\n");
				break;
				
			case ASCII_4:
				pktFullRate = HDR_FULLRATE_QAM_16;
				xil_printf("Tx Full Rate = 16-QAM\r\n");
				break;
				
			case ASCII_5:
			    randomData_Mode=(randomData_Mode+1)%2;
				xil_printf("randomData_Mode: %d\r\n",randomData_Mode);
				break;

				
			case ASCII_6:
				pktFullRate = HDR_FULLRATE_QAM_64;
				xil_printf("Tx Full Rate = 64-QAM\r\n");
				break;
			
			case ASCII_8:
				debug_sisoMode = (debug_sisoMode ^ 1) & 0x1;
				if(debug_sisoMode){
					warpphy_setAntennaMode(TX_ANTMODE_SISO_ANTA, RX_ANTMODE_SISO_ANTA);
				}
				else{
					warpphy_setAntennaMode(TX_ANTMODE_ALAMOUTI_ANTA, RX_ANTMODE_ALAMOUTI_ANTA);
				}
				xil_printf("SISO mode = %d\r\n", debug_sisoMode);

				break;
			case ASCII_9:
				agcPrintMode = (agcPrintMode ^ 1) & 0x1;
				break;

			case ASCII_O:
				warpphy_set_FFTOffset_Plus();
				break;
			case ASCII_o:
				warpphy_set_FFTOffset_Minus();
				break;
			case ASCII_F:
				if(chan<14) chan++;
				warpphy_setChannel(GHZ_2, chan);
				xil_printf("Current channel: %d\r\n",chan);
				break;
			case ASCII_f:
				if(chan>1) chan--;
				warpphy_setChannel(GHZ_2, chan);
				xil_printf("Current channel: %d\r\n",chan);
				break;
			case ASCII_G:
				txPower++;
				warpphy_setTxPower(txPower);
				xil_printf("New Tx Power = 0x%x\r\n",txPower);
				break;
			case ASCII_g:
				txPower--;
				warpphy_setTxPower(txPower);
				xil_printf("New Tx Power = 0x%x\r\n",txPower);
				break;
			case ASCII_C:
				ofdm_txrx_mimo_WriteReg_Rx_ControlBits(0, ofdm_txrx_mimo_ReadReg_Rx_ControlBits(0) | COARSE_CFO_EN);
				xil_printf("Coarse CFO Enabled\r\n");
				break;
			case ASCII_c:
				ofdm_txrx_mimo_WriteReg_Rx_ControlBits(0, ofdm_txrx_mimo_ReadReg_Rx_ControlBits(0) & ~(COARSE_CFO_EN));
				xil_printf("Coarse CFO Disabled\r\n");
				break;
			case ASCII_A:
				xil_printf("AF/ReTx enabled\r\n");
				respondToNacks = 1;
				break;
			case ASCII_a:
				xil_printf("AF/ReTx disabled\r\n");
				respondToNacks = 0;
				break;
			case ASCII_t:
				rx_correThresh = rx_correThresh - 50;
				warpphy_setLongCorrThresh(rx_correThresh);
				break;
			case ASCII_T:
				rx_correThresh = rx_correThresh + 50;
				warpphy_setLongCorrThresh(rx_correThresh);
				break;

		    case ASCII_V:
				cfoVar++;
				mimo_ofdmRx_setCFOMaxDiff(cfoVar);
				xil_printf("cfoVar = %d\r\n",cfoVar);
				break;
		    case ASCII_v:
				cfoVar--;
				mimo_ofdmRx_setCFOMaxDiff(cfoVar);
				xil_printf("cfoVar = %d\r\n",cfoVar);
				break;

				
			case ASCII_J:
				cfoDly++;
				mimo_ofdmRx_setCFOCalcDly(cfoDly);
				xil_printf("CFO delay: %d\r\n", cfoDly);
				break;
			case ASCII_j:
				cfoDly--;
				mimo_ofdmRx_setCFOCalcDly(cfoDly);
				xil_printf("CFO delay: %d\r\n", cfoDly);
				break;
				
			case ASCII_x:
				//warpmac_uhoh();
				break;
				
				
			case ASCII_S:
				//warpphy_setAFScalingPlus();
				break;
				
			case ASCII_s:
				//warpphy_setAFScalingMinus();
				break;
			
			case ASCII_P:
				warpphy_setPktDetPlus(200);
                break;

            case ASCII_p:
				warpphy_setPktDetMinus(200);
                break;
                
			case ASCII_B:
				//antB_preambleShift++;
				//warpphy_setAntBPreambleShift(antB_preambleShift);
				//xil_printf("AntB Preamble Shift: %d\r\n", antB_preambleShift);
				break;
				
			case ASCII_b:
				//antB_preambleShift--;
				//warpphy_setAntBPreambleShift(antB_preambleShift);
				//xil_printf("AntB Preamble Shift: %d\r\n", antB_preambleShift);
				break;
				
			case ASCII_R:
				autoRespDelay_AFTx++;
				autoRespDelay_ReTx++;
				xil_printf("autoRespDelay_AFTx: %d - autoRespDelay_ReTx: %d\r\n", autoRespDelay_AFTx, autoRespDelay_ReTx);
				break;
				
			case ASCII_r:
				autoRespDelay_AFTx--;
				autoRespDelay_ReTx--;
				xil_printf("autoRespDelay_AFTx: %d - autoRespDelay_ReTx: %d\r\n", autoRespDelay_AFTx, autoRespDelay_ReTx);
				break;
			
                
            case ASCII_q:
                ignoreSeqNums = (ignoreSeqNums ^ 1)%2;
                xil_printf("Ignore seq nums: %d\r\n", ignoreSeqNums);
                break;

			case ASCII_N:
//				autoRespDelay_NACKTx++;
//	autoResp_action = PHY_AUTORESPONSE_TXACTION_CONFIG(pktBuf_tx_NACK, PHY_AUTORESPONSE_ACT_TRANS_HDR, autoRespDelay_NACKTx, (REQ_DST_ME | REQ_DATA | PHY_AUTORESPONSE_REQ_GOODHDR | PHY_AUTORESPONSE_REQ_GOODPKT)); //Use for NACKs in place of ACKs
//				mimo_ofdmTxRx_setAction1(autoResp_action);
//				xil_printf("NACK delay: %d\r\n", autoRespDelay_NACKTx);
				break;
				
			case ASCII_n:
//				autoRespDelay_NACKTx--;
//	autoResp_action = PHY_AUTORESPONSE_TXACTION_CONFIG(pktBuf_tx_NACK, PHY_AUTORESPONSE_ACT_TRANS_HDR, autoRespDelay_NACKTx, (REQ_DST_ME | REQ_DATA | PHY_AUTORESPONSE_REQ_GOODHDR | PHY_AUTORESPONSE_REQ_GOODPKT)); //Use for NACKs in place of ACKs
//				mimo_ofdmTxRx_setAction1(autoResp_action);
//				xil_printf("NACK delay: %d\r\n", autoRespDelay_NACKTx);
				break;

			case ASCII_Z:
				autoRespDelay_ACKTx++;
				autoResp_action = PHY_AUTORESPONSE_TXACTION_CONFIG(pktBuf_tx_ACK, 1, autoRespDelay_ACKTx, (REQ_DST_ME | REQ_DATA | PHY_AUTORESPONSE_REQ_GOODHDR | PHY_AUTORESPONSE_REQ_GOODPKT));
				mimo_ofdmTxRx_setAction0(autoResp_action);
				xil_printf("ACK delay: %d\r\n", autoRespDelay_ACKTx);
				break;

			case ASCII_z:
				autoRespDelay_ACKTx--;
				autoResp_action = PHY_AUTORESPONSE_TXACTION_CONFIG(pktBuf_tx_ACK, 1, autoRespDelay_ACKTx, (REQ_DST_ME | REQ_DATA | PHY_AUTORESPONSE_REQ_GOODHDR | PHY_AUTORESPONSE_REQ_GOODPKT));
				mimo_ofdmTxRx_setAction0(autoResp_action);
				xil_printf("ACK delay: %d\r\n", autoRespDelay_ACKTx);
				break;
				
			case ASCII_m:
				
				break;
				
			default:
				xil_printf("Undefined command\r\n");
				break;
		}
	}
	
	return;
}
///@brief Callback for the expiration of timers
///
///This function is responsible for handling #TIMEOUT and #BACKOFF.
///The job responsibilities of this function are to:
///-increase the contention window upon the expiration of a #TIMEOUT
///-initiate a #BACKOFF timer upon the expiration of a #TIMEOUT
///-retransmit a packet upon the expiration of a #BACKOFF
///@param timerType #TIMEOUT or #BACKOFF
void timer_callback(unsigned char timerType){
	switch(timerType){
	
		case BUZZER_TIMER:
		
			warp_userioboard_set_buzzer_en(0);
		//	xil_printf("!\r\n");
		break;
		case TIMEOUT_TIMER:
		
			if(!relayMode)
			{
				//Source sent a DATA but never got ACK or NACK; time to re-transmit or give up
				
				//Check if this will be our last transmission attempt
				if(txBuffer.remainingTx == 1)
				{
					//Disable the auto-retransmitter, in case a NACK is received after a relay transmission during our backoff
					autoResp_action_reTx = 0;
					mimo_ofdmTxRx_setAction2(autoResp_action_reTx);

					//Overwrite the relay address to our own, disabling cooperation from now on
					txBuffer.header.relAddr = NODEID_TO_ADDR(myID);
					
					//Copy the updated header down to the PHY pkt buffer
					warpmac_prepPhyForXmit(&txBuffer, pktBuf_tx_DATA);
				}
				
				//Start a random backoff before attempting to re-transmit
				warpmac_setTimer(BACKOFF_TIMER);
			}
			else //relayMode == 1
			{
				//Relay never got a ACK or NACK, so disable the autoResponse actor for cooperative transmissions
				
				
				
				autoResp_action_coopTx = 0;
				mimo_ofdmTxRx_setAction4(autoResp_action_coopTx);
				


				
				
				//De-assert relay mode
				relayMode = 0;
				
				//Clear autoResponse FLAGA (was set by previous reception of DATA)
				warpphy_clearAutoResponseFlag(AUTORESP_FLAGID_A);
				
				//Re-enable capture of AF waveforms
				//warpmac_setDebugGPIO(0xF);
				warpphy_AFrecordEnable(1);
			}
		break;//END TIMEOUT_TIMER

		case BACKOFF_TIMER:
			if(txBuffer.remainingTx == 0)
			{
				//No more re-transmissions for this packet; discard it and return
				//Re-enable new packet handling (effectively discards current packet)
				warpmac_enableDataFromNetwork();
			}
			else
			{
				if(txBuffer.remainingTx == 1)
				{
					//Disable the autoResponder for data re-transmissions (it already started its last transmission)
					autoResp_action_reTx = 0;
					mimo_ofdmTxRx_setAction2(autoResp_action_reTx);
				}
				
				//Re-transmit the packet
				warpmac_startPhyXmit(pktBuf_tx_DATA);

				//Wait for it to finish
				warpmac_finishPhyXmit();
							warpphy_setTxAntennaSwap(TX_ANTMODE_ALAMOUTI_ANTA_SWAPPED);
				//Start the timeout interval immediately after the Tx finishes
				warpmac_setTimer(TIMEOUT_TIMER);

				//Decrement the packet's remaining transmisisons
				txBuffer.remainingTx = txBuffer.remainingTx - 1;
				
				
			}
		break;//END BACKOFF_TIMER
	}
}


void processCommand(warpnetCommand* commandStruct){
	//xil_printf("Process Command Packet\r\n");
	switch(commandStruct->cmdID){
		case COMMANDID_STARTTRIAL:
//			xil_printf("Starting trial\r\n");
			resetStats();
			if(enablePktGen) warpmac_startPacketGeneration(pktGen_length, pktGen_period);
			XTime_GetTime(&startTime);
			enableStats = 1;
		break;
		case COMMANDID_STOPTRIAL:
//			xil_printf("Stopping trial\r\n");
			enableStats=0;
			XTime_GetTime(&stopTime);
			warpmac_stopPacketGeneration();
			
		break;
		case COMMANDID_PKTGEN:
			if(commandStruct->cmdParam==COMMANDPARAM_ENABLE){
				enablePktGen=1;
//				xil_printf("Enabled Packet Generation\r\n");
			}
			
			if(commandStruct->cmdParam==COMMANDPARAM_DISABLE){
				enablePktGen=0;
//				xil_printf("Disabled Packet Generation\r\n");
			}
		break;
		case COMMANDID_RELAYSTATE:
			if(commandStruct->cmdParam==COMMANDPARAM_OFF){
				allowRelay=0;
				alternating=0;
			}
			else if(commandStruct->cmdParam==COMMANDPARAM_AF){
				allowRelay=1;
			}
			else if(commandStruct->cmdParam==COMMANDPARAM_ALTERNATING){
				alternating=1;
			}
		break;
			
	}
}

void processRequest(warpnetRequest* requestStruct, warpnetObserve* observeStruct, char mode){
	//xil_printf("Processing Request: NodeID = %d, cmdID = %d\r\n",myID,requestStruct->cmdID);
	observeStruct->structID=STRUCTID_OBSERVE;
	observeStruct->nodeID=myID;
	observeStruct->sourceNode=requestStruct->cmdID;
	observeStruct->numDataTx=stats_numDataTx[requestStruct->cmdID][myID][mode];
	observeStruct->numNACKTx=stats_numNACKTx[requestStruct->cmdID][myID][mode];
	observeStruct->numDataRx=stats_numDataRx[myID][requestStruct->cmdID][mode];
	observeStruct->numNACKRx=stats_numNACKRx[myID][requestStruct->cmdID][mode];
	observeStruct->numBadHeaderRx=stats_numBadHeaderRx;
	observeStruct->sumGains=stats_sumGains[myID][requestStruct->cmdID][mode];
	observeStruct->sumRSSI=stats_sumRSSI[myID][requestStruct->cmdID][mode];
	observeStruct->sumPacketCountRx=stats_sumPacketsRx[myID][requestStruct->cmdID][mode];
	
	//xil_printf("sumPacketCountRx = %d \r\n",stats_sumPacketsRx[myID][requestStruct->cmdID][mode]);
	
	observeStruct->trialDuration = stopTime-startTime;
//	XTime_GetTime(&(observeStruct->timeStamp));


//xil_printf("Requested gains = %d\r\n",stats_sumGains[myID][requestStruct->cmdID]);
//xil_printf("Saved gains = %d\r\n",stats_sumGains[myID][0]);

}

void processControl(warpnetControl* controlStruct){
	pktGen_length = controlStruct->pktGen_length;
	pktGen_period = controlStruct->pktGen_period;
	chan = controlStruct->channel;
	warpphy_setChannel(GHZ_2, chan);
	
	xil_printf("Channel = %d\r\n",chan);
	//xil_printf("LEN: %d, Period: %d\r\n",pktGen_length,pktGen_period);
}

///@brief Callback for the reception of Ethernet Management packets
///
///This function is called by the ethernet MAC drivers
///and process the packet to perform specific operations
///depending on the packet struct
///@param length Length, in bytes, of received Ethernet frame
///@brief Callback for the reception of Ethernet Management packets
///
///This function is called by the ethernet MAC drivers
///and process the packet to perform specific operations
///depending on the packet struct
///@param length Length, in bytes, of received Ethernet frame
void mgmtFromNetworkLayer_callback(Xuint32 length, char* payload){

//xil_printf("MGMT RX!\r\n");

	int pktBuf_payloadAddr;
	warpnetEthernetPktHeader* receivedPkt;
	warpnetControllerGroup* copyStruct;
	warpnetCommand* commandStruct;
	warpnetControl* controlStruct;
	warpnetRequest* requestStruct;
	warpnetAck ackStruct;
	warpnetObserve observeStruct;
	int sizeofcopy,rxPktOffset,numRxStructs,rxSeqNum,i,thisStructType;
	sizeofcopy = sizeof(warpnetControllerGroup);
	receivedPkt = (warpnetEthernetPktHeader*) payload;
		
	//If this is a WARPnet Ethernet packet, process it immediately
	if((receivedPkt->ethType) == WARPNET_ETHTYPE_SVR2NODE){
		rxPktOffset = sizeof(warpnetEthernetPktHeader);
		numRxStructs = receivedPkt->numStructs;
		rxSeqNum = receivedPkt->seqNum;
		
		//Itereate over each struct in the Ethernet packet
		for(i=0; i<numRxStructs; i++)
		{
			//Punt if numStructs would have us march past the end of the received packet
			if(rxPktOffset >= length)
			{
				xil_printf("Error! Mgmt pktLength too short for numStructs\r\n");
				return;
			}
			
			copyStruct = (warpnetControllerGroup*)(payload+rxPktOffset);
			
			//Extract the struct type (always the first byte of a struct)
			thisStructType = *(unsigned char*)(payload+rxPktOffset+sizeofcopy);
			switch(thisStructType)
			{
				case STRUCTID_COMMAND:
					commandStruct = (warpnetCommand*)(payload+rxPktOffset+sizeofcopy);
					if((commandStruct->nodeID) == myID){

						ackStruct.structID = STRUCTID_COMMAND_ACK;
						ackStruct.nodeID = myID;
						ackStruct.cmdID = commandStruct->cmdID;
						
						txEthPktHeader.pktLength = sizeof(warpnetEthernetPktHeader) + sizeof(warpnetAck) + sizeofcopy;
						txEthPktHeader.numStructs = 1;
						
						pktBuf_payloadAddr = warpphy_getBuffAddr(WARPNET_NODE2SVR_PKTBUFFINDEX);
						memcpy((void *)pktBuf_payloadAddr, &(txEthPktHeader), sizeof(warpnetEthernetPktHeader));
						memcpy((void *)pktBuf_payloadAddr+sizeof(warpnetEthernetPktHeader), copyStruct, sizeofcopy);
						memcpy((void *)pktBuf_payloadAddr+sizeof(warpnetEthernetPktHeader)+sizeofcopy, &(ackStruct), sizeof(warpnetAck));
						
						warpmac_prepPktToNetwork((void *)pktBuf_payloadAddr, txEthPktHeader.pktLength);
						warpmac_startPktToNetwork(txEthPktHeader.pktLength);
						processCommand(commandStruct);
					}
					rxPktOffset += sizeof(warpnetCommand);
					break;
				case STRUCTID_CONTROL:
					controlStruct = (warpnetControl*)(payload+rxPktOffset+sizeofcopy);
					if((controlStruct->nodeID) == myID){

						ackStruct.structID = STRUCTID_CONTROL_ACK;
						ackStruct.nodeID = myID;
						
						txEthPktHeader.pktLength = sizeof(warpnetEthernetPktHeader) + sizeof(warpnetAck) + sizeofcopy;
						txEthPktHeader.numStructs = 1;
						
						pktBuf_payloadAddr = warpphy_getBuffAddr(WARPNET_NODE2SVR_PKTBUFFINDEX);
						memcpy((void *)pktBuf_payloadAddr, &(txEthPktHeader), sizeof(warpnetEthernetPktHeader));
						memcpy((void *)pktBuf_payloadAddr+sizeof(warpnetEthernetPktHeader), copyStruct, sizeofcopy);
						memcpy((void *)pktBuf_payloadAddr+sizeof(warpnetEthernetPktHeader)+sizeofcopy, &(ackStruct), sizeof(warpnetAck));
						
						warpmac_prepPktToNetwork((void *)pktBuf_payloadAddr, txEthPktHeader.pktLength);
						warpmac_startPktToNetwork(txEthPktHeader.pktLength);
						processControl(controlStruct);
					}
					
					rxPktOffset += sizeof(warpnetControl);
					break;
				case STRUCTID_OBSERVE_REQUEST_NORELAY:
					requestStruct = (warpnetRequest*)(payload+rxPktOffset+sizeofcopy);
					if((requestStruct->nodeID) == myID){
						processRequest(requestStruct,&observeStruct,0);
						txEthPktHeader.pktLength = sizeof(warpnetEthernetPktHeader) + sizeof(warpnetObserve) + sizeofcopy;
						txEthPktHeader.numStructs = 1;
						
						pktBuf_payloadAddr = warpphy_getBuffAddr(WARPNET_NODE2SVR_PKTBUFFINDEX);
						memcpy((void *)pktBuf_payloadAddr, &(txEthPktHeader), sizeof(warpnetEthernetPktHeader));
						memcpy((void *)pktBuf_payloadAddr+sizeof(warpnetEthernetPktHeader), copyStruct, sizeofcopy);
						memcpy((void *)pktBuf_payloadAddr+sizeof(warpnetEthernetPktHeader)+sizeofcopy, &(observeStruct), sizeof(warpnetObserve));
						
						warpmac_prepPktToNetwork((void *)pktBuf_payloadAddr, txEthPktHeader.pktLength);
						warpmac_startPktToNetwork(txEthPktHeader.pktLength);
					}
					
					rxPktOffset += sizeof(warpnetControl);
					break;
				case STRUCTID_OBSERVE_REQUEST_RELAY:
					requestStruct = (warpnetRequest*)(payload+rxPktOffset+sizeofcopy);
					if((requestStruct->nodeID) == myID){
						processRequest(requestStruct,&observeStruct,1);
						txEthPktHeader.pktLength = sizeof(warpnetEthernetPktHeader) + sizeof(warpnetObserve) + sizeofcopy;
						txEthPktHeader.numStructs = 1;
						
						pktBuf_payloadAddr = warpphy_getBuffAddr(WARPNET_NODE2SVR_PKTBUFFINDEX);
						memcpy((void *)pktBuf_payloadAddr, &(txEthPktHeader), sizeof(warpnetEthernetPktHeader));
						memcpy((void *)pktBuf_payloadAddr+sizeof(warpnetEthernetPktHeader), copyStruct, sizeofcopy);
						memcpy((void *)pktBuf_payloadAddr+sizeof(warpnetEthernetPktHeader)+sizeofcopy, &(observeStruct), sizeof(warpnetObserve));
						
						warpmac_prepPktToNetwork((void *)pktBuf_payloadAddr, txEthPktHeader.pktLength);
						warpmac_startPktToNetwork(txEthPktHeader.pktLength);
					}
					
					rxPktOffset += sizeof(warpnetControl);
					break;
			}/* END switch(thisStructType) */
			
		}/* END for(numRxStructs) */
	}/* END if(warpNet packet) */
	
	return;


}

///@brief Callback for the reception of Ethernet packets
///
///This function is called by the ethernet MAC drivers
///when a packet is available to send. This function fills
///the Macframe transmit buffer with the packet and sends
///it over the OFDM link
///@param length Length, in bytes, of received Ethernet frame
void dataFromNetworkLayer_callback(Xuint32 length, char* payload){

	unsigned char rxSeqNum;
	unsigned char destNode;
	unsigned char relayNode;
	
	warpnetRTObserve rtObserve;
	warpnetControllerGroup controllerGroup;
	XTime timeStamp;
	
	XTime_GetTime(&timeStamp);

	

//	xil_printf("data from network Layer\r\n");

	//Disable any new packets until we're finished with this one
	warpmac_disableDataFromNetwork();

	warpphy_setTxAntennaSwap(TX_ANTMODE_ALAMOUTI_ANTA);
	
	//Reset the contention window to its smallest size for this exchange
	warpmac_resetCurrentCW();

	//Update the Tx pkt buffer index if we're cycling through them (for non-Ethernet traffic testing only)
	if(randomData_Mode){
		pktBuf_tx_DATA = (((pktBuf_tx_DATA+1)%25) + 4);
		warpmac_setPHYTxBuffer(pktBuf_tx_DATA);
	}


	//Define and enable the autoResponse actor for sending our retransmissions when NACKs are received
	autoResp_action_reTx = PHY_AUTORESPONSE_TXACTION_CONFIG(pktBuf_tx_DATA, 0, autoRespDelay_ReTx, (REQ_DST_ME | REQ_NACK | PHY_AUTORESPONSE_REQ_GOODHDR | PHY_AUTORESPONSE_REQ_GOODPKT));
	mimo_ofdmTxRx_setAction2(autoResp_action_reTx);

	//For now, assume our destination is our opposite ID (all traffic is 0 <-> 1)
	// and the node 2 is always our relay
	destNode = (myID+1)%2;
	
	if(alternating&&currentAlt){
		relayNode = 2;
	}
	else{
		relayNode = myID;
	}

//relayNode = 2;
	
//xil_printf("RELAY NODE%d\r\n",relayNode);

	//Increment the sequence number for this destination
	txSequences[destNode] = ( (txSequences[destNode]+1) & 0xFFFF);

	//Set this packet's remainingTx count to the maximum
	txBuffer.remainingTx = max_numTransmissions;

	//Construct the new wireless packet's header
	txBuffer.header.length = length;		//Packet length (payload only)
	txBuffer.header.pktType = DATAPACKET;	//Packet type DATA
	txBuffer.header.fullRate = pktFullRate;	//Full-rate modulation scheme
	txBuffer.header.codeRate = pktCodeRate;	//Coding rate (ignored for uncoded PHY)
	txBuffer.header.seqNum = txSequences[destNode];	//Dest node's next seqeunce number
	txBuffer.header.srcAddr = NODEID_TO_ADDR(myID);			//Source MAC address
	txBuffer.header.destAddr = NODEID_TO_ADDR(destNode);	//Destination MAC address
	txBuffer.header.relAddr = NODEID_TO_ADDR(relayNode);	//Relay MAC address

	
	//Copy the header over to the DATA Tx packet buffer
	warpmac_prepPhyForXmit(&txBuffer, pktBuf_tx_DATA);


	if(enableStats) stats_numDataTx[destNode][myID][currentAlt]++;
	currentAlt = (currentAlt+1)%2;	

	//Packet is ready for transmission
	if(warpmac_carrierSense()){
		//Medium is idle; transmit right away
		warpmac_startPhyXmit(pktBuf_tx_DATA);
		
		////CRH
		//				xil_printf("****TRANSMIT****\r\n");
		//				unsigned int test = 64;
		//				unsigned int testIndex;
						
		//				for(testIndex=0;testIndex<test;testIndex++){
		//					xil_printf("%x ",0xFF&(*((char *)warpphy_getBuffAddr(pktBuf_tx_DATA)+NUM_HEADER_BYTES+testIndex)));
		//				}
		//				xil_printf("****TRANSMIT****\r\n");
	////CRH

		//Wait for it to finish and enable the receiver
		warpmac_finishPhyXmit();
		warpphy_setTxAntennaSwap(TX_ANTMODE_ALAMOUTI_ANTA_SWAPPED);

		//Start a timeout timer
		warpmac_setTimer(TIMEOUT_TIMER);

		//Decrement the remaining resent counter
		if(txBuffer.remainingTx > 0){
			txBuffer.remainingTx = txBuffer.remainingTx - 1;
		}
	}
	
	else{
		//Medium was busy in the last DIFS period; wait a random backoff before transmitting
		warpmac_setTimer(BACKOFF_TIMER);
	}
	
	if(enableStats){
	controllerGroup.controllerID = 0;
	controllerGroup.controllerGrp = 0;
	controllerGroup.access = 1;

	rtObserve.structID = STRUCTID_RTOBSERVE;
	rtObserve.nodeID = myID;
	rtObserve.sequenceNumber = txBuffer.header.seqNum;
	rtObserve.pktType = txBuffer.header.pktType;
	rtObserve.srcNode = txBuffer.header.srcAddr;
	rtObserve.destNode = txBuffer.header.destAddr;
	rtObserve.relNode = txBuffer.header.relAddr;
	rtObserve.rssi = 0xFFFF;
	rtObserve.gain = 0xFFFF;
	rtObserve.state=0xFF;
	rtObserve.timeStampHigh = (unsigned short)((timeStamp>>32)&0xFFFF);	
	rtObserve.timeStampLow = (unsigned int)(timeStamp&0xFFFFFFFF);
	txEthPktHeader.pktLength = sizeof(warpnetEthernetPktHeader) + sizeof(warpnetRTObserve) + sizeof(warpnetControllerGroup);
	
	int pktBuf_payloadAddr = warpphy_getBuffAddr(WARPNET_NODE2SVR_PKTBUFFINDEX);
	memcpy((void *)pktBuf_payloadAddr, &(txEthPktHeader), sizeof(warpnetEthernetPktHeader));
	memcpy((void *)pktBuf_payloadAddr+sizeof(warpnetEthernetPktHeader), &controllerGroup, sizeof(warpnetControllerGroup));
	memcpy((void *)pktBuf_payloadAddr+sizeof(warpnetEthernetPktHeader)+sizeof(warpnetControllerGroup), &(rtObserve), sizeof(warpnetRTObserve));
	


	//xil_printf("%d, %d, %d, %d\r\n" ,rtObserve.sequenceNumber,rtObserve.srcNode ,rtObserve.destNode ,rtObserve.relNode );
				
	warpmac_prepPktToNetwork((void *)pktBuf_payloadAddr, txEthPktHeader.pktLength);
	warpmac_startPktToNetwork(txEthPktHeader.pktLength);
	
	
	
	
	}
	
	
	return;
}

///@brief Callback for the reception of bad wireless headers
///
///@param packet Pointer to received Macframe
void phyRx_badHeader_callback() {

	
	userIOBoard_LEDs = userIOBoard_LEDs==0x80 ? 0x01 : (userIOBoard_LEDs << 1);

	if(enableStats) stats_numBadHeaderRx++;

	
    if(agcPrintMode)
	{
		agcGains = ofdm_AGC_GetGains();
		pktDet_rssi = ofdm_txrx_mimo_ReadReg_Rx_PktDet_midPktRSSI_antA();
		xil_printf("(bad)\tAGC Gains: %d %d\t%d %d\t", agcGains&0x3, (agcGains>>2)&0x1F, (agcGains>>8)&0x3, (agcGains>>10)&0x1F);
		//		xil_printf("AGC Thresholds: %d / %d / %d\t", (0xFFFFFF00 | agc_coarseThresh_1), (0xFFFFFF00 | agc_coarseThresh_2), (0xFFFFFF00 | agc_coarseThresh_3) );
        xil_printf("RSSI (ant A): %d\r\n", pktDet_rssi);
	}

	
	return;
}

///@brief Callback for the reception of good wireless headers
///
///This function then polls the PHY to determine if the entire packet passes checksum
///thereby triggering the transmission of the ACK and the transmission of the received
///data over Ethernet.
///@param packet Pointer to received Macframe
int phyRx_goodHeader_callback(Macframe* packet){

int manualClear = 0;



warpmac_setDebugGPIO(0x1,0x1);
		unsigned int currPhaseInc;
	unsigned char state=PHYRXSTATUS_INCOMPLETE;
	unsigned char srcNode,dstNode,relNode,relMode;
	unsigned char shouldSend = 0;
	warpnetRTObserve rtObserve;
	warpnetControllerGroup controllerGroup;
	XTime timeStamp;
	int i;
	shouldSend = 0;
	
	XTime_GetTime(&timeStamp);
	
	controllerGroup.controllerID = 0;
	controllerGroup.controllerGrp = 0;
	controllerGroup.access = 1;
	
	//Extract the source node ID from the packet's source address
	srcNode = ADDR_TO_NODEID(packet->header.srcAddr);
	relNode = ADDR_TO_NODEID(packet->header.relAddr);
	dstNode = ADDR_TO_NODEID(packet->header.destAddr);

	rtObserve.structID = STRUCTID_RTOBSERVE;
	rtObserve.nodeID = myID;
	rtObserve.sequenceNumber = packet->header.seqNum;
	rtObserve.pktType = packet->header.pktType;
	rtObserve.srcNode = srcNode;
	rtObserve.destNode = dstNode;
	rtObserve.relNode = relNode;
	rtObserve.rssi = ofdm_txrx_mimo_ReadReg_Rx_PktDet_midPktRSSI_antA();
	rtObserve.gain = warpphy_returnGainsDB();
	rtObserve.timeStampHigh = (unsigned short)((timeStamp>>32)&0xFFFF);	
	rtObserve.timeStampLow = (unsigned int)(timeStamp&0xFFFFFFFF);
	txEthPktHeader.pktLength = sizeof(warpnetEthernetPktHeader) + sizeof(warpnetRTObserve) + sizeof(warpnetControllerGroup);
	

	
	int pktBuf_payloadAddr = warpphy_getBuffAddr(WARPNET_NODE2SVR_PKTBUFFINDEX);
	memcpy((void *)pktBuf_payloadAddr, &(txEthPktHeader), sizeof(warpnetEthernetPktHeader));
	memcpy((void *)pktBuf_payloadAddr+sizeof(warpnetEthernetPktHeader), &controllerGroup, sizeof(warpnetControllerGroup));
	memcpy((void *)pktBuf_payloadAddr+sizeof(warpnetEthernetPktHeader)+sizeof(warpnetControllerGroup), &(rtObserve), sizeof(warpnetRTObserve));
						

	
	if(relNode==2){
		relMode = 1;
	}
	else{
		relMode = 0;
	}
	
	if(enableStats) stats_sumGains[myID][srcNode][relMode] += warpphy_returnGainsDB();
	//xil_printf("GAINS = %d\r\n",warpphy_returnGainsDB());
	if(enableStats) stats_sumPacketsRx[myID][srcNode][relMode]++;
	if(enableStats)	stats_sumRSSI[myID][srcNode][relMode]+=ofdm_txrx_mimo_ReadReg_Rx_PktDet_midPktRSSI_antA();
	
	//Debug-only: print AGC values for this packet
	if(agcPrintMode)
	{
		agcGains = ofdm_AGC_GetGains();
		pktDet_rssi = ofdm_txrx_mimo_ReadReg_Rx_PktDet_midPktRSSI_antA();
		xil_printf("(good)\tAGC Gains: %d %d\t%d %d\t", agcGains&0x3, (agcGains>>2)&0x1F, (agcGains>>8)&0x3, (agcGains>>10)&0x1F);
        xil_printf("RSSI (ant A): %d\r\n", pktDet_rssi);
	}
	
	//Rotate the ring of LEDs clockwise
	userIOBoard_LEDs = userIOBoard_LEDs==0x1 ? 0x80 : (userIOBoard_LEDs >> 1);
	
	//Print the header (debug only- completely ruins actual performance)
#if 0
	xil_printf("Rx Header: ");
	for(i=0; i<24; i++)
	{
		xil_printf("%d.",*((unsigned char*)&(packet->header) + i));
	}
	xil_printf("\r\n");
#endif

	//If packet is from self, it was (hopefully) transmitted by the relay
	// Count this as a retransmission
	if(packet->header.srcAddr == ( NODEID_TO_ADDR(myID)) ) {
		switch(packet->header.pktType){
			case DATAPACKET:
				warpmac_clearTimer(TIMEOUT_TIMER);
				warpmac_clearTimer(BACKOFF_TIMER);
				warpmac_finishPhyRecv();
				warpmac_setTimer(TIMEOUT_TIMER);
				if(txBuffer.remainingTx > 0){
					txBuffer.remainingTx = txBuffer.remainingTx - 1;					
				}
			break;

			default:
				//This should never, ever happen...
			break;
		}

		//return;
	}
	
	//If the packet is addressed to this node
	if(packet->header.destAddr == ( NODEID_TO_ADDR(myID)) ) {

		switch(packet->header.pktType){
			case DATAPACKET:
				
				if((packet->header.length)>1400){
					updateLCD();
				}

				state = warpmac_finishPhyRecv();
				
				if(state&PHYRXSTATUS_GOOD){
					//Go, go autoResponder ACK!
					warpmac_incrementLEDHigh();
					
					
					//Check if the last received seq number for this partner node matches this received pkt
					// If not, record the new number and allow the pkt to be forwarded over the wire
					if(ignoreSeqNums || ( (srcNode<MAX_NUMNODES) && (rxSequences[srcNode] != (packet->header.seqNum))) ){
						rxSequences[srcNode] = (packet->header.seqNum);
						//CRH - NACK CHARACTERIZATION
							if(enableStats) stats_numDataRx[packet->header.destAddr][packet->header.srcAddr][relMode]++;
						//CHR
						//shouldSend = 1;
					}
					

					
					////
					//warp_userioboard_buzzer_set(PERHIGH, DUTYHIGH);
					//warp_userioboard_set_buzzer_en(1&buzzerEnable);
					//warpmac_setTimer(BUZZER_TIMER);
					////
					
					//Update the right-hex display with the current resend count for the received pkt
					warpmac_leftHex(0xF & (packet->header.seqNum));
					
					//Starts the DMA transfer of the payload into the EMAC
					if(shouldSend)
					{

						warpmac_prepPktToNetwork((void *)warpphy_getBuffAddr(pktBuf_rx)+NUM_HEADER_BYTES, (packet->header.length));
						
						
					//	xil_printf("****RECEIVE**\r\n");
					//	unsigned int test = 64;
					//	unsigned int testIndex;
						
					//	for(testIndex=0;testIndex<test;testIndex++){
					//		xil_printf("%x ",0xFF&(*((char *)warpphy_getBuffAddr(pktBuf_rx)+NUM_HEADER_BYTES+testIndex)));
					//	}
					//	xil_printf("****RECEIVE****\r\n");
						
						warpmac_finishPhyXmit();//? needed?
						warpmac_startPktToNetwork(packet->header.length);
						


						
					}
					
				}
				
				if(state&PHYRXSTATUS_BAD){
				warpmac_incrementLEDLow();
					//Go, go autoReponder NACK!
					//CRH - NACK CHARACTERIZATION
					if(enableStats) stats_numNACKTx[packet->header.srcAddr][myID][relMode]++;
					//CHR
					
					////

					//warp_userioboard_buzzer_set(PERLOW, DUTYLOW);
					//warp_userioboard_set_buzzer_en(1&buzzerEnable);
					//warpmac_setTimer(BUZZER_TIMER);
					////


					
					
					//Rotate the ring of LEDs counter-clockwise
					userIOBoard_LEDs = userIOBoard_LEDs==0x80 ? 0x01 : (userIOBoard_LEDs << 1);
					
				}
			
			break;//END DATAPACKET
				
			case ACKPACKET:
				pktCount_good++;


				//Clear the timeout timer, set when we transmitted the data packet
				warpmac_clearTimer(TIMEOUT_TIMER);

				//Clear the backoff timer; this handles the case of receiving an ACK for a forwarded DATA packet
				warpmac_clearTimer(BACKOFF_TIMER);

				//Clear the remaining transmit count to assure this packet won't be re-transmitted
				txBuffer.remainingTx = 0;

				//Start a backoff, to gaurantee a random time before attempting to transmit again
				warpmac_setTimer(BACKOFF_TIMER);

			break;//END ACKPACKET
				
			case NACKPACKET:
				
				if(warpmac_inTimeout())
				{

				//CRH - NACK CHARACTERIZATION
				if(enableStats) stats_numNACKRx[packet->header.destAddr][packet->header.srcAddr][relMode]++;
				//CHR
				
					//Go, go autoResponder DATA reTx (if it's still enabled)!



					//Clear the timeout timer, set when we transmitted the data packet
					warpmac_clearTimer(TIMEOUT_TIMER);

					if(txBuffer.remainingTx == 0)
					{
						//No more re-transmits; start a backoff, to gaurantee a random time before attempting to transmit a new data packet
						warpmac_setTimer(BACKOFF_TIMER);
					}
					else if(txBuffer.remainingTx == 1)
					{
						//Decrement the remaining transmit count
						txBuffer.remainingTx = txBuffer.remainingTx - 1;
						
						//Disable the autoResponder for data re-transmissions (it already started its last transmission)
						autoResp_action_reTx = 0;
						mimo_ofdmTxRx_setAction2(autoResp_action_reTx);

						//Wait for the transmission to finish the autoRepsonse trigger re-trasnmit
						warpmac_finishPhyXmit();

						//Start a timeout, waiting for a ACK/NACK
						warpmac_setTimer(TIMEOUT_TIMER);

						
					}
					else//remainingTx > 1
					{
						//Decrement the remaining transmit count
						txBuffer.remainingTx = txBuffer.remainingTx - 1;
						
						//Wait for the transmission to finish the autoRepsonse trigger re-trasnmit
						warpmac_finishPhyXmit();
						
						//Start a timeout, waiting for a ACK/NACK
						warpmac_setTimer(TIMEOUT_TIMER);

						
					}
				}
				else if(warpmac_inBackoff())
				{
					//Covers case where we missed a NACK and missed the relay's re-transmission but heard the terminal NACK
					warpmac_clearTimer(TIMEOUT_TIMER);
					warpmac_clearTimer(BACKOFF_TIMER);
					if(txBuffer.remainingTx > 0)
					{
						txBuffer.remainingTx = txBuffer.remainingTx - 1;
					}
					warpmac_setTimer(BACKOFF_TIMER);
				}
				else
				{
						//warp_userioboard_set_buzzer_en(1);
				}
			break;//END NACKPACKET
		}

		//return;
	}//END dstAddr == self

	//Relay address matches this node
	if(packet->header.relAddr == ( NODEID_TO_ADDR(myID)) ) {

		switch(packet->header.pktType){
			//If received packet is data
			case DATAPACKET:
			
//				warpmac_setDebugGPIO(0xF);

warpmac_setDebugGPIO(0x4,0x4);

				state = warpmac_finishPhyRecv();
//					warpmac_setDebugGPIO(0x0);	
	
				if((state&PHYRXSTATUS_GOOD)&&allowRelay){
warpmac_incrementLEDHigh();
			
//		if(enableStats) stats_numDataRx[myID][packet->header.srcAddr][relMode]++;
					//We can cooperate on this packet
					relayMode = 1;
					
					//Define and enable the cooperative autoResponse transmitter
					//Flag A will already be set (that actor is always enabled)
					

						//autoResp_action_coopTx = PHY_AUTORESPONSE_TXACTION_CONFIG(pktBuf_rx, PHY_AUTORESPONSE_ACT_USE_PRECFO, autoRespDelay_AFTx, (REQ_RLY_ME | REQ_NACK | PHY_AUTORESPONSE_REQ_GOODHDR | PHY_AUTORESPONSE_REQ_GOODPKT | PHY_AUTORESPONSE_REQ_FLAGA));
						autoResp_action_coopTx = PHY_AUTORESPONSE_TXACTION_CONFIG(pktBuf_tx_AF, 0, autoRespDelay_AFTx, (REQ_RLY_ME | REQ_NACK | PHY_AUTORESPONSE_REQ_GOODHDR | PHY_AUTORESPONSE_REQ_GOODPKT | PHY_AUTORESPONSE_REQ_FLAGA));
						mimo_ofdmTxRx_setAction4(autoResp_action_coopTx);


					
					//AF recording must be disabled *after* the good pkt interrupt goes high
					// Doing it before will prevent the AF system from knowing when the incoming packet actually stopped
		
					warpphy_AFrecordEnable(0);
					
					//Increment the Rx packet buffer, to preserve the received data packet
					pktBuf_rx_ind = ((pktBuf_rx_ind + 1) % NUM_RX_PKTBUFS);
					pktBuf_rx = rxPktBufs[pktBuf_rx_ind];
                    
					//warpphy_setBuffs(pktBuf_tx_DATA, pktBuf_rx);
					warpmac_setRxBuffers(&rxBuffer, pktBuf_rx);
						
					
					warpphy_clearRxPktStatus();
					manualClear = 1;
					
					relayDestID = ADDR_TO_NODEID(packet->header.destAddr);
					relayLen = packet->header.length;
					//Finally, start a timeout, waiting for a ACK/NACK
					warpmac_clearTimer(TIMEOUT_TIMER);
					warpmac_setTimer(TIMEOUT_TIMER);
					
					////
				//warp_userioboard_buzzer_set(PERHIGH, DUTYHIGH);
				//warp_userioboard_set_buzzer_en(1&buzzerEnable);
				//warpmac_setTimer(BUZZER_TIMER);
					////
					
					return;
					//DEBUG print

				}

				if(state&PHYRXSTATUS_BAD){
				warpmac_incrementLEDLow();
									////
				//warp_userioboard_buzzer_set(PERLOW, DUTYLOW);
				//warp_userioboard_set_buzzer_en(1&buzzerEnable);
				//warpmac_setTimer(BUZZER_TIMER);

					////
					//Let the timeout handler reset things (like restarting AF capture, disabling coop autoResponse, etc.
				}
			
					
			break;//END DATAPACKET

			case NACKPACKET:
				if(enableStats) stats_numNACKRx[myID][packet->header.srcAddr][relMode]++;
			

				
									////
					//warp_userioboard_buzzer_set(PERLOW, DUTYLOW);
					//warp_userioboard_set_buzzer_en(1&buzzerEnable);
					//warpmac_setTimer(BUZZER_TIMER);

					////
					
				if(relayMode) {


				
					if(enableStats) if(enableStats) stats_numDataTx[packet->header.srcAddr][myID][relMode]++;
//CRH ADDME					if(enableStats) stats_numDataTx[][]++;
					
					

					//Go, go autoRepsonder AF Tx!

					
					
//					warpmac_setDebugGPIO(0xF);

					//Clear the timeout (started when we recevied the DATA packet)
					warpmac_clearTimer(TIMEOUT_TIMER);
					
					//Wait for the autoResponse transmission to finish
					warpmac_finishPhyXmit();

//					warpmac_setDebugGPIO(0x0);
					//Restart capturing the AF waveform, now that we've sent the good one we had
					//warpmac_setDebugGPIO(0xF);
					warpphy_AFrecordEnable(1);

					//Disable the cooperative autoResponse transmitter
					autoResp_action_coopTx = 0;
					mimo_ofdmTxRx_setAction4(autoResp_action_coopTx);
					
		

					
					
					relayMode = 0;
				}
				else{
				
				}
			break;//END NACKPACKET

			case ACKPACKET:
				//Clear the timeout (started when we recevied the DATA packet)
				warpmac_clearTimer(TIMEOUT_TIMER);

				//Restart capturing the AF waveform, now that we've sent the good one we had
				//warpmac_setDebugGPIO(0xF);
				warpphy_AFrecordEnable(1);
				
				//Disable the cooperative autoResponse transmitter
				autoResp_action_coopTx = 0;
				mimo_ofdmTxRx_setAction4(autoResp_action_coopTx);
				
	

		
				
				
				relayMode = 0;
			break;//END ACKPACKET
		}
	//	return;
	}
	
	
	
	if(enableStats){
	//	xil_printf("%d,%d,%d,%d\r\n",rtObserve.sequenceNumber,rtObserve.srcNode,rtObserve.destNode,rtObserve.relNode);
	
				
				unsigned char* stateAddr = (void *)pktBuf_payloadAddr+sizeof(warpnetEthernetPktHeader)+sizeof(warpnetControllerGroup) + 8;
				*stateAddr = warpmac_finishPhyRecv();
				
				warpmac_prepPktToNetwork((void *)pktBuf_payloadAddr, txEthPktHeader.pktLength);
				warpmac_startPktToNetwork(txEthPktHeader.pktLength);

	}
	
	return manualClear;

}

///@brief Main function
///
///This function configures MAC parameters, enables the underlying frameworks, and then loops forever.
int main(){		
	
	//Initialize global variables
	cfoVar = 38;
	currentAnt = 0;
	chan = 4;

	agcPrintMode = 0;
	randomData_Mode = 0;
	
	pktCount_good = 0;
	pktCount_bad = 0;

	//Not a relay by default
	relayMode = 0;

	
	debug_sisoMode = 0;
    ignoreSeqNums = 0;
    
        
	txPower = 0x3f;
	
	//Set the full-rate modulation to QPSK by default
//	pktFullRate = HDR_FULLRATE_QPSK;
	pktFullRate = HDR_FULLRATE_QAM_16;
	
	//Set the payload coding rate to NONE by default
	//Note: Reference Design 12 PHY does not provide channel coding
	pktCodeRate = CODE_RATE_NONE;
	
	//Initialize the framework
	warpmac_init();
	max_numTransmissions = 2;
	warpmac_setMaxResend(max_numTransmissions);
	warpmac_setMaxCW(5);

	//Select single-antenna Alamouti mode for both Tx and Rx
	if(debug_sisoMode)
	//	warpphy_setAntennaMode(TX_ANTMODE_SISO_ANTA, RX_ANTMODE_SISO_ANTA);
	warpphy_setAntennaMode(TX_ANTMODE_ALAMOUTI_2ANT, RX_ANTMODE_ALAMOUTI_ANTA);
	else
		warpphy_setAntennaMode(TX_ANTMODE_ALAMOUTI_ANTA, RX_ANTMODE_ALAMOUTI_ANTA);

	warpmac_setTimeout(140); //100
	warpmac_setSlotTime(22); //9 20
	
	//Read Dip Switch value from FPGA board.
	//This value will be used as an index into the routing table for other nodes
	myID = (unsigned short int)warpmac_getMyId();
	warpmac_leftHex(myID);
	//Define the available Rx pkt buffers (1 and 2 act as Ping and Pong for now)
	rxPktBufs[0] = 1;
	rxPktBufs[1] = 2;
	pktBuf_rx_ind = 0;

	pktBuf_rx = rxPktBufs[pktBuf_rx_ind];
	pktBuf_rx_emac = 3;
	pktBuf_tx_DATA = 4;
	pktBuf_tx_ACK = 29;
	pktBuf_tx_NACK = 30;
	pktBuf_tx_AF = 31;

	cfoDly = 16;
	antB_preambleShift = 3;

	autoRespDelay_NACKTx = 2; //100
	autoRespDelay_AFTx = 2;
	autoRespDelay_ReTx = 2;
	autoRespDelay_ACKTx = 0;
	
	rx_correThresh = 1400;
	

	
	//Rx buffer is where the EMAC will DMA Wireless payloads from
	warpmac_setRxBuffers(&rxBuffer, pktBuf_rx);
	//Tx buffer is where the EMAC will DMA Ethernet payloads to
	warpmac_setPHYTxBuffer(pktBuf_tx_DATA);
	warpmac_setEMACRxBuffer(pktBuf_rx_emac);//VERY VERY IMPORTANT
	
	//Copy this node's MAC address into the Tx buffer's source address field
	txBuffer.header.srcAddr = (unsigned short int)( NODEID_TO_ADDR(myID) );
	
	//Register callbacks

	
	warpmac_setCallback(EVENT_PHYGOODHEADER, (void *)phyRx_goodHeader_callback);
	warpmac_setCallback(EVENT_PHYBADHEADER, (void *)phyRx_badHeader_callback);
	warpmac_setCallback(EVENT_MGMTPKT, (void *)mgmtFromNetworkLayer_callback);
	warpmac_setCallback(EVENT_DATAFROMNETWORK, (void *)dataFromNetworkLayer_callback);
	warpmac_setCallback(EVENT_UARTRX, (void *)uartRecv_callback);
	warpmac_setCallback(EVENT_TIMER, (void *)timer_callback);
	
	warpmac_setCallback(EVENT_UPBUTTON, (void *)up);
	warpmac_setCallback(EVENT_MIDDLEBUTTON, (void *)middle);
	warpmac_setCallback(EVENT_LEFTBUTTON, (void *)left);
	warpmac_setCallback(EVENT_RIGHTBUTTON, (void *)right);
	
	//Configure a buzzer tone, to be enabled on odd error conditions
//	warp_userioboard_buzzer_set(40000, 13107);

	userIOBoard_LEDs = 1;
	//Initialize the LCD for slot 1 (i.e. don't flip the image) and non-inverted colors
	warp_userioboard_lcd_init(1, 0);
	warp_userioboard_set_lcd_charbuf(1);
	
	//Print some static lines to the user I/O board LCD
	//snprintf(charsToPrint, 16, " Ref Design v12 ", pktCount_bad);
	warp_userio_lcd_printline("RefDesign v14", 16, 1, 1);
	//snprintf(charsToPrint, 16, "  Rx Pkt Counts", pktCount_bad);
	warp_userio_lcd_printline("  Rx Pkt Counts ", 16, 3, 1);
	
	//Set the user I/O board LEDs
	warp_userioboard_set_leds(userIOBoard_LEDs);
	
	//Set the default center frequency
	warpphy_setChannel(GHZ_2, chan);
	
	//Enable carrier sensing
	warpmac_setCSMA(1);

/*****************************************/
/******** START autoResponse setup *******/
/*****************************************/

	/*****************************************/
	//Header Match Unit Setup
	/*****************************************/
	// 0: Destination address == self
	// 1: Relay address == self
	// 2: Packet type == DATAPACKET
	// 3: Packet type == NACK
	
	//Match condition 0: received header's destination address is me
	autoResp_matchCond = PHY_AUTORESPONSE_MATCH_CONFIG(PKTHEADER_INDX_DSTADDR, 2, htons(NODEID_TO_ADDR(myID)));
	mimo_ofdmTxRx_setMatch0(autoResp_matchCond);

	//Match condition 1: received header's relay address is me
	autoResp_matchCond = PHY_AUTORESPONSE_MATCH_CONFIG(PKTHEADER_INDX_RLYADDR, 2, htons(NODEID_TO_ADDR(myID)));
	mimo_ofdmTxRx_setMatch1(autoResp_matchCond);
	
	//Match condition 2: packet type is data
	autoResp_matchCond = PHY_AUTORESPONSE_MATCH_CONFIG(PKTHEADER_INDX_TYPE, 1, DATAPACKET);
	mimo_ofdmTxRx_setMatch2(autoResp_matchCond);
	
	//Match condition 3: packet type is NACK
	autoResp_matchCond = PHY_AUTORESPONSE_MATCH_CONFIG(PKTHEADER_INDX_TYPE, 1, NACKPACKET);
	mimo_ofdmTxRx_setMatch3(autoResp_matchCond);

	/*****************************************/
	//Header translation setup
	/*****************************************/
	
	//Configure the header translator to use the Rx pkt's src address as the outgoing pkt's dst address
	// Addresses are two bytes, so two entries in the header translator need to be overridden
	// Except for these bytes, the ACK/NACK pktBuf's contents will be sent unaltered
	// Macro is PHY_HEADERTRANSLATE_SET(templatePktBuf, byteAddrToOverwrite, srcPktBuf, srcByteAddr)
	PHY_HEADERTRANSLATE_SET(pktBuf_tx_ACK, (PKTHEADER_INDX_DSTADDR+0), pktBuf_rx, (PKTHEADER_INDX_SRCADDR+0));
	PHY_HEADERTRANSLATE_SET(pktBuf_tx_ACK, (PKTHEADER_INDX_DSTADDR+1), pktBuf_rx, (PKTHEADER_INDX_SRCADDR+1));
	PHY_HEADERTRANSLATE_SET(pktBuf_tx_ACK, (PKTHEADER_INDX_RLYADDR+0), pktBuf_rx, (PKTHEADER_INDX_RLYADDR+0));
	PHY_HEADERTRANSLATE_SET(pktBuf_tx_ACK, (PKTHEADER_INDX_RLYADDR+1), pktBuf_rx, (PKTHEADER_INDX_RLYADDR+1));
	PHY_HEADERTRANSLATE_SET(pktBuf_tx_NACK, (PKTHEADER_INDX_DSTADDR+0), pktBuf_rx, (PKTHEADER_INDX_SRCADDR+0));
	PHY_HEADERTRANSLATE_SET(pktBuf_tx_NACK, (PKTHEADER_INDX_DSTADDR+1), pktBuf_rx, (PKTHEADER_INDX_SRCADDR+1));
	PHY_HEADERTRANSLATE_SET(pktBuf_tx_NACK, (PKTHEADER_INDX_RLYADDR+0), pktBuf_rx, (PKTHEADER_INDX_RLYADDR+0));
	PHY_HEADERTRANSLATE_SET(pktBuf_tx_NACK, (PKTHEADER_INDX_RLYADDR+1), pktBuf_rx, (PKTHEADER_INDX_RLYADDR+1));

	//Create a template ACK packet
	templatePkt.header.fullRate = pktFullRate;
	templatePkt.header.codeRate = pktCodeRate;
	templatePkt.header.length = 0;
	templatePkt.header.srcAddr = (unsigned short)(NODEID_TO_ADDR(myID));
	templatePkt.header.pktType = ACKPACKET;
	
	//Copy the header down to the PHY's packet buffer
	// This doesn't actually send anything; the autoResponse system will use this template when sending ACKs
	warpmac_prepPhyForXmit(&templatePkt, pktBuf_tx_ACK);

	//Create a template NACK packet
	templatePkt.header.fullRate = pktFullRate;
	templatePkt.header.codeRate = pktCodeRate;
	templatePkt.header.length = 0;
	templatePkt.header.srcAddr = (unsigned short)(NODEID_TO_ADDR(myID));
	templatePkt.header.pktType = NACKPACKET;
	
	//Copy the header down to the PHY's packet buffer
	// This doesn't actually send anything; the autoResponse system will use this template when sending ACKs
	warpmac_prepPhyForXmit(&templatePkt, pktBuf_tx_NACK);
	
	/*****************************************/
	//Actions setup
	// Action definitions come last; bad things might happen if an action is enabled before the template pkt is ready
	/*****************************************/
	//Actors:
	// 0: ACK transmission for good header, good payload DATA
	// 1: NACK transmission for good header, bad payload DATA
	// 2: DATA retransmisson from source
	// 3: SetFlagA at relay
	// 4: AF retransmission from relay
	
	autoResp_action = PHY_AUTORESPONSE_TXACTION_CONFIG(pktBuf_tx_ACK, PHY_AUTORESPONSE_ACT_TRANS_HDR, autoRespDelay_ACKTx, (REQ_DST_ME | REQ_DATA | PHY_AUTORESPONSE_REQ_GOODHDR | PHY_AUTORESPONSE_REQ_GOODPKT));
	mimo_ofdmTxRx_setAction0(autoResp_action);
//	mimo_ofdmTxRx_setAction0(0); //Use this to disable ACKs
	
	autoResp_action = PHY_AUTORESPONSE_TXACTION_CONFIG(pktBuf_tx_NACK, PHY_AUTORESPONSE_ACT_TRANS_HDR, autoRespDelay_NACKTx, (REQ_DST_ME | REQ_DATA | PHY_AUTORESPONSE_REQ_GOODHDR | PHY_AUTORESPONSE_REQ_BADPKT));
//	autoResp_action = PHY_AUTORESPONSE_TXACTION_CONFIG(pktBuf_tx_NACK, PHY_AUTORESPONSE_ACT_TRANS_HDR, autoRespDelay_NACKTx, (REQ_DST_ME | REQ_DATA | PHY_AUTORESPONSE_REQ_GOODHDR | PHY_AUTORESPONSE_REQ_GOODPKT)); //Use for NACKs in place of ACKs
	mimo_ofdmTxRx_setAction1(autoResp_action);
	
//	autoResp_action_reTx = PHY_AUTORESPONSE_TXACTION_CONFIG(pktBuf_tx_DATA, 0, autoRespDelay_ReTx, (REQ_DST_ME | REQ_NACK | PHY_AUTORESPONSE_REQ_GOODHDR | PHY_AUTORESPONSE_REQ_GOODPKT));
//	mimo_ofdmTxRx_setAction2(autoResp_action_reTx);
	
	autoResp_action = PHY_AUTORESPONSE_ACTION_SETFLAGA_CONFIG((REQ_RLY_ME | REQ_DATA | PHY_AUTORESPONSE_REQ_GOODHDR | PHY_AUTORESPONSE_REQ_GOODPKT));
	mimo_ofdmTxRx_setAction3(autoResp_action);
	
//	autoResp_action_coopTx = PHY_AUTORESPONSE_TXACTION_CONFIG(pktBuf_tx_AF, 0, autoRespDelay_AFTx, (REQ_RLY_ME | REQ_NACK | PHY_AUTORESPONSE_REQ_GOODHDR | PHY_AUTORESPONSE_REQ_GOODPKT | PHY_AUTORESPONSE_REQ_FLAGA));
//	autoResp_action_coopTx = PHY_AUTORESPONSE_TXACTION_CONFIG(pktBuf_rx, 0, autoRespDelay_AFTx, (REQ_RLY_ME | REQ_NACK | PHY_AUTORESPONSE_REQ_GOODHDR | PHY_AUTORESPONSE_REQ_GOODPKT | PHY_AUTORESPONSE_REQ_FLAGA));
//	mimo_ofdmTxRx_setAction4(autoResp_action_coopTx);
	
/*****************************************/
/******** END autoResponse setup *******/
/*****************************************/
	
	//Enable local packet generation (ignoring Ethernet)
	warpmac_setDummyPacketMode(1);
	
	//Listen for new packets to send (either from Ethernet or local dummy packets)
	warpmac_enableDataFromNetwork();
	
	//Set the modulation scheme use for base rate (header) symbols
//	warpmac_setBaseRate(QPSK);
	warpmac_setBaseRate(BPSK);
	
	XTime rand_time;
	XTime_GetTime(&rand_time);
	
	//Seed Random Number Generator
	srand(myID ^ ((int) rand_time) ^ WarpRadio_v1_RSSIData(RADIO2_ADDR));
	
	
	unsigned int pktBuf_payloadAddr;
	
	
	
	
	xil_printf("Reference Design v15.04 DOCMAC\r\n");
	xil_printf("Beginning main loop\r\n");
	
	
	//////
	ofdm_txrx_mimo_WriteReg_Rx_ControlBits(0, ofdm_txrx_mimo_ReadReg_Rx_ControlBits(0) & ~(COARSE_CFO_EN));
	xil_printf("Coarse CFO Disabled\r\n");

	//mimo_ofdmRx_setOptions(mimo_ofdmRx_getOptions() | BYPASS_CARR_REC, DEFAULT_STATUSBITS);
	//xil_printf("Carrier Rec Bypass\r\n");

	//////
	int i,j;
	////Load Up Random Data into Pkt Buffers 3-28////
	for (i=3; i<=28; i++) {
		unsigned char* byteLoc = (unsigned char*)warpphy_getBuffAddr(i);
		for (j=0; j<2000; j++) {
			*(byteLoc+j) = (unsigned char)(rand()&0xFF);
		}
	}
	/////////////////////////////////////////////////
	
	//Hard-code which nodes send which Alamouti streams - relay always sense stream B
	if(myID == 0 || myID == 1)
		warpphy_setTxAntennaSwap(TX_ANTMODE_ALAMOUTI_ANTA);
	else if(myID == 2)
		warpphy_setTxAntennaSwap(TX_ANTMODE_ALAMOUTI_ANTA); //CRH
	
	//if(myID == 0)
	//{
	//Be the source
	//	warpmac_startPacketGeneration(1300, 1500); //900,1500
	//}
	
	respondToNacks = 1;
	buzzerEnable = 0;
	
	docPreambleScaling=2480;
	docPayloadScaling=9792;
	mimo_ofdmTx_setTxScaling(docPreambleScaling,docPayloadScaling);
	warpphy_setAntBPreambleShift(antB_preambleShift);
	
	warpphy_setTxPower(txPower);
	
	
	//WARPnet configuration
	txEthPktHeader.ethType = WARPNET_ETHTYPE_NODE2SVR;
	txEthPktHeader.srcAddr[0]=0x00;
	txEthPktHeader.srcAddr[1]=0x50;
	txEthPktHeader.srcAddr[2]=0xC2;
	txEthPktHeader.srcAddr[3]=0x63;
	txEthPktHeader.srcAddr[4]=0x3F;
	txEthPktHeader.srcAddr[5]=0x80+myID;
	//HARDCODED TO MACBOOK!!
	txEthPktHeader.dstAddr[0]=0x00;
	txEthPktHeader.dstAddr[1]=0x50;
	txEthPktHeader.dstAddr[2]=0xc2;
	txEthPktHeader.dstAddr[3]=0x63;
	txEthPktHeader.dstAddr[4]=0x3f;
	txEthPktHeader.dstAddr[5]=0xef;	

	sleep(8);
	unsigned char ethernetAddr[] = {0x00,0x50,0xC2,0x63,0x3F,0x80+myID};
	warpnet_setMacAddr((void*)&ethernetAddr);
	warpnet_sendGratuitousArp(myID);

	///////
	
	allowRelay = 0;
	alternating = 0;
//	if(myID==0) warpmac_startPacketGeneration(1300, 10000);

warpphy_AFrecordEnable(0);
warpphy_AFrecordEnable(1);
	
	while(1){
		warpmac_pollPeripherals();		
	}
	
	return;
}