/***********************************************************************/
/** @name general.c 
    Contains functions for general use.    
    
    @author Birgit Graf, UWA, 1998
*/
/***********************************************************************/

/*@{*/


#include "global.h"

/***********************************************************************/
/** Check for keypress.
    Check if KEY3 is pressed and set end_flag respectively.

    Process started by main().
    @see main()
*/
/***********************************************************************/

void PPend_test()
{
  while(!end_flag)
  {
    if (KEYRead() == KEY3)
      end_flag = 1;
    else
      OSReschedule();
  }
  
  OSKill(0);
}



/***********************************************************************/
/** Error message.
    Print error message and end program.

    @param str string, which should be printed
*/
/***********************************************************************/

void error(char *str)
{
  LCDPrintf("ERROR: %s\n", str);
  OSWait(200);
  exit(0);
}



/***********************************************************************/
/** Return neg/pos value.
    Return -1 for input<0, 1 for input > 0 and 0 for input = 0.

    Called by avoid_obstacle(), drive_to_goal(), start_spline_to_ball(),
    drive_spline_IRQ()    
    @see avoid_obstacle()
    @see drive_to_goal()
    @see start_spline_to_ball()
    @see drive_spline_IRQ()
    @param number floating point number
    @return sign of number (float)
*/
/***********************************************************************/

float fsign(float number)
{
  if (number < 0.0)
    return -1.0;
  else
    if (number > 0.0)
      return 1.0;
    else
      return 0.0;
}


/***********************************************************************/
/** Say whether player is goalkeeper.
 */
/***********************************************************************/

int I_am_goalie()
{
  return ((int) OSMachineID() == 1);
}



/***********************************************************************/
/** Set Parameter over keyboard (float).

    Called by main().
    @see main()
    @param text[] name of parameter
    @param minp,maxp minimum and maximum value of parameter 
    @param start start value for parameter
    @param inc step to increment parameter
*/
/***********************************************************************/

float set_fparam(char text[], float minp, float start, float maxp, float inc)
{
  float val;
  int   done = FALSE;

  LCDClear();
  LCDPrintf("Set Parameter\n%s\n", text);

  LCDMenu("+", "-", " ","OK");
  val = start;
  do
  {
    LCDSetPos(5, 0);
    LCDPrintf("%f\n", val);
    switch(KEYGet())
    {
    case KEY1:
      if (val < maxp)
	val += inc;
      break;
    case KEY2:
      if (val > minp)
	val -= inc;
      break;
    case KEY4:
      done = TRUE;
    }
  } while (!done);

  LCDClear();
  return val;
}



/***********************************************************************/
/** Set Parameter over keyboard (int).

    Called by main().
    @see main()
    @param text[] name of parameter
    @param minp,maxp minimum and maximum value of parameter 
    @param start start value for parameter
    @param inc step to increment parameter
*/
/***********************************************************************/

int set_iparam(char text[], int minp, int start, int maxp, int inc)
{
  int val;
  int done = FALSE;

  LCDClear();
  LCDPrintf("Set Parameter\n%s\n", text);
  LCDMenu("+", "-", " ","OK");
  val = start;
  do
  {
    LCDSetPos(5, 0);
    LCDPrintf("%3d\n", val);
    switch(KEYGet())
    {
    case KEY1:
      if (val <maxp)
	val += inc;
      break;
    case KEY2:
      if (val > minp)
	val -= inc;
      break;
    case KEY4:
      done = TRUE;
    }
  } while (!done);

  LCDClear();
  return val;
}





/***********************************************************************/
/** Discrete arc-tangent.
    Function to calculate arc-tangent only using integers.

    @param dx, dy coordinates (multiplied by 1000 -> integers)
    @return atan of dy / dx
*/
/***********************************************************************/
float DiscAtan(int dy, int dx)
{
  int phi;
  long quo, Adx, Ady;
  int return_value;
  
  Adx = (long) abs(dx);
  Ady = (long) abs(dy);

  if (Adx == 0 || Ady == 0)
    return_value = ((Adx == 0 && Ady == 0) ? 0:
            ((Adx == 0) ?
             ((dy < 0) ? -4 : 4):   /* 12:4 */
             ((dx < 0) ? 8 : 0)));
  else
  {
    quo = (100 * Ady) / Adx;

    phi = ((quo < 20) ? 0:
           ((quo < 67) ? 1:
            ((quo < 150) ? 2:
             ((quo < 503) ? 3 : 4))));

    return_value = ((dy > 0) ?
            ((dx > 0) ? phi : 8-phi):    /* phi:8-phi */
            ((dx < 0) ? -8+phi:          /* 8+phi */
             ((phi == 0) ? 0 : -phi)));  /* 16-phi */

  }
  return (float)return_value * PI/8.0;
}





/*@}*/
