/* Konstanten */
float fahrgeschwindigkeit=30.0;

/* Logische Werte */
int TRUE = 1;
int FALSE = 0;

float max_clicks_sec =  50.0;  /* Maximalgeschwindigkeit */

/* Globale Flags */
/* Bumper und IR-Sensoren */
int left_ir=FALSE;
int right_ir=FALSE;
int left_bumper=FALSE;
int right_bumper=FALSE;
int back_bumper=FALSE;
int all_bumper = FALSE;
 
 /* Parameter */
long  timefaktor = 2L;       /* zur Bumperjustierung */
int Schwell_Light = 18;      /* Schwellwert fuer gute Lichterkennung */
float wait_time = 15.0;      /* Wartezeit fuer Licht suchen mit Rotation */
float drall_speed =-2.0;     /* leichten Bogen fahren */


void beepbeep()
{
  tone(700.5, 0.200);
  tone(1396.9, 0.200);
  tone(1046.5, 0.200);
  tone(698.5, 0.200);
}

void music()
{
   int t1;
   float t;
   float f;

   for ( t1 = 0; t1 <= 30; t1++ )
   {
     t = ((float)t1)*100.0;
     f = 700.0 * (sin(t / 1000.0 * 3.141592654) + 1.01 ) + 300.0;
     tone(f, 0.2);
   }
}

int abs( int x )
{
  if ( x >= 0 ) return x;
  else return -x;
}

/* Mittelwert der Lichtintensitaet ermitteln */
int get_light()
{
  return( (int) (analog(photo_right) + analog(photo_left)) / 2 );
}

/* Infrarotsensoren ueberpruefen und globale Flags setzen */
void check_ir()
{
  int IR_COUNT=5;

  int n;
  int l_n;
  int r_n;
  int ir;

  left_ir=FALSE;
  right_ir=FALSE;
  while(TRUE)
  {
   l_n = 0;
   r_n = 0;
   for (n =1 ; n <= IR_COUNT ; n++)
   {
    ir=ir_detect();
    if (ir==1) l_n++;
    if (ir==2) r_n++;
    if (ir==3) { l_n++; r_n++; }
   }
   left_ir=(l_n>=IR_COUNT);
   right_ir=(r_n>=IR_COUNT);
  }
}

void check_bumper()
{
  int BUMPER_COUNT=10;

  int n;      
  int ln;
  int rn;
  int bn;
  int bmp;

  left_bumper=FALSE;
  right_bumper=FALSE;
  back_bumper=FALSE;
  all_bumper = FALSE;

  while(TRUE)
  {
   ln=0;
   rn=0;
   bn=0;
   for (n=1;n<=BUMPER_TEST;n++)
   {
    bmp=bumper();
    if (bmp&0b001)
    {
      ln++;
    }
    if (bmp&0b010)
    {
      rn++;
    }
    if (bmp&0b100)
    {
      bn++;
    }
    defer();
   }
   left_bumper=(ln==BUMPER_COUNT);
   right_bumper=(rn==BUMPER_COUNT);
   back_bumper=(bn==BUMPER_COUNT);
   all_bumper = left_bumper | right_bumper | back_bumper;
   defer();
  }
}


/* sucht Helligkeitsmaximum im Raum durch Rotation und gibt diesen Wert zurueck */
int find_min_light( float speed )
{
  int light;
  int min_light = 10000;
  int clicks = 0;

  beep();
  printf("min\n");

  get_left_clicks();      /* Clicks ruecksetzen */
  get_right_clicks();


  if ( analog(photo_right) < analog(photo_left) )
  {
    motor(0, speed);      /* rechtsdrehen */
    motor(1, -speed);
  }
  else
  {
    motor(0, -speed);      /* linksdrehen */
    motor(1, speed);
  }

  clicks = 0;

  while( (  (clicks += (get_left_clicks() + get_right_clicks()) / 2)) < 37
         && !all_bumper )  /* eine Umdregung machen solange kein Bumper betaetigt */
  {
    printf("FIND MIN  %d   %d\n", clicks, light);
    light = get_light();                   /* Lichtwert holen */
    if ( light < min_light )               /* minimum merken */
    {
      min_light = light;
    }
    sleep(0.2);
  }
  motor( 0, 0.0 );                         /* Motoren aus */
  motor( 1, 0.0 );
  beep();
  printf("min end  %d\n", min_light);
  return( min_light);
}

