#include <stdio.h>
#include <string.h>

// include webots libraries
#include <webots/robot.h>
#include <webots/keyboard.h>
#include <webots/motor.h>
#include <webots/position_sensor.h>
#include <webots/gps.h> 
#include <webots/accelerometer.h>

// include odometry file
#include "odometry.h"

//-----------------------------------------------------------------------------------//
/*MACRO*/
#define CATCH(X,Y)      X = X || Y
#define CATCH_ERR(X,Y)  controller_error(X, Y, __LINE__, __FILE__)

/*CONSTANTES*/
#define MAX_SPEED 1000          // Maximum speed 
#define INC_SPEED 5             // Increment not expressed in webots 
#define MAX_SPEED_WEB 6.28      // Maximum speed webots
#define TIME_INIT_ACC 5 	      // Time in second

/*VERBOSE_FLAGS*/
#define VERBOSE_GPS false       // Print GPS values
#define VERBOSE_ACC false       // Print accelerometer values
#define VERBOSE_ACC_MEAN false  // Print accelerometer mean values
#define VERBOSE_POSE false      // Print pose values
#define VERBOSE_ENC false       // Print encoder values
#define VERBOSE_KEY false       // Print keyboard values

//-----------------------------------------------------------------------------------//
/*DEFINITIONS*/
typedef struct
{
  int time_step;
  WbDeviceTag gps;
  WbDeviceTag acc;
  WbDeviceTag left_encoder;
  WbDeviceTag right_encoder;
  WbDeviceTag left_motor; 
  WbDeviceTag right_motor; 
} simulation_t;

typedef struct 
{
  double prev_gps[3];
  double gps[3];
  double acc_mean[3];
  double acc[3];
  double prev_left_enc;
  double left_enc;
  double prev_right_enc;
  double right_enc;
} measurement_t;

typedef struct
{
  bool isRunning;
  bool sign_left;
  bool sign_right;
  int  speed;
} motor_t;

//-----------------------------------------------------------------------------------//
/*VARIABLES*/
static simulation_t   _robot;
static measurement_t  _meas;
static pose_t         _pose, _odo_acc, _odo_enc;
static pose_t         _pose_origin = {-0.4, 0.4, 0.0}; // wrt gps origin
static motor_t        _motor = {false, true, true, MAX_SPEED / 4.0};

static FILE *fp;
//-----------------------------------------------------------------------------------//
/*FUNCTIONS*/
static bool controller_init();
static bool controller_init_time_step();
static bool controller_init_gps();
static bool controller_init_acc();
static bool controller_init_encoder();
static bool controller_init_motors();
static bool controller_init_log(const char* filename);

static void controller_get_pose();
static void controller_get_gps();
static double controller_get_heading();
static void controller_get_acc();
static void controller_get_encoder();
static void controller_get_keyboard();

static void controller_set_speed();

static void controller_print_log(double time);

static void controller_compute_mean_acc();

static bool controller_error(bool test, const char * message, int line, const char * fileName);

//-----------------------------------------------------------------------------------//

int main() 
{

  //initialize the webots controller library
  wb_robot_init();
  
  if(CATCH_ERR(controller_init(), "Controller fails to init \n"))
    return 1;

  while (wb_robot_step(_robot.time_step) != -1) 
  {
    // 1. Perception / Measurement
    controller_get_pose();

    controller_get_acc();

    controller_get_encoder();

  // To Do : Uncomment these lines for part 3
	// if( wb_robot_get_time() < TIME_INIT_ACC )
	// {
		// controller_compute_mean_acc();
	// }
	// else
	// {
   
		// 2. Localization
		odo_compute_acc(&_odo_acc, _meas.acc, _meas.acc_mean);
	
		odo_compute_encoders(&_odo_enc, _meas.left_enc - _meas.prev_left_enc, _meas.right_enc - _meas.prev_right_enc);
		
		// 3. Cognition / Action
		controller_get_keyboard();
		
		// 4. Motor Controls
		controller_set_speed();

	// To Do : Uncomment the following line for part 3
  // } 
	
	// 5. Logging Step
	controller_print_log(wb_robot_get_time());
  
  }

  // Close the log file
  if(fp != NULL)
    fclose(fp);

  // End of the simulation
  wb_robot_cleanup();

  return 0;
}

