#include <webots/robot.h>
#include <webots/distance_sensor.h>
#include <math.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,j;
  char name[] = "ps0";
  float msl_w, msr_w;
  
  double speed[2]={0.0, 0.0};
  
  
  // Braitenberg coefficients: 
  // { {a_left_0, a_right_0}, {a_left_1, a_right_1}, ...}
  // MODIFY HERE!
  // Lab 07 - set appropriate coefficients
  double braitenberg_coefficients[8][2] =
  { {30, -25},  {25, -25}, {25, -25}, {-25, -25},
    {-25, -25}, {-25, 25}, {-25, 25}, {-25, 30}  };
  
  
  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(TIME_STEP)!=-1) {
    // Reset wheel speeds for each loop iteration
    speed[0] = 0.0;
    speed[1] = 0.0;
    // 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)
    
    // ***************************************************//
    // Implement Braitenberge controller HERE!
    // computing the speed of each wheel
    
    // ***************************************************//
    // gurantee the speed sent to motor to be smaller than MAXIMUM value
    if(fabs(speed[0]) > MAXIMUM_SPEED)
        speed[0] = copysign(MAXIMUM_SPEED,speed[0]);
    if(fabs(speed[1]) > MAXIMUM_SPEED)
        speed[1] = copysign(MAXIMUM_SPEED,speed[1]);    
    // 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;
}