//now pick between old and new.
    if(pNewParent == NULL) {
	//use the old parent unless it is null;
	pNewParent = pOldParent;
        ulMinTotalCost = oldParentCost;
    } else if((pOldParent != NULL) &&
              (oldParentCost < (SWITCH_THRESHOLD + ulMinTotalCost))){
	//both valid, but use the old parent 
	pNewParent = pOldParent;
        ulMinTotalCost = oldParentCost;
    }

    if (pNewParent) {
      atomic {
        gpCurrentParent = pNewParent;
        gbCurrentHopCount = pNewParent->hop + 1;
        gbCurrentCost = ulMinTotalCost >> 6;
      }
    } else {
      atomic {
        gpCurrentParent = NULL;
        gbCurrentHopCount = ROUTE_INVALID;
        gbCurrentCost = ROUTE_INVALID;
      }

    }
  }

  uint8_t last_entry_sent;

  task void SendRouteTask() {
    TOS_MHopMsg *pMHMsg = (TOS_MHopMsg *) &routeMsg.data[0];
    RoutePacket *pRP = (RoutePacket *)&pMHMsg->data[0];
    struct SortEntry sortTbl[ROUTE_TABLE_SIZE];
    uint8_t length = offsetof(TOS_MHopMsg,data) + offsetof(RoutePacket,estList);
    uint8_t maxEstEntries;
    uint8_t i,j;
    uint8_t last_index_added = 0;

    if (gfSendRouteBusy) {
      return;
    }

    dbg(DBG_ROUTE,"MultiHopEWMA Sending route update msg.\n");
    dbg(DBG_ROUTE,"Current cost: %d.\n", gbCurrentCost);

    maxEstEntries = TOSH_DATA_LENGTH - length;
    maxEstEntries = maxEstEntries / sizeof(RPEstEntry);


    pRP->parent = (gpCurrentParent) ? gpCurrentParent->id : ROUTE_INVALID;
    pRP->cost = gbCurrentCost; 

    for (i = 0,j = 0;i < ROUTE_TABLE_SIZE && j < maxEstEntries; i++) {
      uint8_t table_index = i + last_entry_sent + 1;
      if(table_index >= ROUTE_TABLE_SIZE) table_index -=ROUTE_TABLE_SIZE;
      if (NeighborTbl[table_index].flags & NBRFLAG_VALID && NeighborTbl[table_index].receiveEst > 100) {
        pRP->estList[j].id = NeighborTbl[table_index].id;
        pRP->estList[j].receiveEst = NeighborTbl[table_index].receiveEst;
	j ++;
        length += sizeof(RPEstEntry);
	last_index_added = table_index;
        dbg(DBG_ROUTE,"Adding %d to route msg.\n", pRP->estList[j].id);
      }
    }
    last_entry_sent = last_index_added;
    dbg(DBG_ROUTE,"Added total of %d entries to route msg.\n", j);
    pRP->estEntries = j;
    pMHMsg->sourceaddr = pMHMsg->originaddr = TOS_LOCAL_ADDRESS;
    pMHMsg->hopcount = gbCurrentHopCount;
    pMHMsg->seqno = gCurrentSeqNo++;

    if (call SendMsg.send(TOS_BCAST_ADDR, length, &routeMsg) == SUCCESS) {
      gfSendRouteBusy = TRUE;
    }

  }


  task void SendDebugTask() {
    struct SortDbgEntry sortTbl[ROUTE_TABLE_SIZE];
    uint16_t max_length;
    uint8_t length = offsetof(DebugPacket,estList);
    DebugPacket *pRP = (DebugPacket *)call DebugSendMsg.getBuffer(&debugMsg,&max_length);
    uint8_t maxEstEntries;
    uint16_t parent;
    uint8_t i,j;

    dbg(DBG_ROUTE,"MultiHopEWMA Sending route debug msg.\n");

    maxEstEntries = max_length / sizeof(DBGEstEntry);
    maxEstEntries --;
    parent = (gpCurrentParent) ? gpCurrentParent->id : ROUTE_INVALID;

    for (i = 0,j = 0;i < ROUTE_TABLE_SIZE; i++) {
      if (NeighborTbl[i].flags & NBRFLAG_VALID && NeighborTbl[i].id != parent) {
        sortTbl[j].id = NeighborTbl[i].id;
        sortTbl[j].sendEst = NeighborTbl[i].sendEst;
        sortTbl[j].hopcount = NeighborTbl[i].hop;
        j++;
      }
    }
    qsort (sortTbl,j,sizeof(struct SortDbgEntry),sortDebugEstFcn);

    pRP->estEntries = (j > maxEstEntries) ? maxEstEntries : j;
    pRP->estList[0].id = parent;
    if(gpCurrentParent){
	pRP->estList[0].sendEst = gpCurrentParent->sendEst;
	pRP->estList[0].hopcount = gpCurrentParent->hop;
    }
    length += sizeof(DBGEstEntry);

    for (i = 0; i < pRP->estEntries; i++) {
      pRP->estList[i+1].id = sortTbl[i].id;
      pRP->estList[i+1].sendEst = sortTbl[i].sendEst;
      pRP->estList[i+1].hopcount = sortTbl[i].hopcount;
      length += sizeof(DBGEstEntry);
    }
    pRP->estEntries ++;
    call DebugSendMsg.send(&debugMsg, length);

  }

  int debugCounter;

  task void TimerTask() {
    
    dbg(DBG_ROUTE,"MultiHopEWMA timer task.\n");
    updateTable();

#ifndef NDEBUG
    {
      int i;
      dbg(DBG_ROUTE,"\taddr\tprnt\tcost\tmisd\trcvd\tlstS\thop\trEst\tsEst\tDesc\n");
      for (i = 0;i < ROUTE_TABLE_SIZE;i++) {
        if (NeighborTbl[i].flags) {
          dbg(DBG_ROUTE,"\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\n",
              NeighborTbl[i].id,
              NeighborTbl[i].parent,
              NeighborTbl[i].cost,
              NeighborTbl[i].missed,
              NeighborTbl[i].received,
              NeighborTbl[i].lastSeqno,
              NeighborTbl[i].hop,
              NeighborTbl[i].receiveEst,
              NeighborTbl[i].sendEst,
              NeighborTbl[i].childLiveliness);
        }
      }
      if (gpCurrentParent) {
        dbg(DBG_ROUTE,"MultiHopEWMA: Parent = %d\n",gpCurrentParent->id);
      }
    }
#endif
    chooseParent();
#ifdef MULTI_HOP_DEBUG
    if(TOS_LOCAL_ADDRESS != BASE_STATION_ADDRESS) debugCounter ++;
    if(debugCounter > 1){
	post SendDebugTask();
	debugCounter = 0;
    }else{
#endif //MULTI_HOP_DEBUG
    	post SendRouteTask();
#ifdef MULTI_HOP_DEBUG
    }
#endif //MULTI_HOP_DEBUG

  }


  command result_t StdControl.init() {

    memset((void *)NeighborTbl,0,(sizeof(TableEntry) * ROUTE_TABLE_SIZE));
    BaseStation.id = TOS_UART_ADDR;
    BaseStation.parent = TOS_UART_ADDR;
    BaseStation.flags = NBRFLAG_VALID;
    BaseStation.hop = 0;
    gpCurrentParent = NULL;
    gbCurrentHopCount = ROUTE_INVALID;
    gCurrentSeqNo = 0;
    gwEstTicks = 0;
    gUpdateInterval = DATA_TO_ROUTE_RATIO * DATA_FREQ;
    gfSendRouteBusy = FALSE;

    if (TOS_LOCAL_ADDRESS == BASE_STATION_ADDRESS) {

      gpCurrentParent = &BaseStation;
      gbCurrentHopCount = 0;
      gbCurrentCost = 0;

    }

    return SUCCESS;
  }

  command result_t StdControl.start() {
    post TimerTask();
    call Timer.start(TIMER_REPEAT,gUpdateInterval);
    return SUCCESS;
  }

  command result_t StdControl.stop() {
    call Timer.stop();
    return SUCCESS;
  }

  command bool RouteSelect.isActive() {
#if 0
    bool Result = FALSE;

    if (gpCurrentParent != NULL) {
      Result = TRUE;
    }

    return Result;
#endif

    return TRUE;
  }


  void updateDescendant(uint16_t id){
	uint8_t indes = findEntry(id);
	if (indes == (uint8_t) ROUTE_INVALID) { return;}
	else{
		NeighborTbl[indes].childLiveliness = MAX_DESCENDANT;
	}
  }

  command result_t RouteSelect.selectRoute(TOS_MsgPtr Msg, uint8_t id) {
    TOS_MHopMsg *pMHMsg = (TOS_MHopMsg *)&Msg->data[0];

    uint8_t iNbr;
    bool fIsDuplicate;
    result_t Result = SUCCESS;


    if (gpCurrentParent == NULL) {
      // If the msg is locally generated, then send it to the broadcast address
      // This is necessary to seed the network.
      if ((pMHMsg->sourceaddr == TOS_LOCAL_ADDRESS) &&
          (pMHMsg->originaddr == TOS_LOCAL_ADDRESS)) {
        pMHMsg->sourceaddr = TOS_LOCAL_ADDRESS;
        pMHMsg->hopcount = gbCurrentHopCount;
        pMHMsg->seqno = gCurrentSeqNo++;
        Msg->addr = TOS_BCAST_ADDR;
        return SUCCESS;
      }
      else {
        return FAIL;
      }
    }

    if (gbCurrentHopCount >= pMHMsg->hopcount) {
      // Possible cycle??
      return FAIL;
    }
    
    if ((pMHMsg->sourceaddr == TOS_LOCAL_ADDRESS) &&
        (pMHMsg->originaddr == TOS_LOCAL_ADDRESS)) {
      fIsDuplicate = FALSE;
    }
    else {
      fIsDuplicate = updateNbrCounters(pMHMsg->sourceaddr,pMHMsg->seqno,&iNbr);
    }

    if (!fIsDuplicate) {
      pMHMsg->sourceaddr = TOS_LOCAL_ADDRESS;
      pMHMsg->hopcount = gbCurrentHopCount;
      if (gpCurrentParent->id != TOS_UART_ADDR) {
        pMHMsg->seqno = gCurrentSeqNo++;
      }
      Msg->addr = gpCurrentParent->id;
      if(pMHMsg->originaddr != TOS_LOCAL_ADDRESS){
	  updateDescendant(pMHMsg->originaddr);
      }
    }
    else {
      Result = FAIL;
    }
    dbg(DBG_ROUTE,"MultiHopEWMA: Sequence Number: %d\n", pMHMsg->seqno);

    return Result;

  }

  command result_t RouteSelect.initializeFields(TOS_MsgPtr Msg, uint8_t id) {
    TOS_MHopMsg *pMHMsg = (TOS_MHopMsg *)&Msg->data[0];

    pMHMsg->sourceaddr = pMHMsg->originaddr = TOS_LOCAL_ADDRESS;
    pMHMsg->hopcount = ROUTE_INVALID;

    return SUCCESS;
  }

  command uint8_t* RouteSelect.getBuffer(TOS_MsgPtr Msg, uint16_t* Len) {

  }

  command uint16_t RouteControl.getParent() {

    uint16_t addr;

    addr = (gpCurrentParent != NULL) ? gpCurrentParent->id : 0xffff;

    return addr;
  }

  command uint8_t RouteControl.getQuality() {
    uint8_t val;

    val = (gpCurrentParent != NULL) ? gpCurrentParent->sendEst : 0x00;

    return val;
  }

  command uint8_t RouteControl.getDepth() {
    return gbCurrentHopCount;
  }

  command uint8_t RouteControl.getOccupancy() {
    return 0;
  }

  command uint16_t RouteControl.getSender(TOS_MsgPtr msg) {
    TOS_MHopMsg		*pMHMsg = (TOS_MHopMsg *)msg->data;
    return pMHMsg->sourceaddr;
  }

  command result_t RouteControl.setUpdateInterval(uint16_t Interval) {
    result_t Result;

    call Timer.stop();
    gUpdateInterval = (Interval * 1024);  // * 1024 to make the math simpler
    Result = call Timer.start(TIMER_REPEAT,gUpdateInterval);

    return Result;
  }

  command result_t RouteControl.manualUpdate() {
    result_t Result;

    Result = post TimerTask();
    return Result;
  }



  event result_t Timer.fired() {
    post TimerTask();
    return SUCCESS;
  }

  event TOS_MsgPtr ReceiveMsg.receive(TOS_MsgPtr Msg) {
    TOS_MHopMsg *pMHMsg = (TOS_MHopMsg *)&Msg->data[0];
    RoutePacket *pRP = (RoutePacket *)&pMHMsg->data[0];
    uint16_t saddr;
    uint8_t i, iNbr;

    saddr = pMHMsg->sourceaddr;

    updateNbrCounters(saddr,pMHMsg->seqno,&iNbr);
    //    iNbr = findPreparedIndex(saddr);

    NeighborTbl[iNbr].parent = pRP->parent;
    NeighborTbl[iNbr].hop = pMHMsg->hopcount;
    NeighborTbl[iNbr].cost = pRP->cost;

    // find out my address, extract the estimation
    for (i = 0; i < pRP->estEntries; i++) {
      if (pRP->estList[i].id == TOS_LOCAL_ADDRESS) {
        NeighborTbl[iNbr].sendEst = pRP->estList[i].receiveEst;
        NeighborTbl[iNbr].liveliness = LIVELINESS;
      }
    }

    return Msg;
  }

  event result_t Snoop.intercept[uint8_t id](TOS_MsgPtr Msg, void *Payload, uint16_t Len) {
    TOS_MHopMsg *pMHMsg = (TOS_MHopMsg *)&Msg->data[0];
    uint8_t iNbr;

    updateNbrCounters(pMHMsg->sourceaddr,pMHMsg->seqno,&iNbr);

    return SUCCESS;
  }


  event result_t SendMsg.sendDone(TOS_MsgPtr pMsg, result_t success) {
    gfSendRouteBusy = FALSE;

    return SUCCESS;
  }
  event result_t DebugSendMsg.sendDone(TOS_MsgPtr pMsg, result_t success) {
    return SUCCESS;
  }

}