//======= Copyright (c) Valve Corporation, All rights reserved. ===============
//
// Purpose: Estimates the velocity of an object based on change in position
//
//=============================================================================

using UnityEngine;
using System.Collections;

namespace Valve.VR.InteractionSystem
{
	//-------------------------------------------------------------------------
	public class VelocityEstimator : MonoBehaviour
	{
		[Tooltip( "How many frames to average over for computing velocity" )]
		public int velocityAverageFrames = 5;
		[Tooltip( "How many frames to average over for computing angular velocity" )]
		public int angularVelocityAverageFrames = 11;

		public bool estimateOnAwake = false;

		private Coroutine routine;
		private int sampleCount;
		private Vector3[] velocitySamples;
		private Vector3[] angularVelocitySamples;


		//-------------------------------------------------
		public void BeginEstimatingVelocity()
		{
			FinishEstimatingVelocity();

			routine = StartCoroutine( EstimateVelocityCoroutine() );
		}


		//-------------------------------------------------
		public void FinishEstimatingVelocity()
		{
			if ( routine != null )
			{
				StopCoroutine( routine );
				routine = null;
			}
		}


		//-------------------------------------------------
		public Vector3 GetVelocityEstimate()
		{
			// Compute average velocity
			Vector3 velocity = Vector3.zero;
			int velocitySampleCount = Mathf.Min( sampleCount, velocitySamples.Length );
			if ( velocitySampleCount != 0 )
			{
				for ( int i = 0; i < velocitySampleCount; i++ )
				{
					velocity += velocitySamples[i];
				}
				velocity *= ( 1.0f / velocitySampleCount );
			}

			return velocity;
		}


		//-------------------------------------------------
		public Vector3 GetAngularVelocityEstimate()
		{
			// Compute average angular velocity
			Vector3 angularVelocity = Vector3.zero;
			int angularVelocitySampleCount = Mathf.Min( sampleCount, angularVelocitySamples.Length );
			if ( angularVelocitySampleCount != 0 )
			{
				for ( int i = 0; i < angularVelocitySampleCount; i++ )
				{
					angularVelocity += angularVelocitySamples[i];
				}
				angularVelocity *= ( 1.0f / angularVelocitySampleCount );
			}

			return angularVelocity;
		}


		//-------------------------------------------------
		public Vector3 GetAccelerationEstimate()
		{
			Vector3 average = Vector3.zero;
			for ( int i = 2 + sampleCount - velocitySamples.Length; i < sampleCount; i++ )
			{
				if ( i < 2 )
					continue;

				int first = i - 2;
				int second = i - 1;

				Vector3 v1 = velocitySamples[first % velocitySamples.Length];
				Vector3 v2 = velocitySamples[second % velocitySamples.Length];
				average += v2 - v1;
			}
			average *= ( 1.0f / Time.deltaTime );
			return average;
		}


		//-------------------------------------------------
		void Awake()
		{
			velocitySamples = new Vector3[velocityAverageFrames];
			angularVelocitySamples = new Vector3[angularVelocityAverageFrames];

			if ( estimateOnAwake )
			{
				BeginEstimatingVelocity();
			}
		}


		//-------------------------------------------------
		private IEnumerator EstimateVelocityCoroutine()
		{
			sampleCount = 0;

			Vector3 previousPosition = transform.position;
			Quaternion previousRotation = transform.rotation;
			while ( true )
			{
				yield return new WaitForEndOfFrame();

				float velocityFactor = 1.0f / Time.deltaTime;

				int v = sampleCount % velocitySamples.Length;
				int w = sampleCount % angularVelocitySamples.Length;
				sampleCount++;

				// Estimate linear velocity
				velocitySamples[v] = velocityFactor * ( transform.position - previousPosition );

				// Estimate angular velocity
				Quaternion deltaRotation = transform.rotation * Quaternion.Inverse( previousRotation );

				float theta = 2.0f * Mathf.Acos( Mathf.Clamp( deltaRotation.w, -1.0f, 1.0f ) );
				if ( theta > Mathf.PI )
				{
					theta -= 2.0f * Mathf.PI;
				}

				Vector3 angularVelocity = new Vector3( deltaRotation.x, deltaRotation.y, deltaRotation.z );
				if ( angularVelocity.sqrMagnitude > 0.0f )
				{
					angularVelocity = theta * velocityFactor * angularVelocity.normalized;
				}

				angularVelocitySamples[w] = angularVelocity;

				previousPosition = transform.position;
				previousRotation = transform.rotation;
			}
		}
	}
}