#include <nibobee/iodefs.h>
#include <nibobee/sens.h>
#include <nibobee/motpwm.h>
#include <nibobee/motpid.h>
#include <nibobee/analog.h>
#include <nibobee/led.h>
#include <nibobee/delay.h>
#include <nibobee/clock.h>
#include <nibobee/odometry.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include <util/delay.h>
#include <stdlib.h>

 
 
 
int main() {
motpwm_init();
sens_init();
analog_init();
int8_t matrix=0;
uint8_t toggle;
int16_t lval, rval;
int16_t speed_l=0;
int16_t speed_r=0;
uint16_t zufall = 0, random = 0;
 
activate_output_group(IO_LEDS);  // LED bits als Output
  activate_output_groupbit(IO_EXT, 2);
  activate_output_groupbit(IO_EXT, 3);
 
  //start verzögerung
  led_set(LED_L_RD, 1);    
  led_set(LED_R_RD, 1);
  _delay_ms(700);
  led_set(LED_L_YE, 1);
  led_set(LED_R_YE, 1);
  _delay_ms(700);
 
  led_set(LED_L_RD, 0);    
  led_set(LED_R_RD, 0);
  led_set(LED_L_YE, 0);
  led_set(LED_R_YE, 0);
  _delay_ms(700);
  led_set(LED_L_RD, 1);    
  led_set(LED_R_RD, 1);
  led_set(LED_L_YE, 1);
  led_set(LED_R_YE, 1);
    _delay_ms(300);
  led_set(LED_L_RD, 0);    
  led_set(LED_R_RD, 0);
  led_set(LED_L_YE, 0);
  led_set(LED_R_YE, 0);
 
while(1==1) {
    enable_interrupts();
    sei();
 
    // IR LEDs einschalten  
    clear_output_groupbit(IO_EXT, 2);
    clear_output_groupbit(IO_EXT, 3);
    _delay_ms(10);
 
    // Werte links und rechts messen
    lval = analog_getValue(ANALOG_EXT2);
    rval = analog_getValue(ANALOG_EXT3);
 
    // IR LEDs ausschalten
    set_output_groupbit(IO_EXT, 2);
    set_output_groupbit(IO_EXT, 3);
    _delay_ms(10);
 
    // Werte links und rechts messen und von vorherigem Ergebnis abziehen,
    // damit das Umgebungslicht rausgerechnet wird!
    lval -= analog_getValue(ANALOG_EXT2);
    rval -= analog_getValue(ANALOG_EXT3);
 
 
    if ((rval && lval > 8) && (rval > lval)) {
    led_set(LED_R_YE, 1);
    matrix = 2;
    }
 
    else if ((rval && lval > 8) && (rval < lval)) {
    led_set(LED_L_YE, 1);
    matrix = 2;
    }
 
    else if ((sens_getRight() != 0 ) && (sens_getLeft() != 0 )){
    speed_l = -600;
    speed_r = -600;
    led_set(LED_R_RD, 1);
    led_set(LED_L_RD, 1);
    matrix = 1;
    }
 
    else if (sens_getRight() != 0 ) {
    speed_l = -600;
    speed_r = -300;
    led_set(LED_R_RD, 1);
    matrix = 1;
    }
 
    else if (sens_getLeft() != 0 ) {
    speed_r = -600;
    speed_l = -400;
    led_set(LED_L_RD, 1);
    matrix = 1;
    }
    else {speed_l = 600; speed_r = 600;}
 
//zufällige zurückfahrt    
if (matrix == 2){
zufall = sens_getLeft() + sens_getRight() + rval + lval;
srand(zufall);
random = rand() % 2 ;
if (random == 1){
    speed_l = -600;
    speed_r = 0;
}
else {
    speed_l = 0;
    speed_r = -600;}
}
 
 
//geschwindigkeit einstellen    
motpwm_setLeft(speed_l);
motpwm_setRight(speed_r);
 
//zufällige zeit zum zurückfahren
if (matrix > 0){
matrix = 0;
zufall = 0;
zufall = sens_getLeft() + sens_getRight() + rval + lval;
srand(zufall);
random =  rand() % 1001 ;
delay (random);
    led_set(LED_L_YE, 0);
    led_set(LED_R_YE, 0);
    led_set(LED_L_RD, 0);
    led_set(LED_R_RD, 0);
}
}
return 0;
}