/***********************************************************************/
/** @name sensors.c 
    Contains functions read sensors.
    
    @author Thomas Braunl & Birgit Graf , UWA, 1998
*/
/***********************************************************************/

/*@{*/


#include "global.h"
#include "general.h"

/** threshold for obstacle avoidance (dist in mm) */
#define obstacle_thresh 120

/** 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()
*/
/***********************************************************************/

int InitPSD()
{
  psd_front = PSDInit(PSD_FRONT);
  psd_left  = PSDInit(PSD_LEFT);
  psd_right = PSDInit(PSD_RIGHT);

  PSDStart(psd_front|psd_left|psd_right, TRUE);
  
  return 0;
}



/***********************************************************************/
/** 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 = ((PSDGet(psd_front) < obstacle_thresh) && !see_ball_flag);
  int side_psd = (4 * (PSDGet(psd_right) - 50 < obstacle_thresh) +
    (2 * PSDGet(psd_left) < obstacle_thresh)); 
     
  if (new_obstacle && !obstacle_flag) /* new obstacle detected -> stop */ 
    VWSetSpeed(vw, 0, 0);
  
  return new_obstacle + side_psd; /* 1: front, 2: left, 4: right psd sensor */
}


float min_psd(float thresh, int sensor)
{
  float 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 = (VWStalled(vw));
    if (stall_detected)
    {
      VWSetSpeed(vw, 0.0, 0.0);
      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)
    {
      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) */
	AUTone(1000,200);
      }
      reset_dist = 0.1 + obstacle_dist; /* threshold for resetting
					   position (robot close to wall) */
      
      /* ----- reset position ----- */
      VWGetPosition(vw, &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;
      
      VWSetPosition(vw, pos.x, pos.y, pos.phi);
    }
    OSReschedule();
  }
  end_flag += 1;
  OSKill(0);
}

/*@}*/