//-----------------------------------------------------------------------------------//

/**
 * @brief     Get the gps meas  }urements for the position of the robot. Get the heading angle. Fill the pose structure. 
 */
void controller_get_pose()
{
  // Call the function to get the gps measurements
  controller_get_gps();
  
  // To Do : Fill the structure pose_t {x, y, heading}. Use the _pose_origin and output of controller_get_heading() function.
  // _pose.x = ...
  
  // _pose.y = ...
  
  // _pose.heading = ...

  if(VERBOSE_POSE)
    printf("ROBOT pose : %g %g %g\n", _pose.x , _pose.y , RAD2DEG(_pose.heading));
}


/**
 * @brief      Get the gps measurments for position of the robot.
 */
void controller_get_gps()
{
  // store the previous measurements of the gps _meas.gps into _meas.prev_gps (use memcpy)
  memcpy(_meas.prev_gps, _meas.gps, sizeof(_meas.gps));
  
  // get the positions from webots for the gps. Uncomment and complete the following line Note : Use _robot.gps
  const double * gps_position = wb_gps_get_values(_robot.gps);

  // Copy the gps_position into the measurment structure _meas.gps (use memcpy)
  memcpy(_meas.gps, gps_position, sizeof(_meas.gps));
  
  if(VERBOSE_GPS)
    printf("ROBOT gps is at position: %g %g %g\n", _meas.gps[0], _meas.gps[1], _meas.gps[2]);
  
}


/**
 * @brief      Compute the heading (orientation) of the robot based on the gps position values.
 *
 * @return     return the computed angle
 */
double controller_get_heading()
{
  // Calculate delta x and y position changes and heading
  double delta_x = -(_meas.gps[1] - _meas.prev_gps[1]);
  
  double delta_y = _meas.gps[0] - _meas.prev_gps[0];
  
  double heading = atan2(delta_y, delta_x);
  
  return heading;
}


/**
 * @brief      Read the acceleration values from the sensor
 */
void controller_get_acc()
{
  // Call the function to get the accelerometer measurements. Uncomment and complete the following line. Note : Use _robot.acc
  const double * acc_values = wb_accelerometer_get_values(_robot.acc);

  // Copy the acc_values into the measurment structure (use memcpy)
  memcpy(_meas.acc, acc_values, sizeof(_meas.acc));

  if(VERBOSE_ACC)
    printf("ROBOT acc : %g %g %g\n", _meas.acc[0], _meas.acc[1] , _meas.acc[2]);
}


/**
 * @brief      Read the encoders values from the sensors
 */
void controller_get_encoder()
{
  // Store previous value of the left encoder
  _meas.prev_left_enc = _meas.left_enc;

  _meas.left_enc = wb_position_sensor_get_value(_robot.left_encoder);

  // Store previous value of the right encoder
  _meas.prev_right_enc = _meas.right_enc;
  
  _meas.right_enc = wb_position_sensor_get_value(_robot.right_encoder);

  if(VERBOSE_ENC)
    printf("ROBOT enc : %g %g\n", _meas.left_enc, _meas.right_enc);
}


/**
 * @brief      Change the speed of the motors according to the keys pressed by the user
 */
void controller_get_keyboard()
{
  // Get the ascii code of the key pressed by the user 
  int key = wb_keyboard_get_key();

  char msg[128] = "";

 // move the robot with the keys
  switch (WB_KEYBOARD_KEY & key) 
  {
    case WB_KEYBOARD_UP: 
      sprintf(msg, "Move forward\n");
      _motor.sign_left  = true;
      _motor.sign_right = true;
      break;
  
    case WB_KEYBOARD_DOWN: 
     sprintf(msg, "Move backward\n");
      _motor.sign_left  = false;
      _motor.sign_right = false;
     break;
  
    case WB_KEYBOARD_LEFT: // turn left
     sprintf(msg, "Turn left\n" );
      _motor.sign_left  = false;
      _motor.sign_right = true;
      break;
  
    case WB_KEYBOARD_RIGHT: 
     sprintf(msg, "Turn right\n");
      _motor.sign_left  = true;
      _motor.sign_right = false;
      break;
  
    case 'S':
       if(_motor.isRunning) sprintf(msg, "STOP\n");
      _motor.isRunning = false;
    break;
  
    case 'R':
       if(!_motor.isRunning) sprintf(msg, "RUN\n");
      _motor.isRunning = true;
    break;
  
    case 'U':
       sprintf(msg, "Speed UP\n");
      _motor.speed += INC_SPEED;
    break;
  
    case 'D':
       sprintf(msg, "Speed Down\n");
      _motor.speed -= INC_SPEED;
    break;
	
    default:
      break;
  }

  if(VERBOSE_KEY)
    printf(msg);
}


