/*
 * SENSE.c - asynchronous spider web algorithm...
 * Author:  Murat Demirbas
 * Date:    4/22/02
 * Modification(leader): 6/26/02
 */


#include "tos.h"
#include "SENSE.h"
#include "sensorboard.h"


typedef struct{
  int valx;
  char distx;
  char src;
  char parentx;
  char leaderx;
} int_to_rfm_msg;


//Frame Declaration
#define TOS_FRAME_TYPE SENSE_frame
TOS_FRAME_BEGIN(SENSE_frame) {
  int val;    //self value  
  char dist;   //distance to the bug
  char parent; //path to bug (self | N | S | E | W) 
  int valN;   //value of North neighbor
  char distN;  //dist to bug of N neighbor
  int valS;   //value of South neighbor
  char distS;  //dist to bug of S neighbor
  int valE;   //value of East neighbor
  char distE;  //dist to bug of E neighbor
  int valW;   //value of West neighbor
  char distW;  //dist to bug of W neighbor
  char leader; //who is our fearless leader that detected the bug
  char leaderN, leaderS, leaderE, leaderW;
  char pending;
  char bug;
  char round;
  char downTime;  //How many mesgs did I miss from my parent... 
  TOS_Msg data; 
}
TOS_FRAME_END(SENSE_frame);



/* SENSE_INIT: 
   Clear all the LEDs and initialize state
*/
char TOS_COMMAND(SENSE_INIT)(){
  TOS_CALL_COMMAND(SENSE_SUB_INIT)(); 
  VAR(val)=0;   VAR(dist)=0; VAR(parent)=TOS_LOCAL_ADDRESS;
  VAR(valN)=0;  VAR(distN)=0;
  VAR(valS)=0;  VAR(distS)=0;
  VAR(valE)=0;  VAR(distE)=0;
  VAR(valW)=0;  VAR(distW)=0;
  VAR(pending)=0; VAR(bug)=0;
  VAR(round)=0;
  VAR(leader)=TOS_LOCAL_ADDRESS;
  VAR(leaderN)=TOS_LOCAL_ADDRESS; VAR(leaderS)=TOS_LOCAL_ADDRESS;
  VAR(leaderE)=TOS_LOCAL_ADDRESS; VAR(leaderW)=TOS_LOCAL_ADDRESS;
  VAR(downTime)=0; 
  return 1;
}



/* SENSE_START: 
   initialize clock component to generate periodic events.
*/
char TOS_COMMAND(SENSE_START)(){
  TOS_CALL_COMMAND(SENSE_CLOCK_INIT)(tick8ps); //@@@ tick64ps 
  return 1;
}


/* display which direction the spider should go in order to catch the
   bug. Directions:
   none          --> don't move
   tick1ps      --> N
   tick2ps      --> S
   tick3ps      --> W
   tick4ps      --> E
*/
void display(char dir)
{char N=4;
 if(VAR(round)%2 ==1) TOS_CALL_COMMAND(SENSE_LEDr_off)();

  if (dir==TOS_LOCAL_ADDRESS+N && (VAR(round)%8==0))
    TOS_CALL_COMMAND(SENSE_LEDr_on)();  
  else if (dir==TOS_LOCAL_ADDRESS-N && (VAR(round)%4==0))
    TOS_CALL_COMMAND(SENSE_LEDr_on)();  
  else if (dir==TOS_LOCAL_ADDRESS-1 && (VAR(round)%3==0))
    TOS_CALL_COMMAND(SENSE_LEDr_on)();  
  else if (dir==TOS_LOCAL_ADDRESS+1 && (VAR(round)%2==0))
    TOS_CALL_COMMAND(SENSE_LEDr_on)();  
  //else if (dir==TOS_LOCAL_ADDRESS)

  /*Additionally to the IR, I use green and yellow leds to display
    direction below...
    N --> green 
    S --> yellow 
    W --> green+yellow
    E --> none
    root --> none :(
  */
  TOS_CALL_COMMAND(SENSE_LEDy_off)();
  TOS_CALL_COMMAND(SENSE_LEDg_off)();

  if (dir==TOS_LOCAL_ADDRESS+N) TOS_CALL_COMMAND(SENSE_LEDg_on)();  
  else if (dir==TOS_LOCAL_ADDRESS-N) TOS_CALL_COMMAND(SENSE_LEDy_on)();  
  else if (dir==TOS_LOCAL_ADDRESS-1){ 
    TOS_CALL_COMMAND(SENSE_LEDg_on)();  
    TOS_CALL_COMMAND(SENSE_LEDy_on)(); } 
  else if (dir==TOS_LOCAL_ADDRESS+1) ;//none
}




/* Clock Event Handler:
 * call sense bug (bcast value)
 * compute and display the direction from neighbors values
 */
