call CollectionPacket.getOrigin(msg), 
					 call AMPacket.destination(msg));
	dbg("LQI", "%s: Send to %hu success.\n", __FUNCTION__, call AMPacket.destination(msg));
        fwdbusy = TRUE;
      }
    }
  }

  event message_t* SubReceive.receive(message_t* ONE msg, void* COUNT_NOK(len) payload, uint8_t len) {
    collection_id_t id = call CollectionPacket.getType(msg);
    payload += sizeof(lqi_header_t);
    len -= sizeof(lqi_header_t);

    call CollectionDebug.logEventMsg(NET_C_FE_RCV_MSG, 
				     call CollectionPacket.getSequenceNumber(msg), 
				     call CollectionPacket.getOrigin(msg), 
				     call AMPacket.destination(msg));
    if (call RootControl.isRoot()) {
      dbg("LQI,LQIDeliver", "LQI Root is receiving packet from node %hu @%s\n", getHeader(msg)->originaddr, sim_time_string());
      return signal Receive.receive[id](msg, payload, len);
    }
    else if (call AMPacket.destination(msg) != call AMPacket.address()) {
      return msg;
    }
    else if (signal Intercept.forward[id](msg, payload, len)) {
      dbg("LQI,LQIDeliver", "LQI fwd is forwarding packet from node %hu @%s\n", getHeader(msg)->originaddr, sim_time_string());
      return mForward(msg);
    }
    else {
      return msg;
    }
  }
  
  message_t* nextMsg() {
    int i;
    uint16_t inc = call Random.rand16() & 0xfff;
    for (i = 0; i < FWD_QUEUE_SIZE; i++) {
      int pindex = (i + inc) % FWD_QUEUE_SIZE;
      if (FwdBufBusy[pindex]) {
	return FwdBufList[pindex];
      }
    }
    return NULL;
  }
  
  event void SubSend.sendDone(message_t* msg, error_t success) {
    int8_t buf;
    message_t* nextToSend;
    if (!call PacketAcknowledgements.wasAcked(msg) &&
	call AMPacket.destination(msg) != TOS_BCAST_ADDR &&
	fwd_fail_count < MAX_RETRIES){
      call RouteSelect.selectRoute(msg, 1);
      call PacketAcknowledgements.requestAck(msg);
      if (call SubSend.send(call AMPacket.destination(msg),
			    msg,
			    call SubPacket.payloadLength(msg)) == SUCCESS) {
	dbg("LQI", "Packet not acked, retransmit @%s:\n\t%s\n", sim_time_string(), fields(msg));
        call CollectionDebug.logEventMsg(NET_C_FE_SENDDONE_WAITACK, 
					 call CollectionPacket.getSequenceNumber(msg), 
					 call CollectionPacket.getOrigin(msg), 
                                         call AMPacket.destination(msg));
	fwd_fail_count ++;
	return;
      } else {
	call CollectionDebug.logEventMsg(NET_C_FE_SENDDONE_FAIL, 
					 call CollectionPacket.getSequenceNumber(msg), 
					 call CollectionPacket.getOrigin(msg), 
                                         call AMPacket.destination(msg));
	dbg("LQI", "Packet not acked, retransmit fail @%s:\n\t%s\n", sim_time_string(), fields(msg));
	sendFailures++;
	return;
      }
    }
    else if (fwd_fail_count >= MAX_RETRIES) {
      call CollectionDebug.logEventMsg(NET_C_FE_SENDDONE_FAIL_ACK_FWD, 
				       call CollectionPacket.getSequenceNumber(msg), 
				       call CollectionPacket.getOrigin(msg), 
				       call AMPacket.destination(msg));
      dbg("LQI", "Packet failed:\t%s\n", fields(msg));
    }
    else if (call PacketAcknowledgements.wasAcked(msg)) {
      dbg("LQI", "Packet acked:\t%s\n", fields(msg));
      call CollectionDebug.logEventMsg(NET_C_FE_FWD_MSG, 
				       call CollectionPacket.getSequenceNumber(msg), 
				       call CollectionPacket.getOrigin(msg), 
				       call AMPacket.destination(msg));
    }
    
    fwd_fail_count = 0;
    buf = is_ours(msg);
    if (buf != -1) {
      FwdBufBusy[(uint8_t)buf] = 0;
    }
    
    nextToSend = nextMsg();
    fwdbusy = FALSE;
	  
    if (nextToSend != NULL) {
      forward(nextToSend);
    }
    
    dbg("LQI", "Packet not longer busy:\t%s\n", fields(msg));
  }

  event void SubSendMine.sendDone(message_t* msg, error_t success) {
    if (!call PacketAcknowledgements.wasAcked(msg) &&
	call AMPacket.destination(msg) != TOS_BCAST_ADDR &&
	my_fail_count < MAX_RETRIES){
      call RouteSelect.selectRoute(msg, 1);
      call PacketAcknowledgements.requestAck(msg);
      if (call SubSendMine.send(call AMPacket.destination(msg),
			    msg,
			    call SubPacket.payloadLength(msg)) == SUCCESS) {
	dbg("LQI", "Local packet not acked, retransmit (%hhu) @%s:\n\t%s\n", my_fail_count, sim_time_string(), fields(msg));
	call CollectionDebug.logEventMsg(NET_C_FE_SENDDONE_WAITACK, 
					 call CollectionPacket.getSequenceNumber(msg), 
					 call CollectionPacket.getOrigin(msg), 
                                         call AMPacket.destination(msg));
	my_fail_count ++;
	return;
      } else {
	call CollectionDebug.logEventMsg(NET_C_FE_SENDDONE_FAIL, 
					 call CollectionPacket.getSequenceNumber(msg), 
					 call CollectionPacket.getOrigin(msg), 
                                         call AMPacket.destination(msg));
	dbg("LQI", "Local packet not acked, retransmit fail @%s:\n\t%s\n", sim_time_string(), fields(msg));
	sendFailures++;
	signal Send.sendDone(msg, FAIL);
	return;
      }
    }
    else if (my_fail_count >= MAX_RETRIES) {
      call CollectionDebug.logEventMsg(NET_C_FE_SENDDONE_FAIL_ACK_SEND, 
				       call CollectionPacket.getSequenceNumber(msg), 
				       call CollectionPacket.getOrigin(msg), 
				       call AMPacket.destination(msg));
      dbg("LQI", "Local packet failed:\t%s\n", fields(msg));
    }
    else if (call PacketAcknowledgements.wasAcked(msg)) {
      dbg("LQI", "Local packet acked:\t%s\n", fields(msg));
      call CollectionDebug.logEventMsg(NET_C_FE_SENT_MSG, 
				       call CollectionPacket.getSequenceNumber(msg), 
				       call CollectionPacket.getOrigin(msg), 
				       call AMPacket.destination(msg));
    }

    my_fail_count = 0;
    dbg("LQI", "Local send done with success %d\n", success);
    signal Send.sendDone(msg, success);
  }


  command uint16_t RouteControl.getParent() {
    return call RouteSelectCntl.getParent();
  }

  command uint8_t RouteControl.getQuality() {
    return call RouteSelectCntl.getQuality();
  }

  command uint8_t RouteControl.getDepth() {
    return call RouteSelectCntl.getDepth();
  }

  command uint8_t RouteControl.getOccupancy() {
    uint16_t uiOutstanding = (uint16_t)iFwdBufTail - (uint16_t)iFwdBufHead;
    uiOutstanding %= FWD_QUEUE_SIZE;
    return (uint8_t)uiOutstanding;
  }


  command error_t RouteControl.setUpdateInterval(uint16_t Interval) {
    return call RouteSelectCntl.setUpdateInterval(Interval);
  }

  command error_t RouteControl.manualUpdate() {
    return call RouteSelectCntl.manualUpdate();
  }

  command uint16_t LqiRouteStats.getSendFailures() {
    return sendFailures;
  }

  command void Packet.clear(message_t* msg) {
    
  }

  command void* Send.getPayload(message_t* m, uint8_t len) {
    return call Packet.getPayload(m, len);
  }

  command uint8_t Send.maxPayloadLength() {
    return call Packet.maxPayloadLength();
  }

  command error_t Send.cancel(message_t* m) {
    return FAIL;
  }

  
  command uint8_t Packet.payloadLength(message_t* msg) {
    return call SubPacket.payloadLength(msg) - sizeof(lqi_header_t);
  }
  command void Packet.setPayloadLength(message_t* msg, uint8_t len) {
    call SubPacket.setPayloadLength(msg, len + sizeof(lqi_header_t));
  }
  command uint8_t Packet.maxPayloadLength() {
    return (call SubPacket.maxPayloadLength() - sizeof(lqi_header_t));
  }
  command void* Packet.getPayload(message_t* msg, uint8_t len) {
    void* rval = call SubPacket.getPayload(msg, len + sizeof(lqi_header_t));
    if (rval != NULL) {
      rval += sizeof(lqi_header_t);
    }
    return rval;
  }

  command am_addr_t CollectionPacket.getOrigin(message_t* msg) {
    lqi_header_t* hdr = getHeader(msg);
    return hdr->originaddr;  
  }

  command void CollectionPacket.setOrigin(message_t* msg, am_addr_t addr) {
    lqi_header_t* hdr = getHeader(msg);
    hdr->originaddr = addr;
  }

  command collection_id_t CollectionPacket.getType(message_t* msg) {
    return getHeader(msg)->collectId;
  }

  command void CollectionPacket.setType(message_t* msg, collection_id_t id) {
    getHeader(msg)->collectId = id;
  }
  
  command uint8_t CollectionPacket.getSequenceNumber(message_t* msg) {
    lqi_header_t* hdr = getHeader(msg);
    return hdr->originseqno;
  }
  
  command void CollectionPacket.setSequenceNumber(message_t* msg, uint8_t seqno) {
    lqi_header_t* hdr = getHeader(msg);
    hdr->originseqno = seqno;
  }


  
 default event void Send.sendDone(message_t* pMsg, error_t success) {}
 default event message_t* Snoop.receive[collection_id_t id](message_t* pMsg, void* payload, uint8_t len) {return pMsg;}
 default event message_t* Receive.receive[collection_id_t id](message_t* pMsg, void* payload, uint8_t len) {
   return pMsg;
 }
 default event bool Intercept.forward[collection_id_t id](message_t* pMsg, void* payload, uint8_t len) {
   return 1;
 }

  /* Default implementations for CollectionDebug calls.
   * These allow CollectionDebug not to be wired to anything if debugging
   * is not desired. */
    
  default command error_t CollectionDebug.logEvent(uint8_t type) {
    return SUCCESS;
  }
  default command error_t CollectionDebug.logEventSimple(uint8_t type, uint16_t arg) {
    return SUCCESS;
  }
  default command error_t CollectionDebug.logEventDbg(uint8_t type, uint16_t arg1, uint16_t arg2, uint16_t arg3) {
    return SUCCESS;
  }
  default command error_t CollectionDebug.logEventMsg(uint8_t type, uint16_t msg, am_addr_t origin, am_addr_t node) {
    return SUCCESS;
  }
  default command error_t CollectionDebug.logEventRoute(uint8_t type, am_addr_t parent, uint8_t hopcount, uint16_t metric) {
    return SUCCESS;
  }
}