/**
 * @brief      Compute the mean of the 3-axis accelerometer. The result is stored in array _meas.acc_mean
 */
void controller_compute_mean_acc()
{
  static int count = 0;
  
  count++;
  
  if( count > 20 ) // Remove the effects of strong acceleration at the begining
  {
    for(int i = 0; i < 3; i++)  
        _meas.acc_mean[i] = (_meas.acc_mean[i] * (count - 1) + _meas.acc[i]) / (double) count;
  }
  
  if( count == (int) (TIME_INIT_ACC / (double) _robot.time_step * 1000) )
    printf("Accelerometer initialization Done ! \n");

  if(VERBOSE_ACC_MEAN)
        printf("ROBOT acc mean : %g %g %g\n", _meas.acc_mean[0], _meas.acc_mean[1] , _meas.acc_mean[2]);
}


/**
 * @brief      Set the speed to the motors according to the user commands
 */
void controller_set_speed()
{

  // Check speed bounds
  _motor.speed = (_motor.speed > MAX_SPEED ? MAX_SPEED : _motor.speed);
  _motor.speed = (_motor.speed < 0 ? 0 : _motor.speed);

  double msl_w = 0;
  double msr_w = 0;

  if(_motor.isRunning)
  {
    // Convert speed for Webots
    msl_w = _motor.speed * MAX_SPEED_WEB / MAX_SPEED;
    msr_w = _motor.speed * MAX_SPEED_WEB / MAX_SPEED;

    // Set direction of the motors
    msl_w = (_motor.sign_left  ? msl_w : -msl_w);
    msr_w = (_motor.sign_right ? msr_w : -msr_w);
  }

  // Update the speed on Webots
  wb_motor_set_velocity(_robot.left_motor, msl_w);
  wb_motor_set_velocity(_robot.right_motor, msr_w);
}


/**
 * @brief      Log the usefull informations about the simulation
 *
 * @param[in]  time  The time
 */
void controller_print_log(double time)
{

  if( fp != NULL)
  {
    fprintf(fp, "%g; %g; %g; %g; %g; %g; %g; %g; %g; %g; %g; %g; %g; %g; %g; %g; %g; %g\n",
            time, _pose.x, _pose.y , _pose.heading, _meas.gps[0], _meas.gps[1], 
      _meas.gps[2], _meas.acc[0], _meas.acc[1], _meas.acc[2], _meas.right_enc, _meas.left_enc, 
      _odo_acc.x, _odo_acc.y, _odo_acc.heading, _odo_enc.x, _odo_enc.y, _odo_enc.heading);
  }

}

//-----------------------------------------------------------------------------------//
/*INIT*/

/**
 * @brief      Run the initialization. Set the variables and structure to 0. Try to initialize the Webots components. 
 *
 * @return     Return true if it rise and error
 */
bool controller_init()
{

  bool err = false;

  memset(&_robot, 0 , sizeof(simulation_t));

  memset(&_meas, 0 , sizeof(measurement_t));

  memset(&_pose, 0 , sizeof(pose_t));

  memset(&_odo_enc, 0 , sizeof(pose_t));

  memset(&_odo_acc, 0 , sizeof(pose_t));

  CATCH(err,controller_init_time_step());

  CATCH(err,controller_init_gps());

  CATCH(err,controller_init_acc());

  CATCH(err,controller_init_encoder());

  CATCH(err,controller_init_motors());

  CATCH(err,controller_init_log("data.csv"));
  
  wb_keyboard_enable(_robot.time_step);

  odo_reset(_robot.time_step);

  return err;
}


/**
 * @brief      Initialize the gps sensor from Webots
 *
 * @return     return true if it fails
 */
bool controller_init_gps() 
{
  _robot.gps = wb_robot_get_device("gps");

  bool err = CATCH_ERR(_robot.gps == 0, "No gps node found in the current robot file\n");

  if( !err )
  {
    // Enable the sensor on Webots.
    wb_gps_enable(_robot.gps, _robot.time_step); 
  }

  return err;
}


