/*									tab:4
 *	PLANT_SENS_OUTPUT.c
 *
 *	Authors:	Waylon Bruntte
 *                      Jason Jenks
 *
 *      Previous Contributers: Katy Berg
 *                             Deborah Ford
 *
 *	Date:           created 7/6/01		
 *
 * 	Based on SENS_OUTPUT.c and AM_ROUTE.c
 *
 *=================================================================
 * Header from SENS_OUTPUT.c similar copyright information exists for AM_ROUTE.c
 *=================================================================
 *
 * "Copyright (c) 2000 and 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 and the following
 * two paragraphs 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."
 *
 * Authors:   Jason Hill
 * History:   created 1/23/2001
 *================================================================
 *
 *
 *
 */

#include "tos.h"
#include "PLANT_SENS_OUTPUT.h"

#define NUMBER_OF_READINGS 19             // specifies the size of the array that will hold the readings
#define MAX_READINGS_IN_PACKET 35         // specifies the size of the arrays that will hold the readings in the data packet
#define NUM_READINGS_IN_PACKET (MAX_READINGS_IN_PACKET-3)  // specifies the number of readings that can be sent in the packet
#define SAMPLE_INTERVAL 2                // specifies the number of clock events that occur before the next reading is taken
#define MAX_TIME_READINGS_INTERVAL 5      // max number of readings that can be taken before a packet must be sent 
#define MAX_CLOCK_LOCK 5                  // specifies # of clock events before restarting sens events to clear clock_lock 
#define ACK_WAIT 5
#define MAX_SENT_ACKS 25 

typedef struct{
  unsigned char reading;
  unsigned char frequency;
} sens_reading;

typedef struct{
  char original_mote_address;
  char send_num;
  char seq_num;
  sens_reading sens_data[MAX_READINGS_IN_PACKET];
} data_msg;

typedef struct{
  char acked_mote_address;
  char msg_type;
  char seq_num;
} ack_msg;

//Frame Declaration
#define TOS_FRAME_TYPE PLANT_SENS_OUTPUT_frame
TOS_FRAME_BEGIN(PLANT_SENS_OUTPUT_frame) {
  unsigned char time_counter;		//number of clock events for each sampling
  unsigned char read_counter;		//number of samplings before packet sent
  unsigned char lock_counter;           // number of clock events before sensors readings are restarted
  
  unsigned char photo_index;		//index for photo array		       
  unsigned char temp_index;		//index for temperature array
  unsigned char moist_index;		//index for moisture array
  unsigned char power_index;             //index for power array

  volatile char clock_lock;             // set to 1 when the program is getting sensor information and possibly sending the readings
  volatile char data_pending;           // set to 1 when the program sends data

  char bot_present;
  char bot_acked;

  char ack_counter;		        //counter to track if it is time ot attempt a resend
  TOS_Msg data;                         // copy of the data packet sent             
  char ready_send;

  char messageType;

  unsigned char seq_num;		// Current sequence number range 0-15

  //arrays to hold the sensor readings and frequency of the readings
  sens_reading temperature[NUMBER_OF_READINGS];
  sens_reading photo[NUMBER_OF_READINGS];
  sens_reading moisture[NUMBER_OF_READINGS];
  sens_reading power[NUMBER_OF_READINGS];

}
TOS_FRAME_END(PLANT_SENS_OUTPUT_frame);

// clears all the sensor information
TOS_TASK(PLANT_SENS_OUTPUT_CLEAR)
{
  unsigned char counter;
  sens_reading blank;
  blank.reading = 0;
  blank.frequency = 0;

  // intialize the data arrays to zero
  for(counter = 0; counter < NUMBER_OF_READINGS; counter++){
    VAR(temperature)[(int)counter] = blank;
    VAR(photo)[(int)counter] = blank;
    VAR(moisture)[(int)counter] = blank;
    VAR(power)[(int)counter] = blank;
  }
  VAR(photo_index) = 0;		//clear index for photo array		       
  VAR(temp_index) = 0;		//clear index for temperature array
  VAR(moist_index) = 0;	        //clear index for moisture array
  VAR(power_index) = 0;         //clear index for power array 

  VAR(read_counter) = 0;	        

  VAR(clock_lock) = 0;
}


