// $Id: LqiForwardingEngineP.nc,v 1.15 2008/06/11 00:46:25 razvanm Exp $

/* Copyright (c) 2007 Stanford University.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 * - Redistributions of source code must retain the above copyright
 *   notice, this list of conditions and the following disclaimer.
 * - Redistributions in binary form must reproduce the above copyright
 *   notice, this list of conditions and the following disclaimer in the
 *   documentation and/or other materials provided with the
 *   distribution.
 * - Neither the name of the Stanford University nor the names of
 *   its contributors may be used to endorse or promote products derived
 *   from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL STANFORD
 * UNIVERSITY OR ITS CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
 * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
 * OF THE POSSIBILITY OF SUCH DAMAGE.
 */

/*
 * "Copyright (c) 2000-2003 The Regents of the University  of California.  
 * All rights reserved.
 *
 * Permission to use, copy, modify, and distribute this software and its
 * documentation for any purpose, without fee, and without written agreement is
 * hereby granted, provided that the above copyright notice, the following
 * two paragraphs and the author appear in all copies of this software.
 * 
 * IN NO EVENT SHALL THE UNIVERSITY OF CALIFORNIA BE LIABLE TO ANY PARTY FOR
 * DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES ARISING OUT
 * OF THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF THE UNIVERSITY OF
 * CALIFORNIA HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 * 
 * THE UNIVERSITY OF CALIFORNIA SPECIFICALLY DISCLAIMS ANY WARRANTIES,
 * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY
 * AND FITNESS FOR A PARTICULAR PURPOSE.  THE SOFTWARE PROVIDED HEREUNDER IS
 * ON AN "AS IS" BASIS, AND THE UNIVERSITY OF CALIFORNIA HAS NO OBLIGATION TO
 * PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS."
 *
 * Copyright (c) 2002-2003 Intel Corporation
 * All rights reserved.
 *
 * This file is distributed under the terms in the attached INTEL-LICENSE     
 * file. If you do not find these files, copies can be found by writing to
 * Intel Research Berkeley, 2150 Shattuck Avenue, Suite 1300, Berkeley, CA, 
 * 94704.  Attention:  Intel License Inquiry.
 */

/* 
 * A simple module that handles multihop packet movement.  It accepts 
 * messages from both applications and the network and does the necessary
 * interception and forwarding.
 * It interfaces to an algorithmic componenet via RouteSelect. It also acts
 * as a front end for RouteControl
 */


/**
 * @author Philip Buonadonna
 * @auihor Alec Woo
 * @author Crossbow Inc.
 * @author Philip Levis (port from TinyOS 1.x)
 */

#include "AM.h"
#include "MultiHopLqi.h"
#include "CollectionDebugMsg.h"

module LqiForwardingEngineP {
  provides {
    interface Init;
    interface Send;
    interface Receive[collection_id_t id];
    interface Receive as Snoop[collection_id_t];
    interface Intercept[collection_id_t id];
    interface CollectionPacket;
    interface RouteControl;
    interface LqiRouteStats;
    interface Packet;
  }
  uses {
    interface SplitControl;
    interface Receive as SubReceive;
    interface AMSend as SubSend;
    interface AMSend as SubSendMine;
    interface RouteControl as RouteSelectCntl;
    interface RouteSelect;
    interface Leds;
    interface Packet as SubPacket;
    interface AMPacket;
    interface RootControl;
    interface Random;
    interface PacketAcknowledgements;
    interface CollectionDebug;
  }
}

