extends Node3D
class_name Puppet

@export var handScaleFactor: float = 0.15

enum FOOT { LEFT = 0, RIGHT = 1 }
enum HAND { LEFT = 0, RIGHT = 1}

var gravity_vector : Vector3 = ProjectSettings.get_setting("physics/3d/default_gravity_vector")
var gravity_magnitude : int = ProjectSettings.get_setting("physics/3d/default_gravity")
var gravity = gravity_vector * gravity_magnitude

var controlledByPlayer: bool
var activeFoot: FOOT = FOOT.LEFT
var otherFoot: FOOT = FOOT.RIGHT
var velocities: Array = [0, 0, 0]
var inputDistance: Vector3
var inputBasis: Basis
var controlBarWidth: float
var feetDistance: float
var feet: Array = [null, null]
var hands: Array = [null, null]
var grabbing: Array = [false, false]
var canGrabs: Array = [null, null]

func _ready():
	feet[FOOT.LEFT] = $PhysicsContainer/LeftFoot
	feet[FOOT.RIGHT] = $PhysicsContainer/RightFoot
	hands[HAND.LEFT] = $HandContainer/LeftHand
	hands[HAND.RIGHT] = $HandContainer/RightHand
	canGrabs[HAND.LEFT] = $HandContainer/LeftHand/CanGrab
	canGrabs[HAND.RIGHT] = $HandContainer/RightHand/CanGrab
	for foot in FOOT.values():
		feet[foot].global_position += global_position
	feetDistance = $PhysicsContainer/LeftFoot.global_position.distance_to($PhysicsContainer/RightFoot.global_position)
	$PhysicsContainer/Body.global_position += global_position

func _physics_process(delta):
	# Move active foot based on input
	var distance = inputDistance * feetDistance / controlBarWidth / delta
	feet[activeFoot].velocity = distance
	feet[activeFoot].transform.basis = inputBasis
	transform.basis = inputBasis
	feet[activeFoot].move_and_slide()

	# Move other foot based on gravity and pull active foot behind
	feet[otherFoot].velocity = gravity * delta
	var posBefore = feet[otherFoot].global_position
	feet[otherFoot].move_and_slide()
	var posAfter = feet[otherFoot].global_position
	feet[activeFoot].velocity = posAfter - posBefore
	feet[activeFoot].move_and_slide()

	# Correct overshoot of active foot in order to keep fixed distance between feets
	var d = feet[activeFoot].global_position - feet[otherFoot].global_position
	d = d / (d.length() / feetDistance)
	feet[activeFoot].velocity = feet[otherFoot].global_position + d - feet[activeFoot].global_position
	feet[activeFoot].move_and_slide()

	# Move main body between both feet
	var mainBodyDistance = (feet[FOOT.LEFT].global_position + feet[FOOT.RIGHT].global_position) / 2
	mainBodyDistance -= global_position
	$PhysicsContainer/Body.velocity = mainBodyDistance / delta
	$PhysicsContainer/Body.move_and_slide()
	global_position = $PhysicsContainer/Body.global_position

	# Grab handling
	for hand in HAND.values():
		if grabbing[hand]:
			canGrabs[hand].grab()
		else:
			canGrabs[hand].release()

func setControlledByPlayer(controlled: bool):
	controlledByPlayer = controlled

func setControlbarWidth(width: float):
	controlBarWidth = width

func setActiveFoot(foot: FOOT):
	activeFoot = foot
	otherFoot = 1 - activeFoot as FOOT

func moveActiveFoot(distance: Vector3, myBasis: Basis):
	inputDistance = distance
	# Project rotation to x-z plane
	# Is there a clever mathematical trick, which is faster and more beautiful?
	var v = myBasis * Vector3(0, 0, 1)
	var angle = atan2(v.x, v.z)
	inputBasis = Basis().rotated(Vector3(0, 1, 0), angle)

func moveHand(hand: HAND, handPosition: Vector3, myBasis: Basis):
	hands[hand].position = handPosition * handScaleFactor
	hands[hand].transform.basis = myBasis

func setGrab(hand: HAND, grab: bool):
	grabbing[hand] = grab