char TOS_COMMAND(PLANT_SENS_OUTPUT_START)(){
  TOS_CALL_COMMAND(PLANT_SENS_OUTPUT_SUB_CLOCK_INIT)(255, 7); /* initialize clock component: to the maximium in difference of time*/
  return 1;
}

// determine if a packet needs to be sent, if so send the packet
TOS_TASK(PLANT_SENS_DATA_READY){
  VAR(messageType) = AM_MSG(DATA_PACKET);
  VAR(read_counter)++;		//count that we took another set of readings
  printf("\nCurrent number of readings: %d\n", VAR(read_counter));

  //  TOS_CALL_COMMAND(PLANT_SENS_GREEN_TOGGLE)();

  //moisture, power, temperature, or photo data ready, call a method to send the package if one of the arrays are full.
  //or if the NUM_READINGS_IN_PACKET exist in the arrays (or within 3, because if 4 new readings come in there won't be enough room in packet 
  //or if MAX_TIME_INTERVAL has passed, send the packet
  //only send the packet if one of the send conditions 
  if ( (VAR(ack_counter) == 0) && 
      ((VAR(bot_present) && !VAR(bot_acked))
       || (VAR(read_counter) >= MAX_TIME_READINGS_INTERVAL)
       || (VAR(temperature)[NUMBER_OF_READINGS - 1].frequency != 0) 
       || (VAR(photo)[NUMBER_OF_READINGS - 1].frequency != 0)
       || (VAR(moisture)[NUMBER_OF_READINGS-1].frequency != 0)
       || (VAR(power)[NUMBER_OF_READINGS-1].frequency != 0)
       || (NUM_READINGS_IN_PACKET - (VAR(photo_index) + VAR(temp_index) + VAR(moist_index)+ VAR(power_index)) <= 3)))
    { 
      data_msg* message = (data_msg*)VAR(data).data;
      unsigned char photo_counter = 0;
      unsigned char temp_counter = 0;
      unsigned char moist_counter = 0;
      unsigned char power_counter = 0;
      unsigned char counter;
      sens_reading blank;
      blank.reading = 0;
      blank.frequency = 0;
      
      message->original_mote_address = TOS_LOCAL_ADDRESS;
      
      for(counter = 0; counter < MAX_READINGS_IN_PACKET; counter++)
	{
	  // copy photo values
	  if(photo_counter <= VAR(photo_index))
	    { 
	      if(photo_counter == VAR(photo_index))
		{
		  message->sens_data[(int)counter] = blank;
		} 
	      else 
		{
		  message->sens_data[(int)counter] = VAR(photo)[(int)photo_counter];
		}
	      photo_counter++;
	    }
	  else if(temp_counter <= VAR(temp_index))
	    {
	      if(temp_counter == VAR(temp_index))
		{ 
		  message->sens_data[(int)counter] = blank;
		}
	      else 
		{
		  message->sens_data[(int)counter] = VAR(temperature)[(int)temp_counter];
		}
	      temp_counter++;
	    }
	  else if(moist_counter <= VAR(moist_index))
	    {
	      if(moist_counter == VAR(moist_index))
		{ 
		  message->sens_data[(int)counter] = blank;
		}
	      else 
		{
		  message->sens_data[(int)counter] = VAR(moisture)[(int)moist_counter];
		}
	      moist_counter++;
	    }
	  else if(power_counter < VAR(power_index))
	    {
	      message->sens_data[(int)counter] = VAR(power)[(int)power_counter];
	      power_counter++;
	    }
	  else
	    {
	      message->sens_data[(int)counter] = blank;
	    }
	}
      // We pulled data out of the arrays, increment the sequence number
      TOS_POST_TASK(PLANT_SENS_OUTPUT_CLEAR);
      VAR(seq_num) = (VAR(seq_num) + 1) % 16;	 
      message->send_num = 1;
      message->seq_num = VAR(seq_num);
      
      //TOS_CALL_COMMAND(PLANT_SENS_RED_TOGGLE)();

      if (VAR(bot_present))
	VAR(messageType) = AM_MSG(ROBOT_PACKET);

      VAR(ready_send) = 1;
 
    }
  
  VAR(clock_lock) = 0;
  printf("\nUNLOCKED THE CLOCK\n");
}

