#include <nibobee/iodefs.h> 
#include <nibobee/motpwm.h> 
#include <nibobee/analog.h> 
#include <nibobee/line.h> 
#include <nibobee/led.h> 
#include <nibobee/delay.h> 

#include <avr/io.h> 
#include <avr/interrupt.h> 
#include <util/delay.h> 


int main() { 
  activate_output_group(IO_LEDS);  // LED bits als Output 
  motpwm_init(); 
  motpwm_setLeft(0); 
  motpwm_setRight(0); 
  analog_init(); 
  line_readPersistent(); 
  set_output_group(IO_SENS);       // Pull-ups aktivieren 
  activate_output_bit(IO_LINE_EN); 

  int16_t speed_flt_l=0; 
  int16_t speed_flt_r=0; 

  // Countdown: LEDs blinken lassen 
  for (uint8_t i=0; i<10; ++i) { 
    led_set(LED_L_RD, 1); 
    led_set(LED_R_RD, 1); 
    _delay_ms(10); 
    led_set(LED_L_RD, 0); 
    led_set(LED_R_RD, 0); 
    _delay_ms(990); 
  } 
  led_set(LED_L_YE, 1); 
  led_set(LED_R_YE, 1); 
  _delay_ms(1000); 
  led_set(LED_L_YE, 0); 
  led_set(LED_R_YE, 0); 

  // Hauptschleife: 
  while(1) { 
    sei(); 
    _delay_ms(1); 
    int16_t speed_l=0; 
    int16_t speed_r=0; 

    int16_t lval = line_get(LINE_L); 
    int16_t cval = line_get(LINE_C); 
    int16_t rval = line_get(LINE_R); 

    if (lval+cval+rval < 20) { 
      led_set(LED_L_RD, 0); 
      led_set(LED_R_RD, 0); 
      speed_r=0, speed_l=0; 
    } else if ((lval<cval) && (lval<rval)) { 
      // lval is minimum 
      led_set(LED_L_RD, 1); 
      led_set(LED_R_RD, 0); 
      speed_r=550, speed_l=450-1*(cval-lval); 
    } else if ((rval<cval) && (rval<lval)) { 
      // rval is minimum 
      led_set(LED_L_RD, 0); 
      led_set(LED_R_RD, 1); 
      speed_r=450-1*(cval-rval), speed_l=550; 
    } else { 
      // cval is minimum 
      led_set(LED_L_RD, 1); 
      led_set(LED_R_RD, 1); 
      speed_r=750 + 1*(rval-cval), speed_l=750 + 1*(lval-cval); 
    } 

    speed_flt_l*=3; speed_flt_l+=speed_l; speed_flt_l/=4; 
    speed_flt_r*=3; speed_flt_r+=speed_r; speed_flt_r/=4; 
    
    motpwm_setLeft(speed_flt_l); 
    motpwm_setRight(speed_flt_r); 

  } 
  return 0; 
}