/***************************************************************************
*                                                                          *
*  Copyright (C) Golaem S.A.  All Rights Reserved.                         *
*                                                                          *
***************************************************************************/

#pragma once

#include "glmSDKAPI.h"
#include "glmArray.h"
#include "glmList.h"
#include "glmVector3.h"
#include "glmMemory.h"
#include "glmTrafficVehicle.h"
//#include "glmTrafficCrossingTrajectory.h"
//#include "glmTrafficDebugRenderArray.h"

namespace glm
{
	class TrafficContext;
	class Entity;
	class PositionInRoadData;
	class TrafficEntityComponent;
	class TrafficElement;
    class TrafficWorld;

	// trajectory on a crossing, to display time-colored collision surface
	struct GLM_SDK_API CrossingTrajectoriesDebug
	{
		inline void clear()
		{
			_trajectories.clear();
			_elements.clear();
		}

		struct StartStopIndex
		{
			size_t _startIndex; // inclusive start index in elements
			size_t _stopIndex; // inclusive stop index in elements
		};
		PODArray<StartStopIndex> _trajectories;

		struct CrossingTrajectoryDebugElement
		{
			Vector3 _leftSide;
			Vector3 _rightSide;
			uint32_t _color;
		};
		Array< CrossingTrajectoryDebugElement> _elements;
	};

	class TrafficDetectionData
	{
	public:

		GLM_CLASS_STAT_ID( glm::MemoryStatIds::Simulation);

		TrafficDetectionData();

		void init(TrafficContext& trafficContext, Entity& entity,/*const Vector3& recomputedPosition,*/
			float distance, float dtf, float decelerationSpeed, float targetSpeed);

		void detectObstacles(TrafficWorld& trafficWorld, Entity& entity, Time dt);

		float getMaxAngleOnSafeTrajectory(int firstVertex);

	private:

		void detectArrival(PositionInRoadData& positionInRoad);
        void detectNextCarOnLane(const GlmList<TrafficElement>::iterator& trafficRoadElement, bool checkForLaneChange, Time dt);
        void detectClosestCarOnCrossing(GlmList<TrafficElement>::iterator& crossingChunk);
        void detectCrossingAndRedLightAtEndOfLane(const GlmList<TrafficElement>::iterator& trafficRoadElement, Time dt);

		void fillSafetyDistanceLaneTrajectory(PositionInRoadData& laneData, float fromAbscissa, float toAbscissa);
		void fillSafetyDistanceCrossingTrajectory(GlmList<TrafficElement>::iterator& trafficElement, float distance);

		float getArrivalSpeedFromLength(float speed, float remainingLength);

		//bool isLaneChangePossible(PositionInRoadData& positionInRoad, float bestAbscissaDiffCurrentLaneAfterSwitch, int& iNewLane);
		bool selectBestPossibleSwitchLane(PositionInRoadData& positionInRoad, float minimalSafeDistance, float forecastDistanceToImprove, int& iNewLane);
		void getBackAndFrontCar(TrafficWorld& trafficWorld, PositionInRoadData& positionInRoad, int laneIndex, float& backCarDistance, TrafficEntityComponent*& backCarComp, float& frontCarDistance, TrafficEntityComponent*& frontCarComp) const;

		static float getStoppingDistance(float speed, float deceleration);
	public:
		Vector3 _obstaclePosition;
        Vector3 _steerTarget = Vector3::getNullVector();
		glm::Array<Vector3> _safeDistanceTrajectory;
		TrafficContext* _trafficContext;
		Entity* _entity;
		//Vector3 _recomputedPosition;
		float _originalDistance; // original distance checked for safety, based on current position of trailer, without front / back length
		float _detectionRemainingDistance; // remaining distance to travel, based on current position of trailer. It does NOT include front / back length as it depends on the context
		float _dtf;
		float _decelerationSpeed;
		float _targetSpeed;
		bool _mustBrake;
		bool _mustValidateLaneChange; // check to give priority between lanes change
		bool _hasObstacle;
		Time _stopWaitTime = 0.f;

		TrafficEntityComponent* _trafficComp;

#ifndef NDEBUG
		CrossingTrajectoriesDebug _crossingRequestCollisionBoxes;
#endif
	};

}