// initialize all the components
char TOS_COMMAND(PLANT_SENS_OUTPUT_INIT)(){
  unsigned char counter;
  sens_reading blank;
  blank.reading = 0;
  blank.frequency = 0;

  // intialize the data arrays to zero
  for(counter = 0; counter < NUMBER_OF_READINGS; counter++){
    VAR(temperature)[(int)counter] = blank;
    VAR(photo)[(int)counter] = blank;
    VAR(moisture)[(int)counter] = blank;
    VAR(power)[(int)counter] = blank;
  }
  VAR(photo_index) = 0;		//clear index for photo array		       
  VAR(temp_index) = 0;		//clear index for temperature array
  VAR(moist_index) = 0;	        //clear index for moisture array
  VAR(power_index) = 0;         //clear index for power array 

  VAR(bot_present) = 0;
  VAR(bot_acked) = 0;

  VAR(read_counter) = 0;	        

  VAR(clock_lock) = 0;
  VAR(data_pending) = 0;
  VAR(lock_counter) = 0;

  VAR(ack_counter) = 0;
  VAR(time_counter) = 0;
  VAR(seq_num) = 0;
  VAR(ready_send) = 0;
 
  TOS_CALL_COMMAND(PLANT_SENS_PHOTO_DATA_INIT)(); /* initialize photo component */
  TOS_CALL_COMMAND(PLANT_SENS_TEMP_DATA_INIT)(); /* initialize temp component */
  TOS_CALL_COMMAND(PLANT_SENS_SUB_LED_INIT)(); /* initialize LEDS component */
  TOS_CALL_COMMAND(PLANT_SENS_YELLOW_OFF)();
  TOS_CALL_COMMAND(PLANT_SENS_GREEN_OFF)();
  TOS_CALL_COMMAND(PLANT_SENS_RED_OFF)();
  TOS_CALL_COMMAND(PLANT_SENS_OUTPUT_RFM_SUB_INIT)();

  printf("ALL INITIALIZATION COMPLETE");
  return 1;
}

/* Clock Event Handler:
         increment counters, process flow control, and determine if its time for a new set of sensor readings to be taken */
