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

#pragma once

#include "glmSDKAPI.h"
#include "glmMotionDefinitions.h"

#include "glmMutex.h"
#include "glmArray.h"
#include "glmVector3.h"
#include "glmQuaternion.h"

namespace glm
{
	class SynchronizedBlendBN;

	/**
	* \brief Class handling locomotion database trajectory sampling
	*/
	class GLM_SDK_API BlendBNTrajectorySampler
	{

	public:		// structure and class
		struct LocomotionInfo
		{
			float _minLinearVelocity;
			float _maxLinearVelocity;
			float _minAngularVelocity;
			float _maxAngularVelocity;
			SynchronizedBlendBN* _syncBlendNode;
		};

		struct TrajectoryStatus
		{
			enum Value
			{
				OK,
				DISMISSED_OBSTACLE_COLLISION,
				DISMISSED_ENTITY_COLLISION,
				END
			};
		};

		struct TrajectoryInfo
		{
			TrajectoryInfo(size_t sampleIndex=0, /*float linearVelocity=-1, float angularVelocity=-1,*/ TrajectoryStatus::Value status= TrajectoryStatus::OK)
				: _sampleIndex(sampleIndex)
				//, _linearVelocity(linearVelocity)	//TODO ?
				//, _angularVelocity(angularVelocity)
				, _status(status)
			{}

			size_t _sampleIndex;
			//float _linearVelocity;
			//float _angularVelocity;
			TrajectoryStatus::Value _status;
		};

		struct EntityData
		{
			EntityData()
				: _debugStoreMatchedTrajectory(false)
				, _debugStoreMatchingTrajectory(false)
				, _debugStoreAllTrajectories(false)
			{}
			~EntityData()
			{
				_matchedTrajectoryPositions.clear();
				_matchedTrajectoryPositions.trim();
				_matchedTrajectoryOrientations.clear();
				_matchedTrajectoryOrientations.trim();

				_matchingTrajectoryPositionsPrediction.clear();
				_matchingTrajectoryPositionsPrediction.trim();
				_matchingTrajectoryOrientationsPrediction.clear();
				_matchingTrajectoryOrientationsPrediction.trim();

				_trajectoryPredictionIndices.clear();
				_trajectoryPredictionIndices.trim();
				_trajectoryPositionsPrediction.clear();
				_trajectoryPositionsPrediction.trim();
				_trajectoryOrientationsPrediction.clear();
				_trajectoryOrientationsPrediction.trim();
			}

			bool _debugStoreMatchedTrajectory;
			Array<Vector3> _matchedTrajectoryPositions;
			Array<Quaternion> _matchedTrajectoryOrientations;

			bool _debugStoreMatchingTrajectory;
			size_t _matchingTrajectoryIndex;
			Array<Vector3> _matchingTrajectoryPositionsPrediction;
			Array<Quaternion> _matchingTrajectoryOrientationsPrediction;

			bool _debugStoreAllTrajectories;
			Array<TrajectoryInfo> _trajectoryPredictionIndices;
			Array<Vector3> _trajectoryPositionsPrediction;
			Array<Quaternion> _trajectoryOrientationsPrediction;
		};

	public:		// methods
		GLM_CLASS_STAT_ID( glm::MemoryStatIds::Simulation)

		BlendBNTrajectorySampler();
		~BlendBNTrajectorySampler();

		void doAllocate(int64_t maxCount);
		void doDeallocate();
		void doResize(int64_t instancesCount);

		void init();
		void finish();
		bool isInitialized() const { return _initialized; }
		void init(int64_t entityIndex, const LocomotionInfo& locomotionInfo, int linearSpeedBlendingSamples, int angularSpeedBlendingSamples, int linearSpeedBlendingSamplesOnNextCommands, int angularSpeedBlendingSamplesOnNextCommands, float leftOrRightPreference, bool useHyperbolicLinearSpeedDistribution, bool useHyperbolicAngularSpeedDistribution);
		void finish(int64_t entityIndex);

		const bool hasEntityData(int64_t entityIndex) const { return (size_t)entityIndex < _entitiesData.size(); }
		const EntityData& getEntityData(int64_t entityIndex) const { return _entitiesData[entityIndex]; }
		EntityData& touchEntityData(int64_t entityIndex) { return _entitiesData[entityIndex]; }