void TOS_EVENT(SENSE_CLOCK_EVENT)(){
  char N=4;
  char D=16; //delta for deciding that the parent is dead 
  // detection of failure approx D/4 secs...

  VAR(round)= (VAR(round)+1)%8;  //@@@ was %64
  display(VAR(parent));   //display direction on IR led
  if( VAR(round)%2 !=0 ) return;  ///@@@ uncomment
  /* There is no need to perform the following code ("sense bug", etc.) 
     32 times a second... Instead, I execute the rest of the procedure
     only 4 times a second... So I will better return... */

  VAR(downTime)=VAR(downTime)+1; // increment down time...
  TOS_CALL_COMMAND(SENSE_GET_DATA)(); //sense bug


  //Action 2:  //grid max dist==2*N
  if((VAR(parent)==TOS_LOCAL_ADDRESS && VAR(dist)!=0)|| VAR(dist)>=2*N ){ 
    VAR(parent)=TOS_LOCAL_ADDRESS; VAR(dist)=0; VAR(leader)=TOS_LOCAL_ADDRESS; VAR(downTime)=0;}
  if(VAR(parent)==TOS_LOCAL_ADDRESS && (VAR(downTime)!=0 || VAR(leader)!=TOS_LOCAL_ADDRESS)){ 
    VAR(leader)=TOS_LOCAL_ADDRESS; VAR(downTime)=0;}
  if(VAR(parent)!=TOS_LOCAL_ADDRESS && VAR(parent)!=TOS_LOCAL_ADDRESS+N &&
     VAR(parent)!=TOS_LOCAL_ADDRESS-N && VAR(parent)!=TOS_LOCAL_ADDRESS+1 &&
     VAR(parent)!=TOS_LOCAL_ADDRESS-1){
    VAR(parent)=TOS_LOCAL_ADDRESS; VAR(dist)=0; VAR(leader)=TOS_LOCAL_ADDRESS; VAR(downTime)=0;}
  if( (VAR(parent)==TOS_LOCAL_ADDRESS+N && TOS_LOCAL_ADDRESS>N*(N-1)) ||
      (VAR(parent)==TOS_LOCAL_ADDRESS-N && TOS_LOCAL_ADDRESS<N+1) ||
      (VAR(parent)==TOS_LOCAL_ADDRESS-1 && TOS_LOCAL_ADDRESS%N==1) ||
      (VAR(parent)==TOS_LOCAL_ADDRESS+1 && TOS_LOCAL_ADDRESS%N==0)){
    VAR(parent)=TOS_LOCAL_ADDRESS; VAR(dist)=0; VAR(leader)=TOS_LOCAL_ADDRESS; VAR(downTime)=0;}


 
  //Action 3:
  if(VAR(parent)==TOS_LOCAL_ADDRESS+N){
      VAR(val)=VAR(valN); VAR(dist)=VAR(distN)+1; VAR(leader)=VAR(leaderN);}
  else if(VAR(parent)==TOS_LOCAL_ADDRESS-N){
      VAR(val)=VAR(valS); VAR(dist)=VAR(distS)+1; VAR(leader)=VAR(leaderS);}
  else if(VAR(parent)==TOS_LOCAL_ADDRESS-1){
      VAR(val)=VAR(valW); VAR(dist)=VAR(distW)+1; VAR(leader)=VAR(leaderW);}
  else if(VAR(parent)==TOS_LOCAL_ADDRESS+1){
      VAR(val)=VAR(valE); VAR(dist)=VAR(distE)+1;  VAR(leader)=VAR(leaderE);}




     /* The idea for action 4 is as follows:
	if you give me a shorter path, I will definitely follow you,
	if our leaders are the same, I will follow you only if my parent is dead,
	if our leaders are different and your leaders detection is more recent I will follow you...
     */

  //Action 4:
  if((VAR(val)<=VAR(valN) && VAR(dist)>VAR(distN)+1) ||
     (VAR(leader)==VAR(leaderN) && VAR(downTime)>D && VAR(distN)<2*N) ||
     (VAR(leader)!=VAR(leaderN) && VAR(val)<VAR(valN) && VAR(distN)<2*N)
     ){
    VAR(val)=VAR(valN);  VAR(parent)=TOS_LOCAL_ADDRESS+N; 
    VAR(dist)=VAR(distN)+1; VAR(leader)=VAR(leaderN); VAR(downTime)=0;}

  if((VAR(val)<=VAR(valS) && VAR(dist)>VAR(distS)+1) ||
     (VAR(leader)==VAR(leaderS) && VAR(downTime)>D && VAR(distS)<2*N) ||
     (VAR(leader)!=VAR(leaderS) && VAR(val)<VAR(valS) && VAR(distS)<2*N)
     ){
    VAR(val)=VAR(valS);  VAR(parent)=TOS_LOCAL_ADDRESS-N; 
    VAR(dist)=VAR(distS)+1; VAR(leader)=VAR(leaderS); VAR(downTime)=0;}
  
  if((VAR(val)<=VAR(valW) && VAR(dist)>VAR(distW)+1) ||
     (VAR(leader)==VAR(leaderW) && VAR(downTime)>D && VAR(distW)<2*N) ||
     (VAR(leader)!=VAR(leaderW) && VAR(val)<VAR(valW) && VAR(distW)<2*N)
     ){
    VAR(val)=VAR(valW);  VAR(parent)=TOS_LOCAL_ADDRESS-1; 
    VAR(dist)=VAR(distW)+1; VAR(leader)=VAR(leaderW); VAR(downTime)=0;}

  if((VAR(val)<=VAR(valE) && VAR(dist)>VAR(distE)+1) ||
     (VAR(leader)==VAR(leaderE) && VAR(downTime)>D && VAR(distE)<2*N) ||
     (VAR(leader)!=VAR(leaderE) && VAR(val)<VAR(valE) && VAR(distE)<2*N)
     ){
    VAR(val)=VAR(valE);  VAR(parent)=TOS_LOCAL_ADDRESS+1; 
    VAR(dist)=VAR(distE)+1; VAR(leader)=VAR(leaderE); VAR(downTime)=0;}

}