void TOS_EVENT(PLANT_SENS_OUTPUT_CLOCK_EVENT)(){
  printf("CLOCK EVENT - get the data");

  TOS_CALL_COMMAND(PLANT_SENS_RED_TOGGLE)();

  if(VAR(ready_send))
    {
      VAR(data_pending) = 1;
      
      // send the data packet
      if (TOS_CALL_COMMAND(PLANT_SENS_OUTPUT_SUB_SEND_MSG)(TOS_BCAST_ADDR, VAR(messageType), &VAR(data))) {
	TOS_CALL_COMMAND(PLANT_SENS_GREEN_TOGGLE)();
	VAR(ack_counter) = 1;
      }
      else
	{
	  // TODO: If the send fails, what should happen?
	  VAR(data_pending) = 0;
	}

      VAR(ready_send) = 0;
      return;
    }


  // increment the counter that keeps track of when the next sensor reading will take place
  VAR(time_counter)++;

  printf("Ack counter = %d, data pending = %d\n", VAR(ack_counter), VAR(data_pending));

  // TODO: Determine if we are in a distress state
  if (0)
    {
      // Handle distress
      return;
    }
  
  if (TOS_CALL_COMMAND(PLANT_SENS_POWER_GET_BOT_HERE)() && !VAR(bot_present))
    {
      // Set appropriate bits
      VAR(bot_present) = 1;
    }
  else if (!TOS_CALL_COMMAND(PLANT_SENS_POWER_GET_BOT_HERE)() && VAR(bot_present))
  {
    // The bot has left us *sniff*
    VAR(bot_present) = 0;
    VAR(bot_acked) = 0;
  }

  // Handle ack stuff
  if (VAR(ack_counter) > 0 && VAR(data_pending) == 0)
  {
    printf("\n ATTEMPTING TO SEND ACK - ack counter = %d, data pending = %d\n", VAR(ack_counter), VAR(data_pending));
    VAR(ack_counter)++;

    if (VAR(ack_counter) >= MAX_SENT_ACKS) // Note that this should not be a multiple of 5
    { 
      // Give up
      VAR(ack_counter) = 0;
    }
    else if (VAR(ack_counter) % ACK_WAIT == 0 || (VAR(bot_present) && !VAR(bot_acked)))
    { 
      // Resend the packet
      ((data_msg*)&VAR(data).data)->send_num++;
      VAR(data_pending) = 1;
      if (!TOS_CALL_COMMAND(PLANT_SENS_OUTPUT_SUB_SEND_MSG)(TOS_BCAST_ADDR, VAR(data).type, &VAR(data))) {
	VAR(data_pending) = 0;
      }
    }
  }


  // if clock lock is on prevent another set of events from overriding
  // lock counter is incremented and checked if counterincase an event somewhere has caused the control flow to not ever execute a clock lock release
  if(VAR(clock_lock)){
    VAR(lock_counter)++;
    if(VAR(lock_counter) > MAX_CLOCK_LOCK)
      {
	VAR(lock_counter) = 0;

	TOS_CALL_COMMAND(PLANT_SENS_YELLOW_TOGGLE)();

	//get the readings after a specified amount of time (SAMPLE_INTERVAL) 
	TOS_CALL_COMMAND(PLANT_SENS_PHOTO_GET_DATA)();
	printf("\n get photo called \n");
      }
    return;
  }

  TOS_CALL_COMMAND(PLANT_SENS_YELLOW_TOGGLE)();
  
  // check to see if it's time for sensor readings to be taken
  if(VAR(time_counter) >= SAMPLE_INTERVAL){
    // set control flow stuff
    VAR(time_counter) = 0;
    VAR(clock_lock) = 1;
    VAR(lock_counter) = 0;
    
    //get the readings after a specified amount of time (SAMPLE_INTERVAL) 
    TOS_CALL_COMMAND(PLANT_SENS_PHOTO_GET_DATA)();
    printf("\n get photo called \n");
  }
}

/* Data ready event Handler:*/
/* Photo Data ready, get temperature data.*/
char TOS_EVENT(PLANT_SENS_PHOTO_DATA_READY)(int photoData){
	unsigned char reading = 0;
	printf("\nPhoto Reading: %d\n", photoData);
	
	// convert the 10-bit value to an 8-bit value using rounding
	photoData >>= 1;
	if((photoData & 0x01))
	  {
	    photoData >>= 1;
	    reading = photoData;
	    if((int)reading < 255)
	      reading++;
	  }
	else
	  {
	    photoData >>=1;
	    reading = photoData;
	  }

	//if the data is the last item already in the array, just increment the frequency
	if(VAR(photo_index) > 0 && reading == VAR(photo)[(int)VAR(photo_index) - 1].reading)
	  {
	    VAR(photo)[(int)VAR(photo_index) - 1].frequency++;
	    printf("\nPHOTO DATA READY, index now is %d\n", VAR(photo_index));
	    printf("\nPHOTO reading %d\n", VAR(photo)[(int)VAR(photo_index) - 1].reading);
	    printf("Reading Frequency: %d\n", VAR(photo)[(int)VAR(photo_index) - 1].frequency); 
	    TOS_CALL_COMMAND(PLANT_SENS_TEMP_GET_DATA)();
	    printf("\n get temp called \n");
	    return 1;
	  }

	//otherwise, add the reading to the array, increment the index
	if(VAR(photo_index) < NUMBER_OF_READINGS)
	  {
	    VAR(photo)[(int)VAR(photo_index)].reading = reading;
	    VAR(photo)[(int)VAR(photo_index)].frequency++;
	    VAR(photo_index)++;
	  }

	printf("PHOTO DATA READY, index now is %d", VAR(photo_index));
	TOS_CALL_COMMAND(PLANT_SENS_TEMP_GET_DATA)();
	printf("\n get temp called \n");
	return 1;
}