/* Roboter in die Richtung rotieren, in der es am hellsten ist */
int find_light(float speed)
{
  int light_left;
  int light_right;
  int light;
  int minimum;
  int count = 0;

  minimum = find_min_light( speed );     /* Minimum suchen durch Rotation */

  light_left = analog(photo_left);
  light_right = analog(photo_right);
  light = (light_left + light_right) / 2;
  beep();

  printf("%d\n", light );

  if ( analog(photo_right) < analog(photo_left) )
  {
    motor(0, speed);      /* rechtsdrehen */
    motor(1, -speed);
  }
  else
  {
    motor(0, -speed);      /* linksdrehen */
    motor(1, speed);
  }


  while( light > minimum && count < 1000 )     /* Hellste Stelle aufsuchen */
  {
     printf("FIND LIGHT  %d    %d\n", light, minimum);
     light = get_light();
     count++;
     defer();
  }

  motor(0, 0.0);         /* stop */
  motor(1, 0.0);
  beep();
  return( light_left);   /* Helligkeitswert zurueckgeben */
}

/* Fahren und sich dem Licht zuwenden, d.h. Roboter dreht nach links, wenn
   es links heller ist 
*/
void go( int distance, float speed, int direction )
{
  float time;
  float motor_s;

  int clicks;
  int p_left;
  int p_right;
  float v_rot;

  motor_s = speed;
  if ( direction == 1 )
  {
    motor_s = -motor_s;
  }

  get_left_clicks();
  get_right_clicks();

  clicks = 0;

  while( (  (clicks += (get_left_clicks() + get_right_clicks()) / 2)) < distance
           & !all_bumper )   /* Solange Strecke noch nicht zurueckgelegt und kein Bumper */
  {
    if ( (p_right = analog(photo_right)) < (p_left = analog(photo_left)) )
    {
      v_rot = -((float)(p_left - p_right)) * 2.5;   /* links heller */
    }
    else
    {
      v_rot = ((float)(p_right - p_left)) * 2.5;    /* rechts heller */
    }
    if ( direction == 1)
    {
      v_rot = v_rot;     /* - */
    }
    drive( motor_s, v_rot );
    printf("GO  %d  %d\n", p_left, p_right );
    msleep(150L);
  }
  drive( 0.0, 0.0 );   /* stop */
}


/* Fahrt geradeaus und versucht einem Hindernis auszuweichen.
   Wird ein Hindernis rechts erkannt, dann wird nach links abgedreht.
*/
void go2( int distance, float speed, int direction )
{
  float motor_s;
  int p_left;
  int p_right;
  int clicks;
  float v_rot;

  go_active = 1;

  motor_s = speed;
  if ( direction == 1 )
  {
    motor_s = -motor_s;
  }

  get_left_clicks();        /* Clicks ruecksetzen */
  get_right_clicks();

  clicks = 0;

  while( (  (clicks += (get_left_clicks() + get_right_clicks()) / 2)) < distance
           & !all_bumper )
  {
    if ( left_ir && !right_ir )
    {
      v_rot =  -0.3 * speed;
       motor_s = 1.0 * speed;
    }
    else if ( right_ir && !left_ir )
    {
      v_rot = 0.3 * speed;
      motor_s = 1.0 * speed;
    }
    else if ( left_ir && right_ir )
    {
      v_rot = 2.0 * speed;           /* rotieren */
      motor_s = 0.0 * speed;
    }
    else
    {
      motor_s = speed;
      v_rot = 0.0;
    }
    if ( direction == 1)
    {
      v_rot = v_rot;     /* - */
    }

    drive( motor_s, v_rot + drall_speed );
    printf("GO2  %d  %d\n", motor_s, v_rot );
    defer();
  }
  drive( 0.0, 0.0 );   /*stop */
  go_active = 0;
}



