class_name XRToolsVelocityAverager


## XR Tools Velocity Averager class
##
## This class assists in calculating the velocity (both linear and angular)
## of an object. It accepts the following types of input:
##  - Periodic distances
##  - Periodic transforms (for the origin position)
##
## It provides the average velocity calculated from the total distance
## divided by the total time.


# Count of averages to perform
var _count: int

# Array of time deltas (in float seconds)
var _time_deltas := Array()

# Array of linear distances (Vector3 Castesian Distances)
var _linear_distances := Array()

# Array of angular distances (Vector3 Euler Distances)
var _angular_distances := Array()

# Last transform
var _last_transform := Transform3D()

# Has last transform flag
var _has_last_transform := false


## Initialize the XRToolsVelocityAverager with an averaging count
func _init(count: int):
	_count = count

## Clear the averages
func clear():
	_time_deltas.clear()
	_linear_distances.clear()
	_angular_distances.clear()
	_has_last_transform = false

## Add linear and angular distances to the averager
func add_distance(delta: float, linear_distance: Vector3, angular_distance: Vector3):
	# Sanity check
	assert(delta > 0, "Velocity averager requires positive time-deltas")

	# Add data averaging arrays
	_time_deltas.push_back(delta)
	_linear_distances.push_back(linear_distance)
	_angular_distances.push_back(angular_distance)

	# Keep the number of samples down to the requested count
	if _time_deltas.size() > _count:
		_time_deltas.pop_front()
		_linear_distances.pop_front()
		_angular_distances.pop_front()

## Add a transform to the averager
func add_transform(delta: float, transform: Transform3D):
	# Handle saving the first transform
	if !_has_last_transform:
		_last_transform = transform
		_has_last_transform = true
		return

	# Calculate the linear cartesian distance
	var linear_distance := transform.origin - _last_transform.origin

	# Calculate the euler angular distance
	var angular_distance := (transform.basis * _last_transform.basis.inverse()).get_euler()

	# Update the last transform
	_last_transform = transform

	# Add distances
	add_distance(delta, linear_distance, angular_distance)

## Calculate the average linear velocity
func linear_velocity() -> Vector3:
	# Skip if no averages
	if _time_deltas.size() == 0:
		return Vector3.ZERO

	# Calculate the total time in the average window
	var total_time := 0.0
	for dt in _time_deltas:
		total_time += dt

	# Sum the cartesian distances in the average window
	var total_linear := Vector3.ZERO
	for dd in _linear_distances:
		total_linear += dd

	# Return the average cartesian-velocity
	return total_linear / total_time

## Calculate the average angular velocity as a Vector3 euler-velocity
func angular_velocity() -> Vector3:
	# Skip if no averages
	if _time_deltas.size() == 0:
		return Vector3.ZERO

	# Calculate the total time in the average window
	var total_time := 0.0
	for dt in _time_deltas:
		total_time += dt

	# At first glance the following operations may look incorrect as they appear
	# to involve scaling of euler angles which isn't a valid operation.
	#
	# They are actually correct due to the value being a euler-velocity rather
	# than a euler-angle. The difference is that physics engines process euler
	# velocities by converting them to axis-angle form by:
	# - Angle-velocity: euler-velocity vector magnitude
	# - Axis: euler-velocity normalized and axis evaluated on 1-radian rotation
	#
	# The result of this interpretation is that scaling the euler-velocity
	# by arbitrary amounts only results in the angle-velocity changing without
	# impacting the axis of rotation.

	# Sum the euler-velocities in the average window
	var total_angular := Vector3.ZERO
	for dd in _angular_distances:
		total_angular += dd

	# Calculate the average euler-velocity
	return total_angular / total_time