/* Temp Data ready, send packet.*/
char TOS_EVENT(PLANT_SENS_TEMP_DATA_READY)(int tempData){
	unsigned char reading = 0;

	printf("\nTemp Reading: %d\n", tempData);

	// convert the 10-bit value to an 8-bit value using rounding
	tempData >>= 1;
	if((tempData & 0x01))
	  {
	    tempData >>= 1;
	    reading = tempData;
	    if((int)reading < 255)
	      reading++;
	  }
	else
	  {
	    tempData >>=1;
	    reading = tempData;
	  }

	//if the data is the last item already in the array, just increment the frequency
	if(VAR(temp_index) > 0 &&reading == VAR(temperature)[(int)VAR(temp_index) - 1].reading)
	  {
	    VAR(temperature)[(int)VAR(temp_index) - 1].frequency++;
	    printf("TEMP DATA READY, index now is %d", VAR(temp_index));
	    printf("\nTEMP reading %d\n", VAR(temperature)[(int)VAR(temp_index) - 1].reading);
	    printf("Reading Frequency: %d\n", VAR(temperature)[(int)VAR(temp_index) - 1].frequency); 
	    TOS_CALL_COMMAND(PLANT_SENS_MOIST_PWR_ON)();
	    return 1;
	  }
	
	//otherwise, add the reading to the array, increment the index
	if(VAR(temp_index) <  NUMBER_OF_READINGS)
	  {
	    VAR(temperature)[(int)VAR(temp_index)].reading = reading;
	    VAR(temperature)[(int)VAR(temp_index)].frequency++;
	    VAR(temp_index)++;
	  }

	printf("TEMP DATA READY, index now is %d", VAR(temp_index));
	TOS_CALL_COMMAND(PLANT_SENS_MOIST_PWR_ON)();
	return 1;
}

void TOS_EVENT(PLANT_SENS_MOIST_SENS_READY)(void){
  TOS_CALL_COMMAND(PLANT_SENS_MOIST_GET_DATA)();
}

char TOS_EVENT(PLANT_SENS_MOIST_DATA_READY)(int moistData){
	unsigned char reading = 0;
	printf("\nMoist Reading: %d\n", moistData);

	// convert the 10-bit value to an 8-bit value using rounding
	moistData >>= 1;
	if((moistData & 0x01))
	  {
	    moistData >>= 1;
	    reading = moistData;
	    if((int)reading < 255)
	      reading++;
	  }
	else
	  {
	    moistData >>=1;
	    reading = moistData;
	  }

	//if the data is the last item already in the array, just increment the frequency
	if(VAR(moist_index) > 0 && reading == VAR(moisture)[(int)VAR(moist_index) - 1].reading)
	  {
	    VAR(moisture)[(int)VAR(moist_index) - 1].frequency++;
	    printf("MOISTURE DATA READY, index now is %d", VAR(moist_index));
	    printf("\nMOIST reading %d\n", VAR(moisture)[(int)VAR(moist_index) - 1].reading);
	    printf("Reading Frequency: %d\n", VAR(moisture)[(int)VAR(moist_index) - 1].frequency); 
	    TOS_CALL_COMMAND(PLANT_SENS_MOIST_PWR_OFF)();
	    //TOS_POST_TASK(PLANT_SENS_DATA_READY);
	    TOS_CALL_COMMAND(PLANT_SENS_POWER_GET_DATA)();
            return 1;
	  }
	
	//otherwise, add the reading to the array, increment the index
	if(VAR(moist_index) <  NUMBER_OF_READINGS)
	  {
	    VAR(moisture)[(int)VAR(moist_index)].reading = reading;
	    VAR(moisture)[(int)VAR(moist_index)].frequency++;
	    VAR(moist_index)++;
	  }

	printf("MOISTURE DATA READY, index now is %d", VAR(moist_index));
	TOS_CALL_COMMAND(PLANT_SENS_MOIST_PWR_OFF)();
	//TOS_POST_TASK(PLANT_SENS_DATA_READY);
	TOS_CALL_COMMAND(PLANT_SENS_POWER_GET_DATA)();
	return 1;
  
}

