#ifndef _SENSORSAPPLICATIONSTRUCTURES_H_
#define _SENSORSAPPLICATIONSTRUCTURES_H_

#include "Pacpus/kernel/cstdint.h"
#include "Pacpus/kernel/road_time.h"

#include "../StdDbtPlayerComponents/ImageBaseStructure.h"

// some constants defined for the Alasca component
static const uint32_t MAX_SENSOR_POINTS = 500;
static const uint32_t MAX_GRID_POINTS = 360;
static const uint32_t MAX_SENSOR_GRID_POINTS = (uint32_t)(500*500);

//----------------- Sensor Occupancy Grid Properties -------------
typedef struct  
{
	//------------------------- Default parameters --------------------------------------
	std::string sensors_name;	// sensor input name
	int type;					// sensor reading type: 0 = laser; 1 = stereo camera like laser; 2 = generic sensor
	double x;					// sensor pose related to the robot frame
	double y;					// sensor pose related to the robot frame
	double theta;				// sensor pose related to the robot frame

	//------------------------ Specifique parameters ------------------------------------
    int num_readings;			// number of readings
	double sigma_zt;			// standard deviantion in distance. For the camera = s_div*2.0 (2 meters)
	double sigma_thetat;		// standard deviation in angle. For the camera = CV_PI*(camera_FOV/num_readings)/180.0
	double max_range;			// maximum range of the sensor
	int mMaxSensorInputSize;	// size of the input data of sensor
	bool use_udp;				// if the data is from the udp connection

	//--------------------------- UDP parameters ----------------------------------------
	std::string host_address;	// IP address to receive the data
	unsigned int port;			// communication port
} s_properties; // Sensor properties

//----------------- Camera Obstacle Data -------------------------
// Camera obstacle data as laser readings
typedef struct  
{
    int num_readings;					// number of readings
	double radius[MAX_SENSOR_POINTS];	// distance measurements
	double angle[MAX_SENSOR_POINTS];	// angle of each measurement
} camera_occ_data; // Sensor properties

typedef struct
{
    road_time_t time;
    road_timerange_t timerange;
    camera_occ_data data;
} TimestampedCameraOccData;
//----------------------------------------------------------------

//-------------------- Sensor Grid Data --------------------------
// Generic sensor occupancy grid
typedef struct  
{
    int rows;								// number of rows
	int cols;								// number of cols
	float sensor_x0;						// sensor x origin in the occ data (x orientated to the right)
	float sensor_y0;						// sensor y origin in the occ data
	float sensor_x;							// sensor x position in the robot frame
	float sensor_y;							// sensor y position in the robot frame
	float sensor_theta;						// sensor theta orientation in the robot frame
	float ratio;							// sensor grid ratio (m/px)
	float occ_data[MAX_SENSOR_GRID_POINTS];	// sensor occupancy grid data with size rows*cols
} sensor_occ_data; // Sensor properties

typedef struct
{
    road_time_t time;
    road_timerange_t timerange;
    sensor_occ_data data;
} TimestampedSensorOccData;
//----------------------------------------------------------------

//------------------ Occupancy Grid Data -------------------------
typedef struct  
{
    int num_readings;				// number of readings
	double radius[MAX_GRID_POINTS];	// distance measurements
	double angle[MAX_GRID_POINTS];	// angle of each measurement
} occ_grid_data; // Sensor properties

typedef struct
{
    road_time_t time;
    road_timerange_t timerange;
    occ_grid_data data;
} TimestampedOccGridData;
//----------------------------------------------------------------

//--------------------- Robot State Data -------------------------
typedef struct  
{
	double x;			// Delta X related to the last frame
	double y;			// Delta Y related to the last frame
	double theta;		// Delta theta (orientation angle) related to the last frame
	double phi;			// Delta phi (steering angle) related to the last frame
} robot_pose_data; // pose properties

typedef struct  
{
	double v1;			// Linear velocity
	double v2;			// Steering angle velocity
} robot_velocity_data; // velocities properties

typedef struct
{
    road_time_t time;
    road_timerange_t timerange;
    robot_pose_data pose_data;
	robot_velocity_data velocity_data;
} TimestampedRobotStateData;
//----------------------------------------------------------------

//------------------ Line Features Data -------------------------
typedef struct  
{
	int center_point_x, 
		center_point_y;				// Reference line point
	double center_angle;			// Reference line angle
	// For road surface
	int left_point_x,
		 left_point_y;				// Left road boundary
	double left_angle;				// Left road boundary angle
	int right_point_x,
		right_point_y;				// Left road boundary
	double right_angle;				// Left road boundary angle
} line_features_data; // line features

typedef struct
{
    road_time_t time;
    road_timerange_t timerange;
    line_features_data data;
} TimestampedLineFeaturesData;
//----------------------------------------------------------------

//------------------ Robot Control Inputs -------------------------
typedef struct  
{
	double v1;				// Linear velocity in km/h
	double v2;				// Angular velocity for the wifibot and Steering 
							// angle velocity for the car-like robot in degrees/s
	double phi;				// Final steering angle setpoint in degrees
	double acc;				// Desired acceleration/deceleration m/s^2
} control_data; // Control Data

typedef struct
{
    road_time_t time;
    road_timerange_t timerange;
    control_data data;
} TimestampedControlData;
//----------------------------------------------------------------

//------------------ CAN DATA -------------------------
typedef struct  
{
	double frontLeftWheelSpeed;      ///< speed of the front left wheel (in m/s)
	double frontRightWheelSpeed;     ///< speed of the front right wheel (in m/s)
	double rearLeftWheelSpeed;       ///< speed of the rear left wheel (in m/s)
	double rearRightWheelSpeed;      ///< speed of the rear right wheel (in m/s)
	double averageFrontWheelSpeed;   ///< average front wheels speed (in m/s)
} CANSpeed_data; // Speed data

typedef struct
{
    road_time_t time;
    road_timerange_t timerange;
    CANSpeed_data data;
} TimestampedCANSpeedData;

typedef struct  
{
	double angle;                ///< in rad positive in trigonometric direction (to the left)
    double rotationSpeed;        ///< in rad/s
} CANSteering_data; // Steering data converted to the front wheel frame

typedef struct
{
    road_time_t time;
    road_timerange_t timerange;
    CANSteering_data data;
} TimestampedCANSteeringData;
//----------------------------------------------------------------

//------------------ Visual Odometry Data -------------------------
typedef struct  
{
	double delta_d;				// Camera translaction from the last interaction
	double delta_theta;			// Camera angle relate to the last camera frame
	double delta_t;				// Time interval between calculations
} viso_data; // Visual odometry Data

typedef struct
{
    road_time_t time;
    road_timerange_t timerange;
    viso_data data;
} TimestampedVisoData;
//----------------------------------------------------------------

#endif