/*
 * spitter.nxc
 * Version 1.0, December 2006
 * 
 * Sivan Toledo
 *
 * Physical Robot Structure:
 * spitting motor A turns the wheels
 * throat   motor B opens the throat
 * scanning motor C turns the US sensor
 * light sensor senses paper pellets, port 1
 * sound sensor waits for long quiet periods and complains, port 2
 * US sensor searches for something close, turns to him/her
 *    to ask for food, on port 4
 * see http://www.tau.ac.il/~stoledo/lego/Spitter/ for details
 *
 * Version 1.0 was compiled successfully with beta 15 of NBC/NXC
 * 
 */

#include "NXCDefs.h"

#define u8  unsigned char
#define s8  char
#define u16 unsigned int
#define s16 int
#define u32 unsigned long
#define s32 long

u8 y_sound_level       = 0;
u8 y_light_level       = 10;
u8 y_us_level          = 20;
u8 y_angle             = 30;
u8 y_time_since_eating = 40;
u8 y_time_since_loud   = 50;
u8 y_time_since_calling= 60;
u8 x_number            = 72;

u32 quiet_timeout  = 15000;
u32 feedme_timeout = 30000;

u32 base_quiet_timeout  = 15000;
u32 base_feedme_timeout = 30000;

u32 max_quiet_timeout  = 150000;
u32 max_feedme_timeout = 300000;

u8 l;
mutex eating_state_mutex;
u8    eating_state = 0; // not eating
u32   eating_time  = 0; // already hungry!
task eating()
{
  //Follows(main);
  
  SetSensorType(IN_1, IN_TYPE_LIGHT_ACTIVE);
  SetSensorMode(IN_1, IN_MODE_PCTFULLSCALE);
  ResetSensor(IN_1);
  
  while (TRUE) {
	l = ReadSensor(IN_1);
	
	TextOut(0, y_light_level,false,"light level: ");
	TextOut(x_number,y_light_level,false,"    ");
	NumOut (x_number,y_light_level,false,l);

	if (l > 40) {
	  // the light sensor saw a white paper pellet
	  
	  Acquire(eating_state_mutex);
	  StopSound(); // stop talking, if we're taking
	  eating_state = 1;
	  eating_time  = GetTick();
	  Release(eating_state_mutex);

      PlayFile("yuck.rso");
      while (SoundFlags() != SOUND_FLAGS_IDLE);  
      
      quiet_timeout  = base_quiet_timeout;
      feedme_timeout = base_feedme_timeout;
	
	  // turn motor A on
  	  OnFwd(OUT_A, 100);
  	  // wait for them to spin fast
  	  Wait(2000);
  	  // open the throat
  	  OnFwd(OUT_B, 40);
  	  // wait for the throat to open fully
  	  Wait(500);  
  	  Off(OUT_B);
  	  Wait(1000);
  	  // wait for the pellets to drop and be spitted out
  	  Wait(2000);
  	  // close the mouth and stop the wheels
  	  OnRev(OUT_B, 40);
  	  Coast(OUT_A);
  	  Wait(500);
  	  Off(OUT_B);
  	  Off(OUT_A);

	  Acquire(eating_state_mutex);
	  eating_state = 0;
	  Release(eating_state_mutex);
	}
  }  
}

u8  listening_sound_level;
u32 listening_last_loud;
task listening()
{
  SetSensorType(IN_2, IN_TYPE_SOUND_DB);
  SetSensorMode(IN_2, IN_MODE_PCTFULLSCALE);
  ResetSensor(IN_2);
  
  listening_last_loud = GetTick(); // pretend it's loud when we start
  
  while (TRUE) {
	listening_sound_level = ReadSensor(IN_2); 
	if (listening_sound_level > 20) 
      listening_last_loud = GetTick();
    if (listening_sound_level > 50)
	  quiet_timeout = base_quiet_timeout; // loud, Spitter gets impatient
	 
	TextOut(0, y_sound_level,false,"sound level: ");
	TextOut(x_number,y_sound_level,false,"    ");
	NumOut(x_number, y_sound_level,false,listening_sound_level);
	
    TextOut(0,y_time_since_loud,false,"T loud: ");
    TextOut(x_number,y_time_since_loud,false,"    ");
    NumOut(x_number,y_time_since_loud,false,(GetTick() - listening_last_loud)/1000);

	if (GetTick() - listening_last_loud > quiet_timeout) {
      listening_last_loud = GetTick(); // I'm talking!
      PlayFile("itsquiet.rso");      
	  quiet_timeout = 2*quiet_timeout;
  	  if (quiet_timeout > max_quiet_timeout)
	    quiet_timeout = max_quiet_timeout;
	}
	
    Wait(500);
  }
}

u8  usdist;
u8  mindist;
s16 scn_deg;
s16 mindeg;
u8  hungry = FALSE;
u32 time_since_calling;
u32 time_since_eating;
u32 calling_time = 0;
task scanning()
{
  while (TRUE) {
  
   Acquire(eating_state_mutex);
   time_since_eating = GetTick() - eating_time;
   Release(eating_state_mutex);

   TextOut(0,y_time_since_eating,false,"T feeding: ");
   TextOut(x_number,y_time_since_eating,false,"    ");
   NumOut(x_number,y_time_since_eating,false,time_since_eating/1000);

   TextOut(0,y_time_since_calling,false,"T calling: ");
   TextOut(x_number,y_time_since_calling,false,"    ");
   NumOut(x_number,y_time_since_calling,false,(GetTick() - calling_time)/1000);

   if (time_since_eating > feedme_timeout 
       && (GetTick() - calling_time) > feedme_timeout) {

	  calling_time = GetTick();
	  
      mindist = 255;
      SetSensorUltrasonic(IN_4);
      RotateMotor( OUT_C, 30, -100 );
      Wait(100);
      for (scn_deg=-100; scn_deg < 100; scn_deg += 10) {
  	    usdist = ReadSensorUS(IN_4);  
	    if (usdist < mindist) {
	      mindist = usdist;
	      mindeg  = scn_deg;
        }
    
	    TextOut(0,y_us_level,false,"us distance: ");
	    TextOut(x_number,y_us_level,false,"    ");
	    NumOut(x_number,y_us_level,false,usdist);

  	    TextOut(0,y_angle,false,"angle: ");
	    TextOut(x_number,y_angle,false,"    ");
	    NumOut(x_number,y_angle,false,scn_deg);
        
  	    RotateMotor( OUT_C, 30,  10 );
   	    Wait(100);
      }
      RotateMotor( OUT_C, 30, -100 );
      Wait(500);
      
      if (mindist < 255) {
        RotateMotor( OUT_C, 30,  mindeg );
        PlayFile("feedme.rso");
        //while (SoundState() != SOUND_STATE_IDLE) // || SoundFlags() != SOUND_FLAGS_IDLE)
        while (SoundFlags() != SOUND_FLAGS_IDLE);
        PlayFile("feedme.rso");
        while (SoundFlags() != SOUND_FLAGS_IDLE);
        PlayFile("feedme.rso");
        while (SoundFlags() != SOUND_FLAGS_IDLE);
  	    RotateMotor( OUT_C, 30,  -mindeg );
      }
      
	  feedme_timeout = 2*feedme_timeout;
      if (feedme_timeout > max_feedme_timeout)
	    feedme_timeout = max_feedme_timeout;
      
    }
    Wait(1000);
  }
}

task keepalive()
{
  while (TRUE) {
    ResetSleepTimer();
    Wait(60000);
  }
}

// the main task only starts the other ones

task main()
{
  Precedes(listening, eating, scanning, keepalive);
  //Precedes(listening);
}