implementation {

  enum {
    FWD_QUEUE_SIZE = MHOP_QUEUE_SIZE, // Forwarding Queue
    EMPTY = 0xff,
    MAX_RETRIES = 5
  };

  /* Internal storage and scheduling state */
  message_t FwdBuffers[FWD_QUEUE_SIZE];
  message_t *FwdBufList[FWD_QUEUE_SIZE];
  uint8_t FwdBufBusy[FWD_QUEUE_SIZE];
  uint8_t iFwdBufHead, iFwdBufTail;
  uint16_t sendFailures = 0;
  uint8_t fwd_fail_count = 0;
  uint8_t my_fail_count = 0;
  bool fwdbusy = FALSE;
  bool running = FALSE;
 
  lqi_header_t* getHeader(message_t* msg) {
    return (lqi_header_t*) call SubPacket.getPayload(msg, sizeof(lqi_header_t));
  }
  
  /***********************************************************************
   * Initialization 
   ***********************************************************************/


  static void initialize() {
    int n;

    for (n=0; n < FWD_QUEUE_SIZE; n++) {
      FwdBufList[n] = &FwdBuffers[n];
      FwdBufBusy[n] = 0;
    } 
    iFwdBufHead = iFwdBufTail = 0;

    sendFailures = 0;
  }

  command error_t Init.init() {
    initialize();
    return SUCCESS;
  }
 
  message_t* nextMsg();
  static void forward(message_t* msg);

  event void SplitControl.startDone(error_t err) {
    message_t* nextToSend;
    if (err != SUCCESS) {return;}
    nextToSend = nextMsg();
    running = TRUE;
    fwdbusy = FALSE;

    if (nextToSend != NULL) {
      forward(nextToSend);
    }
  }


  event void SplitControl.stopDone(error_t err) {
    if (err != SUCCESS) {return;}
    running = FALSE;
  }
  /***********************************************************************
   * Commands and events
   ***********************************************************************/
  command error_t Send.send(message_t* pMsg, uint8_t len) {
    len += sizeof(lqi_header_t);
    if (len > call SubPacket.maxPayloadLength()) {
      return ESIZE;
    }
    if (call RootControl.isRoot()) {
      return FAIL;
    }
    if (running == FALSE) {
      return EOFF;
    }
    call RouteSelect.initializeFields(pMsg);
    
    if (call RouteSelect.selectRoute(pMsg, 0) != SUCCESS) {
      return FAIL;
    }
    call PacketAcknowledgements.requestAck(pMsg);
    if (call SubSendMine.send(call AMPacket.destination(pMsg), pMsg, len) != SUCCESS) {
      sendFailures++;
      return FAIL;
    }

    return SUCCESS;
  } 
  
  int8_t get_buff(){
    uint8_t n;
    for (n=0; n < FWD_QUEUE_SIZE; n++) {
	uint8_t done = 0;
        atomic{
	  if(FwdBufBusy[n] == 0){
	    FwdBufBusy[n] = 1;
	    done = 1;
	  }
        }
	if(done == 1) return n;
      
    } 
    return -1;
  }

  int8_t is_ours(message_t* ptr){
    uint8_t n;
    for (n=0; n < FWD_QUEUE_SIZE; n++) {
       if(FwdBufList[n] == ptr){
		return n;
       }
    } 
    return -1;
  }

  static char* fields(message_t* msg) {
#ifdef TOSSIM
    static char mbuf[1024];
    lqi_header_t* hdr = getHeader(msg);
    sprintf(mbuf, "origin = %hu, seqno = %hu, oseqno = %hu, hopcount =%hu", hdr->originaddr, hdr->seqno, hdr->originseqno, hdr->hopcount);
    return mbuf;
#else
    return NULL;
#endif
  }

  static void forward(message_t* msg);
  
  static message_t* mForward(message_t* msg) {
    int8_t buf = get_buff();
    dbg("LQI", " Asked to forward packet @%s:\t%s\n", sim_time_string(), fields(msg));
    if (buf == -1) {
      dbg("LQI", "%s Dropped packet due to no space in queue.\n", __FUNCTION__);
      call CollectionDebug.logEvent(NET_C_FE_SEND_QUEUE_FULL);
      return msg;
    }
    if ((call RouteSelect.selectRoute(msg, 0)) != SUCCESS) {
      FwdBufBusy[(uint8_t)buf] = 0;
      call CollectionDebug.logEvent(NET_C_FE_NO_ROUTE);
      dbg("LQI", "%s Dropped packet due to no route.\n", __FUNCTION__);
      return msg;
    }
    else {
      message_t* newMsg = FwdBufList[(uint8_t)buf];
      FwdBufList[(uint8_t)buf] = msg;
      forward(msg);
      return newMsg;
    }
  }
  
  static void forward(message_t* msg) {
    // Failures at the send level do not cause the seq. number space to be 
    // rolled back properly.  This is somewhat broken.
    if (fwdbusy || running == FALSE) {
      dbg("LQI", "%s forwarding busy or off, wait for later.\n", __FUNCTION__);
      return;
    }
    else {
      call PacketAcknowledgements.requestAck(msg);
      if (call SubSend.send(call AMPacket.destination(msg),
			    msg,
			    call SubPacket.payloadLength(msg)) == SUCCESS) {
	call CollectionDebug.logEventMsg(NET_C_DBG_1, 
					 call CollectionPacket.getSequenceNumber(msg),