#ifndef GENERALDEFINITIONS
#define GENERALDEFINITIONS

#define _CRT_SECURE_NO_WARNINGS

//#define WIFIBOT_ROBOT
#define CAR_LIKE_ROBOT

namespace pacpus {

#define MS_DELAY 10
#define PI_VALUE 3.1415926535897932
//#define DELTA_T 0.1					// Time in seconds estimated for each interaction

#ifdef WIFIBOT_ROBOT
//---------------- Wifibot DSPIC constraints ----------------------
#define SIZE_MSG_RECEIVE 21
#define SIZE_MSG_SEND 9
#define SIZE_MSG_PID_SEND 11

#define W_SPEED_MAX     360

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

//------------------- WIFIBOT constraints -------------------------
#define W_WHEEL_R 0.07				// Wheel radius
#define W_DIST_L 0.19				// Distance from the robot frame to the wheel

// Robot dimentions
#define W_LEFT -0.20				// negative
#define W_RIGHT 0.20				// positive
#define W_FRONT 0.20				// positive
#define W_BACK -0.20				// negative
#define W_ENLARGEMENT 0.2

// REAL LIMIT
//#define W_V1_MAX 1.355										// Desired max robot linear velocity (m/s)
//#define W_V1_MIN 0.0											// Min robot linear Velocity (m/s)
//#define W_V1_ACC 0.6											// Max acceleration and desacceleration (m/s^2)
//#define W_V2_MAX (W_V1_MAX/W_DIST_L)							// Max robot angular velocity (rad/s)
//#define W_V2_ACC (W_V1_ACC/W_DIST_L)							// Max acceleration and desacceleration (rad/s^2)
//#define W_DELTA_V1 0.008										// Smallest linear velocity variation (m/s)
//#define W_DELTA_V2 (W_DELTA_V1/(2.0*W_DIST_L))				// Smallest angle velocity variation (rad)
//#define W_WHEEL_ANGULAR_VELOCITY_MAX (W_V1_MAX/W_WHEEL_R)		// Max robot wheel angular velocity (rad/s)

// Smooth actuator limit
#define W_V1_MAX 0.6										// Desired max robot linear velocity (m/s)
#define W_V1_MIN 0.0										// Min robot linear Velocity (m/s)
#define W_V1_ACC 0.4										// Max acceleration and desacceleration (m/s^2)
#define W_V2_MAX (PI_VALUE/4.0)								// Max robot angular velocity (rad/s)
#define W_V2_ACC (PI_VALUE/4.0)								// Max acceleration and desacceleration (rad/s^2)
#define W_DELTA_V1 0.05										// Smallest linear velocity variation (m/s)
#define W_DELTA_V2 0.018									// Smallest angle velocity variation (rad)
#define W_WHEEL_ANGULAR_VELOCITY_MAX (W_V1_MAX/W_WHEEL_R)	// Max robot wheel angular velocity (rad/s)

// Input convertion
#define LINEAR_VELOCITY 125.0 // 125 (int) == 1 (m/s) 
#define WHEEL_ANGULAR_VELOCITY 8.75 // (int) == 1 (rad/s) == 0.07 m/s
#define V1_MAX_INT 127.0 // Maximum robot speed

//-----------------------------------------------------------------
#endif

#ifdef CAR_LIKE_ROBOT

//#define CAN_CARMEN
//#define CAN_FLUENCEZE
#define CAN_ZOE
	
//------------------- Car constraints -------------------------
#ifdef CAN_CARMEN

#define CAR_WHEEL_R 0.5				// Wheel radius
#define CAR_DIST_L 2.75				// Distance from the robot frame to the wheel

// Robot dimentions
#define CAR_LEFT 1.05				// negative
#define CAR_RIGHT -1.05				// positive
#define CAR_FRONT 3.795				// positive
#define CAR_BACK -1.045				// negative
#define CAR_ENLARGEMENT 0.5

// REAL LIMIT
//#define CAR_V1_MAX 1.355									// Desired max car linear velocity (m/s)
//#define CAR_V1_MIN 0.0									// Min car linear Velocity (m/s)
//#define CAR_V1_ACC 0.6									// Max acceleration and desacceleration (m/s^2)
//#define CAR_PHI_MAX 0.5061								// Max steering angle (rad) (29.0*PI_VALUE/180.0)
//#define CAR_V2_MAX 0.2									// Max car steering angle velocity (rad/s)
//#define CAR_DELTA_V1 0.008								// Smallest linear velocity variation (m/s)
//#define CAR_DELTA_PHI 0.002								// Smallest steering angle variation (rad)

// Smooth actuator limit
#define CAR_V1_MAX 1.355								// Desired max car linear velocity (m/s)
#define CAR_V1_MIN 0.0									// Min car linear Velocity (m/s)
#define CAR_V1_ACC 0.6									// Max acceleration and desacceleration (m/s^2)
#define CAR_PHI_MAX 0.5061								// Max steering angle (rad) (29.0*PI_VALUE/180.0)
#define CAR_PHI_WHEEL_MAX 9,5993						// Max steering wheel angle (rad) (550.0*PI_VALUE/180.0)
#define CAR_V2_MAX 0.2									// Max car steering angle velocity (rad/s)
#define CAR_DELTA_V1 0.008								// Smallest linear velocity variation (m/s)
#define CAR_DELTA_PHI 0.002								// Smallest steering angle variation (rad)

#else ////////////////////////////////////////////////////////////////////////////////////////////
#ifdef CAN_FLUENCEZE

#define CAR_WHEEL_R 0.5				// Wheel radius
#define CAR_DIST_L 2.75				// Distance from the robot frame to the wheel

// Robot dimentions
#define CAR_LEFT 1.05				// negative
#define CAR_RIGHT -1.05				// positive
#define CAR_FRONT 3.795				// positive
#define CAR_BACK -1.045				// negative
#define CAR_ENLARGEMENT 0.5

// REAL LIMIT
//#define CAR_V1_MAX 1.355									// Desired max car linear velocity (m/s)
//#define CAR_V1_MIN 0.0									// Min car linear Velocity (m/s)
//#define CAR_V1_ACC 0.6									// Max acceleration and desacceleration (m/s^2)
//#define CAR_PHI_MAX 0.5061								// Max steering angle (rad) (29.0*PI_VALUE/180.0)
//#define CAR_V2_MAX 0.2									// Max car steering angle velocity (rad/s)
//#define CAR_DELTA_V1 0.008								// Smallest linear velocity variation (m/s)
//#define CAR_DELTA_PHI 0.002								// Smallest steering angle variation (rad)

// Smooth actuator limit
#define CAR_V1_MAX 1.355								// Desired max car linear velocity (m/s)
#define CAR_V1_MIN 0.0									// Min car linear Velocity (m/s)
#define CAR_V1_ACC 0.6									// Max acceleration and desacceleration (m/s^2)
#define CAR_PHI_MAX 0.5061								// Max steering angle (rad) (29.0*PI_VALUE/180.0)
#define CAR_PHI_WHEEL_MAX 9,5993						// Max steering wheel angle (rad) (550.0*PI_VALUE/180.0)
#define CAR_V2_MAX 0.2									// Max car steering angle velocity (rad/s)
#define CAR_DELTA_V1 0.008								// Smallest linear velocity variation (m/s)
#define CAR_DELTA_PHI 0.002								// Smallest steering angle variation (rad)

#else /////////////////////////////////////////////////////////////////////////////////////////////////
#ifdef CAN_ZOE

#define CAR_WHEEL_R 0.31045			// Wheel radius
#define CAR_DIST_L 2.588				// Distance from the robot frame to the wheel

// Robot dimentions
#define CAR_LEFT 0.9725				// negative
#define CAR_RIGHT -0.9725			// positive
#define CAR_FRONT 3.427				// positive
#define CAR_BACK -0.657				// negative
#define CAR_ENLARGEMENT 0.9
#define CAR_ENLARGEMENT2 0.3

// REAL LIMIT
//#define CAR_V1_MAX 4.0									// Desired max car linear velocity (m/s)
//#define CAR_V1_MIN 0.0									// Min car linear Velocity (m/s)
//#define CAR_V1_ACC 0.6									// Max acceleration and desacceleration (m/s^2)
//#define CAR_PHI_MAX 0.5061								// Max steering angle (rad) (29.0*PI_VALUE/180.0)
//#define CAR_PHI_WHEEL_MAX 8.6394							// Max steering wheel angle (rad) (495.0*PI_VALUE/180.0)
//#define CAR_V2_MAX 0.5726									// Max car steering angle velocity (rad/s) (Steering wheel = 560 °/s ====> Steering = 32.8 °/s )
//#define CAR_V2_MIN 0.0041									// Min car steering angle velocity (rad/s) (Steering wheel = 4 °/s ====> Steering = 0.234 °/s )
//#define CAR_DELTA_V1 0.008								// Smallest linear velocity variation (m/s)
//#define CAR_DELTA_PHI 0.002								// Smallest steering angle variation (rad)

// Smooth actuator limit
#define CAR_V1_MAX 4.0									// Desired max car linear velocity (m/s)
#define CAR_V1_MIN 0.0									// Min car linear Velocity (m/s)
#define CAR_V1_ACC 0.6									// Max acceleration (m/s^2)
#define CAR_V1_DEC -2.4									// Max deceleration (m/s^2)
#define CAR_PHI_MAX 0.4090								// Max steering angle (rad) (23.43*PI_VALUE/180.0)
#define CAR_PHI_WHEEL_MAX 6.9813						// Max steering wheel angle (rad) (400.0*PI_VALUE/180.0) >>> THIS IS THE MAXIMUM FOR DON'T CRASH THE SYSTEM
#define CAR_V2_MAX 0.2556								// Max car steering angle velocity (rad/s) (Steering wheel = 250 °/s ====> Steering = 14.65 °/s )
#define CAR_V2_MIN 0.0041								// Min car steering angle velocity (rad/s) (Steering wheel = 4 °/s ====> Steering = 0.234 °/s )
#define CAR_DELTA_V1 0.14								// Smallest linear velocity variation (m/s)
#define CAR_DELTA_PHI 0.01								// Smallest steering angle variation (rad)

#endif // CAN_ZOE
#endif // CAN_FLUENCE_ZE
#endif // CAN_CARMEN
//-----------------------------------------------------------------

#endif // CAR_LIKE_ROBOT

//--------------- Camera Grid constraints Wifibot -----------------	
#define D_MAX_CAM_GRID_DEFAULT 10.0			// Max distance in the grid
#define CAM_GRID_COLS 361					// Grid image columns
#define CAM_GRID_ROWS 600					// Grid image rows
//#define CAM_GRID_DIST 60.0					// Max distance in the grid cell (pixels/meter)
#define ROBOT_FRONT_DIST 2.00				// Distance from the robot front to the camera (meters) - Just for symbolic representation
#define ROBOT_FRONT_WIDTH 2.10				// Width of the robot front (meters) - Just for symbolic representation

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

//-------------- Obstacle Detection constraints -------------------
#define ANG_VARIATION 20.0 // Planes are equal to lines with 90° + ANG_VARIATION
#define ANG_VARIATION2 7.0 // Obstacles are equal to lines between 90° +- ANG_VARIATION2
//#define MIN_V_DISPARITY 16
//#define MAX_V_DISPARITY 255

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

//---------------- Path Following Constraints----------------------
#define CONTROL_DELTA_T 0.1			// Estimated time between the control loop
#define DWA_DELTA_T_V1	0.3			// DWA time interval for the linear velocity
#define DWA_DELTA_T_PHI	0.7			// DWA time interval for the angular velocity
#define MAX_LINE_FEATURES_OUT 5		// Maximum image lane features interactions missing
//-----------------------------------------------------------------

//============= Extended Kalman Filter parameters =================
//---------------------- GPS ERROR --------------------------------
#define GPS_DELTA_X			3.0		// Standard deviation of the estimated error of x
#define GPS_DELTA_Y			3.0		// Standard deviation of the estimated error of y
#define GPS_RHO				0.0		// Spartial correlation coefficient
//-----------------------------------------------------------------

//---------------------- CAN ERROR --------------------------------
#define CAN_DELTA_V			0.2778					// Standard deviation of the estimated error of the speed
#define CAN_DELTA_PHI		(0.5*PI_VALUE/180.0)	// Standard deviation of the estimated error of steering rad
//-----------------------------------------------------------------

//------------------- VISUAL ODOMETRY ERROR -----------------------
#define VO_DELTA_D			0.2						// Standard deviation of the estimated error of the translation distance variation
#define VO_DELTA_THETA		(1.0*PI_VALUE/180.0)	// Standard deviation of the estimated error of the rotation variation
//-----------------------------------------------------------------

//------------------------ Model error ----------------------------
#define MODEL_ERROR_X		0.04
#define MODEL_ERROR_Y		0.04
#define MODEL_ERROR_THETA	0.005
#define MODEL_ERROR_PHI		0.0
#define MODEL_ERROR_V1		0.0
//-----------------------------------------------------------------
//=================================================================

//------------------- Robot State definitions ---------------------
#define RS_RATE				10.0				// Frequency Hz
#define RS_RATE_EKF			40.0				// Frequency Hz
//-----------------------------------------------------------------

};

#endif