extends Node3D

@export var puppet: Puppet

var controlPoints: Array = Array()
var mainControlPoint: Puppet.FOOT = Puppet.FOOT.RIGHT
var auxiliaryControlPoint: Puppet.FOOT = Puppet.FOOT.LEFT
var lastPos: Array

func _ready():
	controlPoints.resize(2)
	controlPoints[Puppet.FOOT.LEFT] = $ControlPointLeft
	controlPoints[Puppet.FOOT.RIGHT] = $ControlPointRight
	lastPos.resize(2)
	for i in range(2):
		lastPos[i] = getLocalFootPos(controlPoints[i])
	puppet.setControlbarWidth(controlPoints[Puppet.FOOT.LEFT].global_position.distance_to(controlPoints[Puppet.FOOT.RIGHT].global_position))

func _physics_process(_delta):
	if getLocalFootPos(controlPoints[Puppet.FOOT.LEFT]).y > getLocalFootPos(controlPoints[Puppet.FOOT.RIGHT]).y:
		if (mainControlPoint != Puppet.FOOT.LEFT):
			puppet.setActiveFoot(Puppet.FOOT.LEFT)
		mainControlPoint = Puppet.FOOT.LEFT
		auxiliaryControlPoint = Puppet.FOOT.RIGHT
	else:
		if (mainControlPoint != Puppet.FOOT.RIGHT):
			puppet.setActiveFoot(Puppet.FOOT.RIGHT)
		mainControlPoint = Puppet.FOOT.RIGHT
		auxiliaryControlPoint = Puppet.FOOT.LEFT
	var distance: Vector3 = (
		(getLocalFootPos(controlPoints[mainControlPoint])
		- getLocalFootPos(controlPoints[auxiliaryControlPoint]))
	  - (lastPos[mainControlPoint]
		- lastPos[auxiliaryControlPoint])) 
	puppet.moveActiveFoot(distance, getLocalTransform().basis)
	for i in range(2):
		lastPos[i] = getLocalFootPos(controlPoints[i])

func getLocalFootPos(node):
	return getLocalTransform() * node.position

func getLocalTransform():
	return get_parent().transform * transform