/* Routine um auf die Bumper zu reagieren.
   Wird ein seitlicher Bumper betaetigt, so wird in St�ck 
   rueckwaerts gefahren und ein St�ck vom Hindernis weggedreht
*/
int bumper_react( float speed )
{

  while(TRUE)
  {
     if ( left_bumper && right_bumper )
     {
       printf("LEFT AND RIGHT BUMPER\n");
       drive( -speed, 0.0);   /* rueckwaerts */
       msleep(300L * timefaktor);
       drive( 0.0, speed );  /* rechtsdrehen */
       msleep(300L * timefaktor);
       drive(0.0, 0.0);
     }
     else if ( left_bumper && !right_bumper )
     {
       printf("LEFT BUMPER\n");
       drive( -speed, 0.0);   /* rueckwaerts */
       msleep(300L * timefaktor);
       drive( 0.0, speed );  /* rechtsdrehen */
       msleep(200L * timefaktor);
       drive(0.0, 0.0);
     }
     else if ( right_bumper && !left_bumper)
     {
       printf("RIGHT BUMPER\n");
       drive( -speed, 0.0);   /* rueckwaerts */
       msleep(300L * timefaktor);
       drive( 0.0, -speed );
       msleep(200L * timefaktor);
       drive(0.0, 0.0);
     }
     else if ( back_bumper && !left_bumper && !right_bumper)
     {
       printf("BACK BUMPER\n");
       drive( speed, 0.0);   /* vorwaerts */
       msleep(200L * timefaktor);
       /*
       drive( 0.0, speed );
       msleep(500L * timefaktor);
       */
       drive(0.0, 0.0);
     }

     defer();
  }
}

void main()
{
  int hits;
  int dist = 15;

  int light = 1000;            /* Helligkeitswert */
  int min_light = 1000;        /* Minimum des Helligkeitswertes */
  int abs_min_light = 1000;
  int ziel_light = 10;         /* Schwellwert fuer Zielhelligkeit */

  float rot_speed = 35.0;
  float trans_speed = 45.0;
  float old_time;

  int bumper_react_pid;

  hits = 0;

  init_motors();
  init_velocity();

  sleep(3.0);
  beepbeep();

  start_process( check_bumper() );  /* bumper pr�fen */
  start_process( check_ir() );      /* Ir-Detektor pruefen */
  bumper_react_pid = start_process(bumper_react(40.0)); /* Bumper bearbeiten */


  printf("READY");
  sleep(2.0);

  min_light = find_light(rot_speed);   /* helle Stelle suchen */
  light = min_light;
  old_time = seconds();

  while( light > ziel_light  )
  {
    light = get_light();               /* Helligkeit holen */
    if ( light < abs_min_light ) abs_min_light = min_light;  /* Minimum bestimmen */
    if ( !all_bumper )    /* kein Bumper */
    {
      if (light < Schwell_Light || min_light < Schwell_Light) /* Nah genug an Lichtquelle */
      {
        go( 30, trans_speed, 0);   /* Fahren und sich dem Licht zuwenden */
      }
      else  /* noch zu dunkel */
      {
        go2( 30, trans_speed, 0);   /* Fahren und einem Hindernis ausweichen */
      }
      if ( (seconds() - old_time) > wait_time )  /* wenn Warezeit vorueber */
      {
        light = get_light();
        if ( light < Schwell_Light )             /* und ausreichende Helligkeit */
        {
          min_light = find_light( rot_speed);    /* dann rotiren und Licht suchen */
          old_time = seconds();
        }
      }
    }
    defer();
  }    /* Ziel erreicht */
  drive(0.0, 0.0); /* stop */
  kill_process( bumper_react_pid );   /* nichtmehr auf Bumper reagieren */
  drive(0.0, 0.0);
  printf("ENDE  %d", abs_min_light);
  music();                            /* Let`s grooooove  */
}