/* Data ready event Handler:
 * sense the bug
 * compute 
 *   reading=0 ==>full dark
 *   reading=7 ==>full light
 */
char TOS_EVENT(SENSE_DATA_READY)(short rawdata){
  char reading;

  reading= ((rawdata >> 7) & 0x7);


  if (reading<3){    //bug is sensed
    VAR(bug)++;

    char L=1; //laziness factor...
    //Action 1
    if(VAR(bug)>L){
      VAR(bug)=0;
      VAR(val)++;
      VAR(parent)= TOS_LOCAL_ADDRESS;
      VAR(dist)=0;
      VAR(leader)= TOS_LOCAL_ADDRESS;
      VAR(downTime)= 0;
      //for debugging purposes...
      //TOS_CALL_COMMAND(SENSE_LEDr_on)();
      //TOS_CALL_COMMAND(SENSE_LEDy_on)();
      //TOS_CALL_COMMAND(SENSE_LEDg_on)();
    }

  }

  //bcast val <with moteid included>
  int_to_rfm_msg* message = (int_to_rfm_msg*)VAR(data).data;
  if (!VAR(pending)) {
    VAR(pending) = 1;
    message->valx = VAR(val);
    message->distx = VAR(dist);
    message->src = TOS_LOCAL_ADDRESS;
    message->parentx = VAR(parent); //For the sake of base station visualization demo
    message->leaderx = VAR(leader); //For reducing the fluctuations in the tree once and for all
    if (TOS_CALL_COMMAND(SENSE_SUB_SEND_MSG)(TOS_BCAST_ADDR, AM_MSG(INT_READING), &VAR(data))) {
      return 1;
    } else {
      VAR(pending) = 0;	/* request failed, free buffer */
    }
  }

  return 1;
}



/* Send completion event
   Determine if this event was ours.
   If so, process it by freeing output buffer and signalling event.
*/
char TOS_EVENT(SEND_READING_COMPLETE)(TOS_MsgPtr sentBuffer){
  if (VAR(pending) && sentBuffer == &VAR(data)) {
    VAR(pending) = 0;
    return 1;
  }
  return 0;
}


/* Receive from neighbor, determine which neighbor, and update info accordingly
 */
TOS_MsgPtr TOS_MSG_EVENT(INT_READING)(TOS_MsgPtr msg){
  char N=4;
  int_to_rfm_msg* message = (int_to_rfm_msg*)msg->data;
  //determine which neighbor send the mesg...
  if(TOS_LOCAL_ADDRESS<=N*(N-1) && message->src==TOS_LOCAL_ADDRESS+N){ 
    VAR(valN)=message->valx; VAR(distN)=message->distx; VAR(leaderN)=message->leaderx;
    if(VAR(parent)==TOS_LOCAL_ADDRESS+N) VAR(downTime)=0;
  }
  else if(TOS_LOCAL_ADDRESS>N && message->src==TOS_LOCAL_ADDRESS-N){
    VAR(valS)=message->valx;  VAR(distS)=message->distx; VAR(leaderS)=message->leaderx;
    if(VAR(parent)==TOS_LOCAL_ADDRESS-N) VAR(downTime)=0;
  }
  else if(TOS_LOCAL_ADDRESS%N!=1 && message->src==TOS_LOCAL_ADDRESS-1){
    VAR(valW)=message->valx;  VAR(distW)=message->distx; VAR(leaderW)=message->leaderx;
    if(VAR(parent)==TOS_LOCAL_ADDRESS-1) VAR(downTime)=0;
  }
  else if(TOS_LOCAL_ADDRESS%N!=0 && message->src==TOS_LOCAL_ADDRESS+1){
    VAR(valE)=message->valx;  VAR(distE)=message->distx; VAR(leaderE)=message->leaderx;
    if(VAR(parent)==TOS_LOCAL_ADDRESS+1) VAR(downTime)=0;
  }

  return msg;
}