#include <webots/robot.h>
#include <webots/differential_wheels.h>
#include <webots/accelerometer.h>
#include <webots/keyboard.h>
#include <webots/led.h>

// Global defines
#define LEFT 0
#define RIGHT 1
#define TIME_STEP  32           // [ms]

// LEDs
#define NB_LEDS    8
WbDeviceTag led[NB_LEDS];

//
double ledDelay = 0.20; // 1 second
double lastLedTime = 0.0; // time in seconds
bool isLedOn = false;
double currentTime = 0.0;
//

int speed[2]={0,0};


static void reset(void)
{
  // get Leds (from led0 to led7)
  int i;
  char text[5] = "led0";
  for(i = 0; i < NB_LEDS; i++) {
    led[i] = wb_robot_get_device(text); //get leds
    text[3]++;
  }

  // enable keyboard
  wb_keyboard_enable(TIME_STEP);

  // Switch off leds
  for(i = 0; i < NB_LEDS; i++) {
    wb_led_set(led[i],0);
  }
}


static void switch_off_all_leds() {
  for(int it=0;it<NB_LEDS;it++) {
    wb_led_set(led[it],0); // switch off led[it]
  }
}

static void switch_on_all_leds() {
  for(int it=0;it<NB_LEDS;it++) {
    wb_led_set(led[it],1); // switch on led[it]
  }
}

void blinking_led() {
      if (currentTime - lastLedTime > ledDelay) {
          if (isLedOn)
              switch_on_all_leds();
         else
              switch_off_all_leds();
         isLedOn = !isLedOn;
         lastLedTime = currentTime; }
}

void blinking_right(){
         if (currentTime - lastLedTime > ledDelay) {
          if (isLedOn) {
                  for(int it=0;it<NB_LEDS-4;it++) {
                    wb_led_set(led[it],1); } }
         else
              switch_off_all_leds();
         isLedOn = !isLedOn;
         lastLedTime = currentTime; }
}

void blinking_left(){
if (currentTime - lastLedTime > ledDelay) {
          if (isLedOn) {
                  for(int it=7;it>NB_LEDS-4;it--) {
                    wb_led_set(led[it],1); }

                    wb_led_set(led[0],1);  }
         else
              switch_off_all_leds();
         isLedOn = !isLedOn;
         lastLedTime = currentTime; }
}


static void run(void) {
  currentTime = wb_robot_get_time();

  int key = wb_keyboard_get_key();
  switch (key) {
    case WB_KEYBOARD_UP:
      speed[LEFT]=150;
      speed[RIGHT]=150;
      break;
     case WB_KEYBOARD_DOWN:
      speed[LEFT]=-150;
      speed[RIGHT]=-150;
      break;
     case WB_KEYBOARD_LEFT:
      speed[RIGHT]=50;
      speed[LEFT]=-50;
      break;
    case WB_KEYBOARD_RIGHT:
      speed[LEFT]=50;
      speed[RIGHT]=-50;
      break;
    case '0':
      speed[LEFT]=speed[RIGHT]=0;
      break;
    default:
      break;
  }

     if (speed[LEFT]>0 && speed[RIGHT]<0) blinking_right(); //right
else if (speed[LEFT]<0 && speed[RIGHT]>0) blinking_left();  //left
else if (speed[LEFT]<0 && speed[RIGHT]<0) blinking_led();   //backwards
else switch_off_all_leds();

  wb_differential_wheels_set_speed(speed[LEFT],speed[RIGHT]);

  return;
}

int main() {
  wb_robot_init();
  reset();

  while(wb_robot_step(TIME_STEP) != -1) {
    run();
  }

  wb_robot_cleanup();

  return 0;
}