/***********************************************************************/
/** @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 TRUE;
}



/***********************************************************************/
/** 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()
{
  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 || (psd_detected == 1) || stall_detected ;
    
    OSReschedule();
  }
  end_flag += 1;
  OSKill(0);
}

/*************************************************************************/
/* GetPSDValue
   return the value of the front PSD sensor

   called by avoid_obstacle
*/
/*************************************************************************/

float GetPSDValue()
{
  int disti;
  float distf,distret;
  
  disti = PSDGet(psd_front);
  distf = (float)disti;
  distret = distf/1000;
 
  
  return distret;

}

/*************************************************************************/
/* GetPSDValue
   return the value of the left PSD sensor

   called by avoid_obstacle
*/
/*************************************************************************/

float GetLeftPSDValue()
{
  int disti;
  float distf,distret;
  
  disti = PSDGet(psd_left);
  distf = (float)disti;
  distret = distf/1000;
 
  
  return distret;

}

/*************************************************************************/
/* GetPSDRightValue
   return the value of the right PSD sensor

   called by avoid_obstacle
*/
/*************************************************************************/

float GetRightPSDValue()
{
  int disti;
  float distf,distret;
  
  disti = PSDGet(psd_right);
  distf = (float)disti;
  distret = distf/1000;
 
  
  return distret;

}

/***********************************************************************/
/** 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. Returns true or false.

    Why am I doing this? To see if having a function works better than
    a separate thread.
    
    Process started by PPdrive_field.
    @see main()
*/
/***********************************************************************/

int CheckObs()
{
 

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

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

    stall_detected = (VWStalled(vw));
    /*    stall_detected = FALSE;*/
    if (stall_detected)
    {
      VWSetSpeed(vw, 0.0, 0.0);
      AUTone(1200,200);
    }

    return obstacle_flag || (psd_detected == 1) || stall_detected ;
  
}
/*@}*/