class_name AccelCalibration
extends RefCounted

enum Axis { X, Y, Z }

var axis: Axis = Axis.X
var sign: float = 1.0
var center: float = 0.0
var suggested_threshold: float = 3000.0

var _samples_right: Array[Vector3] = []
var _samples_left: Array[Vector3] = []


func reset() -> void:
	_samples_right.clear()
	_samples_left.clear()


func record_right(accel: Vector3i) -> void:
	_samples_right.append(Vector3(accel))


func record_left(accel: Vector3i) -> void:
	_samples_left.append(Vector3(accel))


func is_ready() -> bool:
	return _samples_right.size() >= 20 and _samples_left.size() >= 20


func analyze() -> bool:
	if not is_ready():
		return false

	var mean_r := _mean(_samples_right)
	var mean_l := _mean(_samples_left)
	var diff := mean_r - mean_l

	var diffs := [absf(diff.x), absf(diff.y), absf(diff.z)]
	axis = Axis.X
	var best: float = diffs[0]
	if diffs[1] > best:
		best = diffs[1]
		axis = Axis.Y
	if diffs[2] > best:
		best = diffs[2]
		axis = Axis.Z

	var delta := _component(diff, axis)
	sign = 1.0 if delta >= 0.0 else -1.0

	var all_samples: Array[Vector3] = []
	all_samples.append_array(_samples_right)
	all_samples.append_array(_samples_left)

	var projections: Array[float] = []
	for sample in all_samples:
		projections.append(_component(sample, axis) * sign)
	projections.sort()
	center = projections[projections.size() / 2]

	var deviations: Array[float] = []
	for value in projections:
		deviations.append(absf(value - center))

	deviations.sort()
	var median_idx := deviations.size() / 2
	var spread := deviations[median_idx] if not deviations.is_empty() else 500.0
	suggested_threshold = clampf(spread * 1.35, 200.0, 12000.0)
	return true


func project(accel: Vector3i) -> float:
	return _component(Vector3(accel), axis) * sign


func axis_name() -> String:
	match axis:
		Axis.X:
			return "x"
		Axis.Y:
			return "y"
		Axis.Z:
			return "z"
	return "?"


func _mean(samples: Array[Vector3]) -> Vector3:
	if samples.is_empty():
		return Vector3.ZERO
	var sum := Vector3.ZERO
	for s in samples:
		sum += s
	return sum / float(samples.size())


func _mean_scalar(samples: Array[Vector3], ax: Axis) -> float:
	return _component(_mean(samples), ax)


func _component(v: Vector3, ax: Axis) -> float:
	match ax:
		Axis.X:
			return v.x
		Axis.Y:
			return v.y
		Axis.Z:
			return v.z
	return 0.0