/***********************************************************************/
/** @name sensors.c 
    Contains functions read sensors.
    
    @author Thomas Braunl & Birgit Graf, UWA, 1998, modified 2000 (Mk3/4)

*/
/***********************************************************************/

/*@{*/


#include "global.h"
#include "general.h"
#include "driveglobal.h"
#include "sensors.h"


/** simulate psd sensors ? */
int sim_psds = FALSE;


/** handles for PSD sensors */
PSDHandle psd_front, psd_left, psd_right;



/***********************************************************************/
/** Init PSD.
    Init PSD senors that are used in the program.
    
    Called by main(). 
    
    @see main()
*/
/***********************************************************************/

void Init_PSD()
{
  if (sim_psds)
    return;

  psd_front = PSDInit(PSD_FRONT);
  psd_left  = PSDInit(PSD_LEFT);
  psd_right = PSDInit(PSD_RIGHT);

  if (PSDStart(psd_front|psd_left|psd_right, TRUE) != 0)
  {
    LCDClear();
    LCDMenu("", "", "", "OK");
    LCDPutString("PSDStart \nerror!\nSwitching to\nsim mode  ");
    KEYWait(KEY4);
    LCDClear();

    sim_psds = TRUE;
  }
}


/***********************************************************************/
/** Stop PSD.
    Stop PSD senors.
*/
/***********************************************************************/

void Stop_PSD()
{
  if (sim_psds)
    return;

  PSDStop();
}


/***********************************************************************/
/** End PSD.
*/
/***********************************************************************/

void End_PSD()
{
    if (sim_psds)
    return;

    PSDRelease();  
}


/***********************************************************************/
/** Check PSD sensors.
    Check psd sensors and set obstacle_flag respectively.
    
    Called by PPobstacle_test()
    @see PPobstacle_test()
    @return with which sensor obstacle has been detected or 0 if none    
*/
/***********************************************************************/

int psd_test()
{
  int new_obstacle;
  int side_psd;

  if (sim_psds)
    return 0;

  new_obstacle = ((PSDGet(psd_front) < obstacle_thresh) && !see_ball_flag);
  side_psd = (4 * (PSDGet(psd_right) - 50 < obstacle_thresh) +
    (2 * PSDGet(psd_left) < obstacle_thresh)); 
   
  if (new_obstacle && !obstacle_flag) /* new obstacle detected -> stop */ 
    drive_stop();

  return new_obstacle + side_psd; /* 1: front, 2: left, 4: right psd sensor */
}


/***********************************************************************/
/** Get Minimum.
    Check psd sensors return smallest value.
    
    Called by PPobstacle_test()
    @see PPobstacle_test()
    @return smalest distance any of activated psd sensors has detected    
*/
/***********************************************************************/

float min_psd(float thresh, int sensor)
{
  float dist = 100.0;

  if (sim_psds)
    return dist;

  switch (sensor)
  {
  case 1: dist = (float) PSDGet(psd_front) / 1000.0; break;
  case 2: dist = (float) PSDGet(psd_left) / 1000.0; break;
  case 4: dist = (float) PSDGet(psd_right) / 1000.0; break;
  default: dist = 100.0; break;
  }
  
  if (dist < thresh) return dist;
  else return thresh;
}


/***********************************************************************/
/** Check for obstacles.
    Check psd sensors and check wheather the robot's wheels have
    stalled - this would indicate that the robot has run into an
    obstacle. Set obstacle_flag respectively.
    
    Process started by main().
    @see main()
*/
/***********************************************************************/

void PPobstacle_test()
{
  /* robot position */
  PositionType pos;
  /* distance from robot middle to detected obstacle */
  float obstacle_dist; 
  /* distance to wall when position value can be reset */
  float reset_dist; 

  int psd_detected = FALSE;	/* flag to show whether psd sensors
				   have detected obstacle */
  int stall_detected = FALSE;	/* flag to show whether stallchecker
				   has detected obstacle */

  while(!end_flag)
  {
    /* ----- check for obstacles (stall or front psd) ----- */
    if (!I_am_goalie())
      psd_detected = psd_test();
    else
      psd_detected = FALSE;

/*    stall_detected = wheels_stalled(); */
    if (stall_detected)
    {
			drive_stop();
			if (USE_AUDIO)
	      AUTone(1200,200);
    }

    /* detected obstacle with front psd sensor or stallchecker */
    obstacle_flag = obstacle_flag || stall_detected || (psd_detected == 1);

    /* ----- reset position (stall or any psd sensor) ----- */
    if (stall_detected || psd_detected)
    {
				LCDSetPos(1, 10);
				LCDPrintf("Wall!\n");
				LCDSetPos(2, 10);
				LCDPrintf("     \n");
				LCDSetPos(3, 10);
				LCDPrintf("     \n");
				
				obstacle_dist = ROBWIDTH2; /* distance to obstacle if robot
																			ran into it (stallchecker
																			aktiv) */
      
      if (psd_detected) 
      {
				obstacle_dist += min_psd((float)obstacle_thresh / 2000.0, psd_detected);
				/* add distance between robot and obstacle (psd value) */
				if (USE_AUDIO)
					AUTone(1000,200);
      }
      reset_dist = 0.1 + obstacle_dist; /* threshold for resetting
					   position (robot close to wall) */
      
      /* ----- reset position ----- */
      get_pos(&pos);
      
      /* left or right sidewall */
      if (fabs(pos.y) > WIDTH2 - reset_dist) 	
				pos.y = fsign(pos.y) * (WIDTH2 - obstacle_dist);
      
      /* field's end on opponent's side */
      if (pos.x > LENGTH - reset_dist) 
				pos.x = LENGTH - obstacle_dist;
      
      /* field's end on our side */
      else if (pos.x < reset_dist) 	
				pos.x = obstacle_dist;
      
      set_pos(pos.x, pos.y, pos.phi);
    }
    OSReschedule();
  }
  end_flag += 1;
  OSKill(0);
}

/*@}*/