/* radar.c */

#define POT_RADAR_CENTER 117
#define POT_RADAR_90_INT 96
#define POT_RADAR_POS_CONST (PI/96.0)
#define POT_RADAR_MIN 90
#define POT_RADAR_MAX 160

int get_radar_pos_int()
{
  return POT_RADAR_CENTER - analog(31);
}

float get_radar_pos()
{
  return (float)get_radar_pos_int()*POT_RADAR_POS_CONST;
}

/* 0: stop, 1: ccw, -1: cw */
void set_radar_movement(int direction)
{
  if ((direction == -1 && motor_speed[MOTOR_RADAR] > 0) ||
      (direction == 1 && motor_speed[MOTOR_RADAR] < 0) ||
      (direction == 0 && motor_speed[MOTOR_RADAR] == 0))
    return;

  set_motor_speed(MOTOR_RADAR, 0);
  if (direction)
  {
    msleep(10L);
    set_motor_speed(MOTOR_RADAR, 100*direction);
  }
}

void init_radar()
{
  start_process(radar_process());

  if (analog(PORT_POT_RADAR) > 128)
    set_radar_movement(1);
  else
    set_radar_movement(-1);
  
}

/* return value: -128 to 128: edge angle
   -1000: timeout (radar not working)
   -1001: no edge found */
/*
int radar_find_edge(long timeout)
{
  long start_time = mseconds();
  int d, t;
  
  if (get_radar_pos_int() > 0)
    d = -1;
  else
    d = 1;

  if (distance(analog(PORT_DISTANCE_RADAR)) == DISTANCE_SENSOR_MAX_INT)
  {
    set_radar_movement(d);
    while(distance(analog(PORT_DISTANCE_RADAR)) ==
	  DISTANCE_SENSOR_MAX_INT && motor_speed[MOTOR_RADAR] &&
	  mseconds() - start_time < timeout)
      msleep(10L);

    d = get_radar_pos_int();
    set_radar_movement(0);
    if (distance(analog(PORT_DISTANCE_RADAR)) != DISTANCE_SENSOR_MAX_INT)
      return d;
    if (mseconds() - start_time >= timeout)
      return -1000;
    else
      return -1001;
  }
  else
  {
    set_radar_movement(-d);
    while(distance(analog(PORT_DISTANCE_RADAR)) !=
	  DISTANCE_SENSOR_MAX_INT && motor_speed[MOTOR_RADAR] &&
	  mseconds() - start_time < timeout)
      msleep(10L);

    d = get_radar_pos_int();
    set_radar_movement(0);
    if (distance(analog(PORT_DISTANCE_RADAR)) == DISTANCE_SENSOR_MAX_INT)
      return d;
    if (mseconds() - start_time >= timeout)
      return -1000;
    else
      return -1001;
  }
}
*/

long radar_time;

/* make sure the radar doesnt hit the end */
void radar_process()
{
  radar_time = mseconds();
  while(1)
  {
    int i = analog(PORT_POT_RADAR);

    if (i <= POT_RADAR_MIN && motor_speed[MOTOR_RADAR] > 0)
    {
      set_motor_speed(MOTOR_RADAR, 0);
      msleep(500L);
      set_motor_speed(MOTOR_RADAR, -100);
      radar_time = mseconds();
    }
    else if (i >= POT_RADAR_MAX && motor_speed[MOTOR_RADAR] < 0)
    {
      set_motor_speed(MOTOR_RADAR, 0);
      msleep(500L);
      set_motor_speed(MOTOR_RADAR, 100);
      radar_time = mseconds();
    }
    else if (mseconds() - radar_time > 5000L)
      set_motor_speed(MOTOR_RADAR, 0);

    msleep(20L);
  }
}