
/*
 * $Id: $
 *
 * Description  : Data structure describing traffic info for the user driven car
 * Author       : 
 *
 * Purpose      :
 *     Long description of what the file is for.
 */


#ifndef __TRAFFIC_INFO_DATA__H__
#define __TRAFFIC_INFO_DATA__H__


	struct TrafficInfoData
	{
		// DEFINITION of structs and constants
		typedef struct RoadNode { // data size: 5*sizeof(float) = 20B
			Vector3 position;   ///< road node position
			float   laneWidth;  ///< lane width
			float   maxSpeed;   ///< lane speed limit
			float   vdop;       ///< max speed taking into account curve radius
			float   vprof;      ///< profile speed (for the given profile)
		} RoadNode;

		// describes a car in lane (if there is any) 
		typedef struct LaneOccupancyInfo { // data size: 10*sizeof(float) + 2*sizeof(int) = 48B
			int     controllerID;    ///< vehicle ID
			int     nodeAheadIndex;  ///< points to the road node ahead of vehicle - points to nodes in surroundingNodes array
			Vector3 position;        ///< vehicle position
			Vector3 velocity;        ///< vehicle velocity
			float   relativeSpeed;   ///< relative speed of the collider wrt the driven vehicle [m/s]
			float   distance;        ///< distance to the collider (vehicle) in the current lane (along road)
			float   timeDistance;    ///< time distance to the vehicle [s] 
			float   timeToLimit;     ///< time distance at which ACC starts to limit the vehicle velocity
		} LaneOccupancyInfo;

		static const int maxNodeCount = 8;
		static const int maxVehicleCount = 10;

		// THE DATA of the message start here!
		// describe the neighbourhood of the driven vehicle (8 front and 8 back nodes in each lane)
		// indices <0 - maxNodeCount) - current lane back nodes
		// indices <maxNodeCount, 2*maxNodeCount) - current lane front nodes
		// indices <2*maxNodeCount, 3*maxNodeCount) - adjacent lane back nodes
		// indices <3*maxNodeCount, 4*maxNodeCount) - adjacent lane front nodes
		RoadNode surroundingNodes[4*maxNodeCount]; // data size: 4*8*sizeof(RoadNode) = 32*20B = 640B

		int vehicleCount;                                       // 4B
		// occupancy of current and adjacent lanes by vehicles
		LaneOccupancyInfo surroundingVehicles[maxVehicleCount]; // 10*sizeof(LaneOccupancyInfo) = 10*48B = 480B

		// indices of four closest vehicles in both lanes in front and back direction -> indices pointing to surroundingVehicles array
		// -1 if no vehicle is present
		int currentLaneFrontCarIndex;  // current lane - closest vehicle in front of     -> 4B
		int currentLaneBackCarIndex;   // current lane - closest vehicle at the back of  -> 4B
		int adjacentLaneFrontCarIndex; // adjacent lane - closest vehicle in front of    -> 4B
		int adjacentLaneBackCarIndex;  // adjacent lane - closest vehicle at the back of -> 4B

		Vector3 targetPoint;    ///< Interpolated navigation point for the driven vehicle -> 3*sizeof(float) = 12B
		Vector3 steeringPoint;  ///< Interpolated steering point for the driven vehicle -> 3*sizeof(float) = 12B

		int currentLane;        ///< current lane index - counted from right to left starting at 0 i.e. 0=right lane, 1=left lane -> 4B
		int controllerID;       ///< vehicle controller ID -> 4B
		Vector3 position;       ///< vehicle current position -> 3*sizeof(float) = 12B
		Vector3 orientation;    ///< vehicle current orientation -> 3*sizeof(float) = 12B
		float speed;            ///< vehicle current speed [m/s] -> sizeof(float) = 4B

		TrafficInfoData() : targetPoint(0.0f, 0.0f, 0.0f), steeringPoint(0.0f, 0.0f, 0.0f)
		{
			for(int i=0; i< 4*maxNodeCount; i++) {
				surroundingNodes[i].position = Vector3(0.0f, 0.0f, 0.0f);
				surroundingNodes[i].laneWidth = 0.0f;
				surroundingNodes[i].maxSpeed = 0.0f;
				surroundingNodes[i].vdop = 0.0f;
				surroundingNodes[i].vprof = 0.0f;
			}

			vehicleCount = 0;

			for(int i=0; i<maxVehicleCount; i++) {
				surroundingVehicles[i].controllerID = -1;
				surroundingVehicles[i].nodeAheadIndex = -1;
				surroundingVehicles[i].position = Vector3(0.0f, 0.0f, 0.0f);
				surroundingVehicles[i].velocity = Vector3(0.0f, 0.0f, 0.0f);
				surroundingVehicles[i].relativeSpeed = 0.0f;
				surroundingVehicles[i].distance = 0.0f;
				surroundingVehicles[i].timeDistance = 0.0f;
				surroundingVehicles[i].timeToLimit = 0.0f;
			}

			currentLaneFrontCarIndex = -1;
			currentLaneBackCarIndex = -1;
			adjacentLaneFrontCarIndex = -1;
			adjacentLaneBackCarIndex = -1;

			currentLane = 0;
			controllerID = -1;

			position = Vector3(0.0f, 0.0f, 0.0f);
			orientation = Vector3(1.0f, 0.0f, 0.0f);
			speed = 0.0f;
		}

		TrafficInfoData(const TrafficInfoData &other)
		{
			*this = other;
#if 0
			targetPoint = other.targetPoint;
			steeringPoint = other.steeringPoint;
			for(int i=0; i<4*maxNodeCount; i++) {
				surroundingNodes[i].position = other.surroundingNodes[i].position;
				surroundingNodes[i].laneWidth = other.surroundingNodes[i].laneWidth;
				surroundingNodes[i].maxSpeed = other.surroundingNodes[i].maxSpeed;
			}

			vehicleCount = other.vehicleCount;

			for(int i=0; i<maxVehicleCount; i++) {
				surroundingVehicles[i].controllerID = other.surroundingVehicles[i].controllerID;
				surroundingVehicles[i].nodeAheadIndex = other.surroundingVehicles[i].nodeAheadIndex;
				surroundingVehicles[i].position = other.surroundingVehicles[i].position;
				surroundingVehicles[i].velocity = other.surroundingVehicles[i].velocity;
				surroundingVehicles[i].relativeSpeed = other.surroundingVehicles[i].relativeSpeed;
				surroundingVehicles[i].distance = other.surroundingVehicles[i].distance;
				surroundingVehicles[i].timeDistance = other.surroundingVehicles[i].timeDistance;
				surroundingVehicles[i].timeToLimit = other.surroundingVehicles[i].timeToLimit;
			}

			currentLaneFrontCarIndex = other.currentLaneFrontCarIndex;
			currentLaneBackCarIndex = other.currentLaneBackCarIndex;
			adjacentLaneFrontCarIndex = other.adjacentLaneFrontCarIndex;
			adjacentLaneBackCarIndex = other.adjacentLaneBackCarIndex;

			currentLane = other.currentLane;
			controllerID = other.controllerID;
			position = other.position;
			orientation = other.orientation;
			speed = other.speed;
#endif
		}

	};
#endif // __TRAFFIC_INFO_DATA__H__
