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

#pragma once

#include "glmSDKAPI.h"
#include <glmSimulation.h>
#include <glmSimuEntityHandle.h>
#include <glmSet.h>
#include <glmMap.h>
#include <glmList.h>
#include <glmVector2.h>
#include <glmVector3.h>
#include <glmMutex.h>
#include <glmTrafficUtils.h>
#include <glmTrafficDetectionData.h>
#include <glmTrafficPerceptionData.h>
#include <glmTrafficCrossingTrajectory.h>
#include <glmTrafficVehicle.h>

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

	// some inspiration sources here :http://www.cs.utexas.edu/~aim/ for automatic intersection management

	//-------------------------------------------------------------------------
	// utils for static car collision test
	bool onSegment(const Vector2& p, const Vector2& q, const Vector2& r);
	int orientation(const Vector2& p, const Vector2& q, const Vector2& r);
	bool doIntersect(const Vector2& p1, const Vector2& q1, const Vector2& p2, const Vector2& q2);
	bool carCollidingStaticTest(const Vector2& thisPosition, const Vector2& otherPosition,
		const Vector2& thisDirection, const Vector2& otherDirection,
		Vehicle& thisComp, Vehicle& otherComp);

	//-------------------------------------------------------------------------
	// A Timed Trajectory on crossing (to check for collisions)
	class CrossingRequest
	{
	public:

		class RequestStatus
		{
		public:
			enum Value
			{
				AUTHORIZED = 0,
				REJECTED,
				RECHECK,
				UNDEFINED
			};
		};

		CrossingRequest();
		~CrossingRequest();

		Entity& getEntity() const;
		TrafficEntityComponent& getComp() const;

		// dt is meant to be FIXED or this won't work
		TrafficContext* _trafficContext;
		SimuEntityHandle _entityHandle;
		int _waitTime;

#ifndef NDEBUG
		int64_t	_entityId;
#endif

		Vector3 _rejectedCausePosition;

		GlmList<TrafficElement>::iterator _trafficElement; // crossing traffic element matching this 

		RequestStatus::Value _requestStatus;

		uint32_t _dtsBeforeCrossing;
		glm::PODArray<float> _abscissaFromStartPerDT; // store all abscissas, deduced from speed reaching crossing speed, from startFrame. 
		bool _fromStop;
	};

	//-------------------------------------------------------------------------
	// The main Crossing class
	class GLM_SDK_API Crossing
	{
		friend class TrafficWorld;

	public:	//methods

		class CrossingType
        {
        public:
            enum Value
            {
                TrafficLights,
                Stop,
                Junction,
                END
            };
        };

		Crossing();

		void updateLights(TrafficWorld& trafficWorld, Time currentTime);
		TrafficLightStatus::Value getRoadLightStatus(TrafficWorld& trafficWorld, int roadIndexInCrossingConnectedRoads) const;
		//void resetCarAuthorization(SimuEntityHandle entityHandle);
		void addRequest(CrossingRequest*);		
		void deleteRequestThreadSafe(CrossingRequest* crossingRequest/*TrafficContext* context, SimuEntityHandle handle*/);
		void deleteRequestNOTThreadSafe(CrossingRequest* crossingRequest/*TrafficContext* context, SimuEntityHandle handle*/);
		bool checkRequest(const TrafficWorld& trafficWorld, CrossingRequest* candidateRequest, int compareUpToRecheckRequestIndex = -1);
		void selectAuthorizedCrossingTrajectories(const TrafficWorld& trafficWorld);
		void invalidateRequestsFrom(CrossingRequest* fromRequest); // when an entity stops on the crossing, need to recompute authorized requests

	private:	//methods
		void setInternalRoadsLightStatus(TrafficWorld& trafficWorld, PODArray<TrafficLightStatus::Value> roadLightStatus);
		void makeRoadPairs(const TrafficWorld& trafficWorld);
		//void purgeDeletedRequests();
		void computeSpaceLeft(const TrafficWorld& trafficWorld);
		void invalidateRequests();

#ifndef NDEBUG		
		void pushRequestTrajectoryDebug(CrossingTrajectoriesDebug& trajectoriesDebug, CrossingRequest& request, Vehicle& vehicleProps/*, float startReferenceAbscissa, int dtCountLimit*/);
#endif
		bool isRequest1Prioritary(CrossingRequest*& request1, CrossingRequest*& request2) const;
		bool swapIfFirstFurtherFromEnd(CrossingRequest*& request1, CrossingRequest*& request2);
		void sortRequests(PODArray<CrossingRequest*>& requests);

	public:	//data
		// definition
		Vector3 _position;
		float _radius;
		glm::PODArray<int>	_connectedRoads;
		glm::PODArray<bool>	_asRoadStart; // is it the start or end of this connected road. a road can be registered twice in different orders
		bool _uTurnForbidden;
		bool _invertPriority;
		float _keepStraightRatio;

		//light parameters
		//bool _isAJunction;
        CrossingType::Value _crossingType;
		bool _automaticLights;
		float _greenLightTime;
		float _orangeLightTime;
		float _redLightTime;

		float _stopMinTime;
        float _stopMaxTime;

		// car trajectories are computed by the entityComponent, and are compared in single thread : each crossing makes its computation to know which car is OK to go
		GlmList<CrossingRequest*> _acceptedRequests; // already accepted TrafficElements
		PODArray<CrossingRequest*> _recheckRequests; // authorized trajectories which are behind a braking car
		PODArray<CrossingRequest*> _newRequests; // -> let priority to speedier car, car that waited the longest time, car ID ? once the car is dropped to _acceptedRequests, it is authorized to go on crossing
		PODArray<CrossingRequest*> _invalidateRequestsFrom; // -> let priority to speedier car, car that waited the longest time, car ID ? once the car is dropped to _acceptedRequests, it is authorized to go on crossing
		int _waitDtsForRecheck;
		
		// runtime to avoid realloc
		glm::PODArray<int> _trafficRoadIndexToConnectedRoadIndex;

		SpinLock _requestMutex;
		SpinLock _invalidateMutex;

		// avoid reallocation at runtime
		PODArray< float > _spaceLeftPerOutRoadLanes;
		PODArray< bool > _hasCarInLane;
		PODArray< int > _lanesCountPerOutRoad;
		PODArray< int > _lanesFirstIndexPerOutRoad;

		GlmString _name; // debug helper

	protected: //structures
		struct RoadPair
		{
			int _first; // index in _connectedRoads array
			int _second; // index in _connectedRoads array
		};

	protected:	//data
		glm::PODArray<RoadPair> _roadPairs;
	};
}
