/** This function takes care of all processing when a message is received.
	It is called after Lithne.available();
	This section is based on examples in the XBee library by Andrew Rapp**/
void LithneClass::readXbee()
{
/*DONT EDIT IF YOU DON'T KNOW WHAT YOU'RE DOING!!!	*/

/* Reads all available serial bytes until a packet is parsed, an error occurs, or the buffer is empty. */
xbee.readPacket();  

if( newXBeePacket ) { newXBeePacket = false; } // Set this flag to false; only raise it if we receive a message

if (xbee.getResponse().isAvailable()) //Returns a reference to the current response Note: once readPacket is called again this response will be overwritten!
{
	newXBeePacket = true;

	int responseType	=	xbee.getResponse().getApiId();

  	if (responseType == ZB_RX_RESPONSE)
    { //Call with instance of ZBRxResponse class only if getApiId() == ZB_RX_RESPONSE to populate response.

     	/* Indicate we have received a new message */
     	newMessage = true; // set flag
     	
     	/* Clear the old received message */
		incomingMessage.clearArguments();
     	
     	xbee.getResponse().getZBRxResponse(rx);
     	
     	/* Retrieve the sender from the packet and store it in the message */
     	XBeeAddress64	addr64	=	rx.getRemoteAddress64();
     	uint16_t		addr16	=	rx.getRemoteAddress16();

		/*  if the 16 bit add shows this is the coordinator, we store the default
			coordinator address 0x0 0x0 instead of the hardware address       */
		if (addr16 == 0) 
		{
			addr64 = XBeeAddress64(0x0, 0x0);
		}
     	

		// Serial.println("-----------");
		
  //   	for( uint16_t i = 0; i <rx.getDataLength(); i++ )
  //     	{
    		
		// 	Serial.print( rx.getData(i) );
		// 	Serial.print( " " );
  //   	}
  //   	Serial.println("");
  //   	Serial.println("-----------");

	incomingMessage.setSender( addr16, addr64 );
     	 
     	/*	The scope of the message is stored in the first two bytes
     		of the payload.	Here we retrieve this and write it to the
     		incoming message	*/
	incomingMessage.setScope( (rx.getData(Message::SCOPE_MSB) << 8) + rx.getData(Message::SCOPE_LSB) );
     	
		/* 	The function identifier (1 byte) is stored in the third
			byte of the payload. Here we retrieve this and write it to
			the incoming message	*/
	incomingMessage.setFunction((rx.getData(Message::FUNCTION_MSB) << 8) + rx.getData(Message::FUNCTION_LSB) );

      	/*	The remainder of the payload contains our arguments. Here
      		we retrieve the number of arguments, by subtracting the 
      		first three bytes (which contain the scope and the function
      		ID) and dividing the number of bytes by two, since we send
      		16-bit values and not 8-bit values (2x 8-bits = 1x 16bits)	*/
      	uint16_t numOfBytes	=	(rx.getDataLength()-Message::MSG_HEADER_SIZE);
		
		/* Store the arguments in the incomingMessage */
    	for( uint16_t i = 0; i < numOfBytes; i++ )
      	{
    		uint16_t pos    =	i + Message::MSG_HEADER_SIZE;
		incomingMessage.addByte( rx.getData(pos) );
    	}
    	
    	/*	Here we always overwrite the 16-bit address. The received address 
    		is directly taken from the header information and thus correct.	*/
    	
		Node * senderNode = getNodeBy64( addr64 );
		if (senderNode != NULL) 
		{	
			senderNode->setAddress16( addr16 );
		}
		// delete senderNode;
    	
    	/* If the sender(!) of the message got an acknowledgement, this code is executed */
    	if (rx.getOption() == ZB_PACKET_ACKNOWLEDGED)
		{
			
		}
		/*	If not, something strange has happened, because we got the message, but
			the sender did not receive an acknowledgement */
      	else
      	{
        	//we got it (obviously) but sender didn't get an ACK
      	}
    }
    
    /*	If the packet is indeed an AT_COMMAND_RESPONSE,	we write it to the atResponse.	*/
	else if (responseType == AT_COMMAND_RESPONSE) 
	{
		AtCommandResponse atResponse 	=	AtCommandResponse();
		uint32_t atAnswer				=	0;
		
		xbee.getResponse().getAtCommandResponse( atResponse );

		/*	If the atResponse is correct (does not return an error code), we can process it.	*/
		if (atResponse.isOk()) 
		{
			newATMessage = true;
			if (atResponse.getValueLength() > 0) 
			{
				for (int i = 0; i < atResponse.getValueLength(); i++) 
				{
					atAnswer	=	(atAnswer << 8) + atResponse.getValue()[i];
				}
       		}
       		
       		/*	If we got a Serial High	('S','H')	*/
       		if( atResponse.getCommand()[0] == atSH[0] &&
       			atResponse.getCommand()[1] == atSH[1] )
       		{
       			myAddress64.setMsb( atAnswer );
       		}
       		/*	If we got a Serial LOW ('S','L')	*/
       		if( atResponse.getCommand()[0] == atSL[0] &&
       			atResponse.getCommand()[1] == atSL[1] )
       		{
       			myAddress64.setLsb( atAnswer );
       		}
       		/*	If we got my 16-bit address ('M','Y')	*/
       		if( atResponse.getCommand()[0] == atMY[0] &&
       			atResponse.getCommand()[1] == atMY[1] )
       		{
       			myAddress16	=	atAnswer & 0xFFFF;
       		}
       		/*	If we got my 16-bit address ('M','Y')	*/
       		if( atResponse.getCommand()[0] == atID[0] &&
       			atResponse.getCommand()[1] == atID[1] )
       		{
       			myPANid		=	atAnswer & 0xFFFF;
       		}
       		/*	If we got my 16-bit address ('M','Y')	*/
       		if( atResponse.getCommand()[0] == atAI[0] &&
       			atResponse.getCommand()[1] == atAI[1] )
       		{
       			myAssStat	=	atAnswer & 0xFFFF;
       		}
		}
		else {	} 
	}
    
    /* NODE JOINING/LEAVING THE NETWORK */
	else if( responseType == MODEM_STATUS_RESPONSE )
	{
    	xbee.getResponse().getModemStatusResponse(msr);

       	if (msr.getStatus() == ASSOCIATED) 
       	{
        	/* A new node has joined the network.
        		Here we add the node if it is not yet known and store the 
        		64-bit and 16-bit address
        	*/
//        	xbee.getResponse().;
       	} 
	  	else if (msr.getStatus() == DISASSOCIATED) 
	  	{
         	/* Node leaves the network
         	*/
       	} 
	}
	/* DB MEASUREMENT RESPONSE */
	else if( responseType == REMOTE_AT_COMMAND_RESPONSE ) 	//REMOTE_AT_COMMAND_RESPONSE
	{	
		// Serial.println("Received Remote AT Command Response");
		xbee.getResponse().getRemoteAtCommandResponse(rATcmd);
		if (rATcmd.isOk()) 
		{
	      	if ( rATcmd.getCommand()[0]	==	atDB[0] &&
	      		 rATcmd.getCommand()[1]	==	atDB[1] )
	      	{
				newRemoteATMessage = true;
		      	XBeeAddress64 rAddress64	=	rATcmd.getRemoteAddress64();
		      	uint16_t rAddress16			=	rATcmd.getRemoteAddress16();

				// Serial.print("AT DB from ");
				// Serial.print(rAddress64.getMsb(), HEX);
				// Serial.println(rAddress64.getLsb(), HEX);
				
				// This is now done at the end of this section
				//getNodeBy64( rAddress64 )->addDBMeasurement( rATcmd.getValue()[0] );
				
				/*	The line above replaces this section
				for (int i = 0; i < numNodes; i++)
				{
					if( nodes[i]->getID() == remoteId )
					{
		    			nodes[i]->addDBMeasurement( rATcmd.getValue()[0] );
			    	}
		      	}
		      	*/
		      	/* we use this function also to relate 16 bit addresses to 64 bit addresses */
		      	if( !nodeKnown16( rAddress16 ) && nodeKnown64( rAddress64 ) )
		      	{
		      		getNodeBy64( rAddress64 )->setAddress16( rAddress16 );
		      		// Serial.println("We know the 64-bit address, but not the 16-bit - now setting");
		      	}
				/* If we receive a message from the coordinator; it has 16-bit add 0. We related this to the node with 64 bit address 0x0 0x0; the coordinator. */
				else if ( rAddress16 == 0 && !nodeKnown16( rAddress16 ) )
				{ 			
					Node * defaultCoordinator = getNodeBy64( XBeeAddress64(0x0,0x0) );
					if (defaultCoordinator != NULL) 
					{
						defaultCoordinator->setAddress16( rAddress16 );
					}
					/*Serial.print("Set 16 bit add of coordinator to ");
					Serial.print( getNodeBy64( XBeeAddress64(0x0,0x0) )->getAddress16(  ) ); 
					Serial.print(" which is Node ID ");
					Serial.print( getNodeBy64( XBeeAddress64(0x0,0x0) )->getID(  ) );  */
				}
				
				/*Serial.print("Received Remote DB from nodeId ");
				Serial.print( getNodeBy16( rAddress16 )->getID() );
				Serial.print(" (64b: ");
				Serial.print( rAddress64.getLsb(), HEX );
				Serial.print(", 16b: ");
				Serial.print( rAddress16, DEC ); 
				
				Serial.print( ") DB val: ");
				Serial.println( rATcmd.getValue()[0] ); */
				
				Node * remoteNode = getNodeBy16( rAddress16 );
				if ( remoteNode != NULL) 
				{
					remoteNode->addDBMeasurement( rATcmd.getValue()[0] );
				}
		    }
		    /*	Here we can add other remote AT command responses
		    else if( rATcmd.getCommand()[0]	==	atSH[0] &&
	      		 	 rATcmd.getCommand()[1]	==	atSH[1] )
			{
			
			}
			*/
		}
    }
    /*	Confirmation on transmitted package. This is received
    	everytime a message is transmitted and has details
    	whether the package is received or not.
	*/
    else if (responseType == ZB_TX_STATUS_RESPONSE)
    {
    	// Serial.println("Received ZBTxStatusResponse");

	    xbee.getResponse().getZBTxStatusResponse(txStatus);
     	
     	if (txStatus.getDeliveryStatus() == SUCCESS) 
	 	{
			/* Als we hier zijn aangekomen, hebben we een berichtje gestuurd,
	 		dat is goed aangekomen. Deze response geeft echter alleen een
	 		16-bit address terug en geen 64-bit, dus die kunnen we niet aan
	 		een node koppelen.
	 		Nu willen we kijken of de 16-bit bekend is in onze node lijst.
	 		Als dat zo is, dan hoeven we niets te doen, we kennen deze node.
	 		Als dat NIET zo is, dan willen we van deze node ook het 64-bit
	 		address opvragen, zodat we 16-bit en 64-bit op kunnen slaan.
	 		*/
			
	 		uint16_t rAddress16	=	txStatus.getRemoteAddress();	//Stores the 16-bit address

	 		setMessageDelivered( true );
	 		
	 		if( !nodeKnown16( rAddress16 ) )
	 		{
	 			/*Here we send something that requests data from the remote node
				We can link the two addresses in the returned data (remote at command response)*/
	 			sendDBRequest16( rAddress16 );
	 		}
		}      	
      	else 
	  	{
        	/* the remote XBee did not receive our packet. 
        		If this is because the 16 bit address is outdated; we wish to reset that */
       		uint16_t rAddress16	=	txStatus.getRemoteAddress();	//Get the 16-bit address
			if( nodeKnown16( rAddress16 ) ) // If we know that address, but the message was not received
			{
				Node * remoteNode = getNodeBy16( rAddress16 );
				if ( remoteNode != NULL) 
				{
					remoteNode->setAddress16( UNKNOWN_16B ); // We reset the 16 bit address so it will use the 64 bit again
				}
			}
     	}
    }
    //Something occured that is unknown, or we do not care about
    else
    {
    }
  }
}
MACTransmissionStatus XBeeMACLayer::send(const uip_lladdr_t* lladdr_dest, uint8_t* data, uint16_t length, int *number_transmissions){
  unsigned long send_start_time;
  unsigned int sending_time;
  uint8_t status;

  // Broadcast is expressed as an all-zeroes address (rimeaddr_null), we check to see if lladdr_dest is a broadcast address comparing it to rimeaddr_null.
  bool isNotBroadcast = !rimeaddr_cmp((rimeaddr_t*)lladdr_dest, &rimeaddr_null, UIP_LLADDR_LEN); // lladdr_dest != NULL

    if (isNotBroadcast){
      // lladdr_dest is NOT a broadcast address
      if (UIP_LLADDR_LEN == UIP_802154_LONGADDR_LEN){
          ((char*)(&dest))[3] = lladdr_dest->addr[0];
          ((char*)(&dest))[2] = lladdr_dest->addr[1];
          ((char*)(&dest))[1] = lladdr_dest->addr[2];
          ((char*)(&dest))[0] = lladdr_dest->addr[3];
          destAddr64.setMsb(dest);
          ((char*)(&dest))[3] = lladdr_dest->addr[4];
          ((char*)(&dest))[2] = lladdr_dest->addr[5];
          ((char*)(&dest))[1] = lladdr_dest->addr[6];
          ((char*)(&dest))[0] = lladdr_dest->addr[7];
          destAddr64.setLsb(dest);
        Tx64Request longReq(destAddr64, data, length);
        xbeeRequest = &longReq;
      }else{
        ((char*)(&dest))[1] = lladdr_dest->addr[0];
        ((char*)(&dest))[0] = lladdr_dest->addr[1];
        Tx16Request shortReq(dest16, data, length);
        xbeeRequest = &shortReq;
      }
    }else{// lladdr_dest is a broadcast address, we send to broadcast
      Tx16Request shortReq(0xFFFF, data, length);
      xbeeRequest = &shortReq;
    }
    send_start_time = millis();
    xbee.send(*xbeeRequest);


    // after sending a tx request, we expect a status response
    // wait up to 5 seconds for the status response
    for(int i=0; i<AT_RESPONSE_MAX_ATTEMPTS; ++i){
      if (xbee.readPacket(5000)) {
          // got a response!
          // should be a znet tx status            	
      	if (xbee.getResponse().getApiId() == TX_STATUS_RESPONSE) {
      	   xbee.getResponse().getTxStatusResponse(txStatus);
      	   // get the delivery status, the fifth byte
             switch (txStatus.getStatus()) {
               case XBEE_SEND_OK:
                 *number_transmissions = getNumberOfTransmissions();
                 ++number_transmissions;//we have always one (the first one), plus the number of times that we had cca failure (collision)

                 return MAC_TX_STATUS_OK;
               case XBEE_SEND_NO_ACK:
                 return MAC_TX_STATUS_NO_ACK;
               case XBEE_SEND_COLLISION:
                 *number_transmissions = getNumberOfTransmissions();
                 ++number_transmissions;//we have always one (the first one), plus the number of times that we had cca failure (collision)

                 return MAC_TX_STATUS_COLLISION;
               case XBEE_SEND_PURGED:
               default:
                 return MAC_TX_STATUS_ERR;
             }     
          }   
      }
    }
    return MAC_TX_STATUS_ERR;

}