/**
 * @brief       Initialize the accelerometer sensor from Webots
 *
 * @return      return true if it fails
 */
bool controller_init_acc()
{
  // Get the acc sensor from webots. Note : the name of the accelerometer is "accelerometer"
  _robot.acc = wb_robot_get_device("accelerometer");

  bool err = CATCH_ERR(_robot.acc == 0, "No accelerometer node found in the current robot file\n");

  if( !err )
  {
    // Enable the sensor on Webots. you should use _robot.acc and _robot.time_step
    wb_accelerometer_enable(_robot.acc, _robot.time_step);
  }

  return err;

}


/**
 * @brief      Initiliaze the the wheel encoders (position sensors) from Webots
 *
 * @return      return true if it fails
 */
bool controller_init_encoder()
{
  // Get the device from Webots
  _robot.left_encoder = wb_robot_get_device("left wheel sensor");
  // Write an error message if the initialization fails
  bool err = CATCH_ERR(_robot.left_encoder == 0, "No left wheel sensor node found in the current robot file\n");

  _robot.right_encoder = wb_robot_get_device("right wheel sensor");

  CATCH(err,CATCH_ERR(_robot.right_encoder == 0, "No right wheel sensor node found in the current robot file\n"));
  
  if( !err ) // if no error initialize the sensors
  {
    wb_position_sensor_enable(_robot.left_encoder,  _robot.time_step);
    wb_position_sensor_enable(_robot.right_encoder, _robot.time_step);
  }

  return err;
}


/**
 * @brief      Initiliaze the the wheel motors from Webots
 *
 * @return     return true if it fails
 */
bool controller_init_motors()
{
  // Get the left motors device from webots. Note : the name of the gps is "left wheel motor"
  _robot.left_motor = wb_robot_get_device("left wheel motor");

  bool err = CATCH_ERR(_robot.left_motor == 0, "No left wheel motor node found in the current robot file\n");

  // Get the right motors device from webots. Note : the name of the gps is "right wheel motor"
  _robot.right_motor = wb_robot_get_device("right wheel motor");

  CATCH(err,CATCH_ERR(_robot.left_motor == 0, "No right wheel motor node found in the current robot file\n"));
  
  if( !err )
  {
    wb_motor_set_position(_robot.left_motor, INFINITY);   // Set the left motor position to INFINITY.
    wb_motor_set_position(_robot.right_motor, INFINITY);  // Set the right motor position to INFINITY. 
    wb_motor_set_velocity(_robot.left_motor, 0.0);        // Set the left motor speed to 0.               
    wb_motor_set_velocity(_robot.right_motor, 0.0);       // Set the right motor speed to 0.      
  }

  return err;
}


/**
 * @brief      Initialize the simulation time step on Webots
 *
 * @return     return true if it fails
 */
bool controller_init_time_step()
{
  _robot.time_step =  wb_robot_get_basic_time_step();

return CATCH_ERR(_robot.time_step == 0,"time step is not is not set\n");
}


/**
 * @brief      Initialize the logging of the file
 *
 * @param[in]  filename  The filename to write
 *
 * @return     return true if it fails
 */
bool controller_init_log(const char* filename)
{

  fp = fopen(filename,"w");
  
  bool err = CATCH_ERR(fp == NULL, "Fails to create a log file\n");

  if( !err )
  {
    fprintf(fp, "time; pose_x; pose_y; pose_heading;  gps_x; gps_y; gps_z; acc_0; acc_1; acc_2; right_enc; left_enc; odo_acc_x; odo_acc_y; odo_acc_heading; odo_enc_x; odo_enc_y; odo_enc_heading\n");
  }

  return err;
}


/**
 * @brief      Do an error test if the result is true write the message in the stderr.
 *
 * @param[in]  test     The error test to run
 * @param[in]  message  The error message
 *
 * @return     true if there is an error
 */
bool controller_error(bool test, const char * message, int line, const char * fileName)
{
  if (test) 
  {
    char buffer[256];

    sprintf(buffer, "file : %s, line : %d,  error : %s", fileName, line, message);

    fprintf(stderr,buffer);

    return(true);
  }

  return false;
}