char TOS_EVENT(PLANT_SENS_POWER_DATA_READY)(int powerData){
  unsigned char reading = 0;

  // convert the 10-bit value to an 8-bit value using rounding
  powerData >>= 1;
  if((powerData & 0x01))
  {
    powerData >>= 1;
    reading = powerData;
    if((int)reading < 255)
      reading++;
  }
  else
  {
    powerData >>=1;
    reading = powerData;
  }

	//if the data is the last item already in the array, just increment the frequency
	if(VAR(power_index) > 0 && reading == VAR(power)[(int)VAR(power_index) - 1].reading)
	  {
	    VAR(power)[(int)VAR(power_index) - 1].frequency++;
	    printf("POWER DATA READY, index now is %d", VAR(power_index));
	    printf("\nPOWER reading %d\n", VAR(power)[(int)VAR(power_index) - 1].reading);
	    printf("Reading Frequency: %d\n", VAR(power)[(int)VAR(power_index) - 1].frequency); 
	    TOS_POST_TASK(PLANT_SENS_DATA_READY);
	    return 1;
	  }
	
	//otherwise, add the reading to the array, increment the index
	if(VAR(power_index) <  NUMBER_OF_READINGS)
	  {
	    VAR(power)[(int)VAR(power_index)].reading = reading;
	    VAR(power)[(int)VAR(power_index)].frequency++;
	    VAR(power_index)++;
	  }

	printf("POWER DATA READY, index now is %d", VAR(power_index));
	TOS_POST_TASK(PLANT_SENS_DATA_READY);
	return 1;
}

char TOS_EVENT(PLANT_SENS_OUTPUT_SUB_MSG_SEND_DONE)(TOS_MsgPtr sentBuffer)
{
  VAR(data_pending) = 0;
  printf("\n JUST CLEARED THE DATA PENDING IN SEND END\n");
  return 1;
}

/* Active Message handler
 */
TOS_MsgPtr TOS_MSG_EVENT(PRIORITY_PACKET)(TOS_MsgPtr msg){
    return msg;
}

TOS_MsgPtr TOS_MSG_EVENT(DATA_PACKET)(TOS_MsgPtr msg){
      return msg;
}

TOS_MsgPtr TOS_MSG_EVENT(DISTRESS_PACKET)(TOS_MsgPtr msg){

    return msg;
}

TOS_MsgPtr TOS_MSG_EVENT(ROBOT_PACKET)(TOS_MsgPtr msg){
    return msg;
}

TOS_MsgPtr TOS_MSG_EVENT(ACK_PACKET)(TOS_MsgPtr msg)
{
  ack_msg* message = (ack_msg*)msg->data;
  data_msg* sent = (data_msg*)&(VAR(data).data);

  printf("\n*******************************************************");
  printf("\n************JUST GOT AN ACK****************************");
  printf("\n*******************************************************\n");

  if (message->acked_mote_address == TOS_LOCAL_ADDRESS && message->msg_type == VAR(data).type && message->seq_num == sent->seq_num)
  {
    VAR(ack_counter) = 0;
    if (msg->type == AM_MSG(ROBOT_PACKET))
      VAR(bot_acked) = 1;
    TOS_CALL_COMMAND(PLANT_SENS_RED_TOGGLE)();
  }

  return msg;
}