		const LocomotionInfo& getLocomotionInfo() const { return _locomotionInfo; }
		int getLinearSpeedBlendingSamplesCount() const { return _linearSpeedBlendingSamplesCount; }
		int getAngularSpeedBlendingSamplesCount() const { return _angularSpeedBlendingSamplesCount; }

		size_t getTrajectoriesCount() const { return _linearSpeedBlendingSamplesCount*_angularSpeedBlendingSamplesCount; }

		const Array<size_t>& getTrajectoriesIndexInTriangulation() const { return _inTriangulationIndices; }
		const Array<size_t>& getTrajectoriesIndexOutOfTriangulation() const { return _outOfTriangulationIndices; }

		void getTrajectoryVelocities(int trajectoryIndex, float& linearVelocity, float& angularVelocity) const;
		void getTrajectoryVelocities(int trajectoryIndex, int& linearVelocitySampleIndex, float& linearVelocity, int& angularVelocitySampleIndex, float& angularVelocity) const;

		int getNearestTrajectoryIndexForVelocities(float& linearVelocity, float& angularVelocity) const;
		int getNearestTrajectoryIndexForVelocities(int& linearVelocitySampleIndex, float& linearVelocity, int& angularVelocitySampleIndex, float& angularVelocity) const;

		void getTrajectoryColor(int trajectoryIndex, int& outRed, int& outGreen, int& outBlue) const;

		static void computeTrajectoryPoints(
			int64_t entityIndex, const LocomotionInfo& locoInfo, float denormalizationFactor,
			float& currentLinearVelocity, float& currentAngularVelocity,
			float desiredLinearVelocity, float desiredAngularVelocity,
			float maxLinearAcceleration, float maxAngularAcceleration,
			const Vector3& currentWorldPosition, const Quaternion& currentWorldOrientation,
			Vector3* retTrajectoryPositions, Quaternion* retTrajectoryOrientations, unsigned int trajectoryPointsCount, glm::Time trajectoryPointsDt,
			const Vector3* subNodesDeltaPositions, const Quaternion* subNodesDeltaOrientations, unsigned int subNodesPointsCount, unsigned int subNodesPointsOffset,	//subNodesPointsCount: how many traj. points in the subnodes deltas, subNodesPointsOffset: were does the current computation starts in the subnodes deltas
			float* blendingWeightsCache		//size of the cache must be the number of subnodes of locoInfo._syncBlendNode
		);

		static void deltasTrajectoryToWorldTrajectory(
			Vector3* outTrajectoryPositions, Quaternion* outTrajectoryOrientations,
			const Vector3* inDeltaPositions, const Quaternion* inDeltaOrientations,
			unsigned int trajectoryPointsCount, const Vector3& pelvisWorldPos, const Quaternion& pelvisWorldOri, float denormalizationFactor);

	public:		// data
		LocomotionInfo	_locomotionInfo;	//TODO: if possible, replace by just the Delaunay2* _paramsTriangulation;

		Array<size_t> _outOfTriangulationIndices;	//indices of the samples that are out of the triangulation (reprojected, so not invalid, but redondant)
		Array<size_t> _inTriangulationIndices;	//inverse of the _outOfTriangulationIndices list

		Array<int> _sampledTrajSubsetIndices;	//indices of a smaller count of samples that also represents the whole sampling space, but less defined

		bool _debugMode;

	protected:	// methods
		void computeBlendingSampling(int64_t entityIndex);
		static void blendTrajectoryDeltasPrediction(Vector3* retDeltaPositions, Quaternion* retDeltaOrientations, unsigned int blendNodesCount, unsigned int trajectorySampleCount, const float* blendingWeights, const Vector3* blendNodesDeltaPositions, const Quaternion* blendNodesDeltaOrientations);

	protected:	// data
		int _linearSpeedBlendingSamplesCount;
		int _angularSpeedBlendingSamplesCount;

		bool _useHyperbolicLinearSpeedDistribution;
		bool _useHyperbolicAngularSpeedDistribution;

		float _leftOrRightPreference;

		glm::Array< EntityData > _entitiesData;

	private:	// data
		SpinLock _threadLock;
		bool _initialized;
	};



}