#include <webots/robot.h>
#include <webots/distance_sensor.h>
#include <math.h>
#include <stdio.h>
#include <webots/motor.h>


#define NB_SENSORS 8
#define TIME_STEP 64
#define RANGE (1024 / 2)
#define MAXIMUM_SPEED  1000
#define MAX_SPEED_WEB      6.28    // Maximum speed webots

WbDeviceTag left_motor; //handler for left wheel of the robot
WbDeviceTag right_motor; //handler for the right wheel of the robot

int main(int argc, const char *argv[]) {
  WbDeviceTag ps[NB_SENSORS];
  static double ds_value[NB_SENSORS];
  int i;
  char name[] = "ps0";
  float msl_w, msr_w;
  
  double speed[2], duration = TIME_STEP;
  
  wb_robot_init();
  
  //get motors
  left_motor = wb_robot_get_device("left wheel motor");
  right_motor = wb_robot_get_device("right wheel motor");
  wb_motor_set_position(left_motor, INFINITY);
  wb_motor_set_position(right_motor, INFINITY);
    
  // get and enable each distance sensor
  for(i = 0; i < NB_SENSORS; i++) {
    ps[i] = wb_robot_get_device(name);
    wb_distance_sensor_enable(ps[i], TIME_STEP);
    name[2]++; // increase the device name to "ps1", "ps2", etc.
  }
  
  // main loop
  while(wb_robot_step(duration)!=-1) {
    // read sensor values
    for (i = 0; i < NB_SENSORS; i++){
      ds_value[i] = wb_distance_sensor_get_value(ps[i]); // range: 0 (far) to 3800 (close)
    } 

//******* modify robot behavior here*****
    //set the speed of each wheel
    speed[0] = 500;
    speed[1] = 500;
    duration = 20; //amount of miliseconds during which the vehicle will have the above speeds
//***************************************
    
    // actuate wheel motors
    msl_w = speed[0]*MAX_SPEED_WEB/1000;
    msr_w = speed[1]*MAX_SPEED_WEB/1000;
    wb_motor_set_velocity(left_motor, msl_w);
    wb_motor_set_velocity(right_motor, msr_w);
  }
//***************************************
  
  return 0;
}