extends Interactable
class_name HingeInteractable

@export var body: RigidBody3D
@export var springStrength: float = 100.0
@export var springDamping: float = 50.0
@export var maxGrabDistance: float = 6.0
@export var mouseForce: float = 0.02

@export var hinge: HingeJoint3D
@export var hingeAngularUpperLimit: float = 120.0
@export var hingeAngularLowerLimit: float = -120.0

@export var snapAngleThreshold: float = 1.0
@export var snapVelocityThreshold: float = 5.0

@export var mouseHorizontalFactor: float = 1.0
@export var mouseVerticalFactor: float = 1.0

var dragging: bool = false
var grabLocal: Vector3 = Vector3.ZERO
var handOffsetLocal: Vector3 = Vector3.ZERO

func _ready() -> void:
	hinge.set_param(HingeJoint3D.PARAM_LIMIT_LOWER, deg_to_rad(hingeAngularLowerLimit))
	hinge.set_param(HingeJoint3D.PARAM_LIMIT_UPPER, deg_to_rad(hingeAngularUpperLimit))

func beginInteract(player, hitPosition: Vector3):
	dragging = true
	handOffsetLocal = Vector3.ZERO
	grabLocal = body.to_local(hitPosition)
	player.setMouseFactors(mouseHorizontalFactor, mouseVerticalFactor)

func updateInteract(player):
	if !dragging:
		return
	body.sleeping = false

	var camLocalX = body.global_basis.inverse() * player.camera.global_basis.x
	var camLocalY = body.global_basis.inverse() * player.camera.global_basis.y

	handOffsetLocal += camLocalX * player.interactionMouseDelta.x * mouseForce
	handOffsetLocal += -camLocalY * player.interactionMouseDelta.y * mouseForce
	handOffsetLocal = handOffsetLocal.limit_length(0.3)
	
	var worldGrab = body.to_global(grabLocal)
	var handDistance = player.hand.global_position.distance_to(worldGrab)
	if handDistance > maxGrabDistance:
		player.stopInteraction()
		return

	var targetHandLocal = body.to_local(player.hand.global_position) + handOffsetLocal
	var errorLocal = targetHandLocal - grabLocal

	var bodyAngularVelocityLocal = body.global_basis.inverse() * body.angular_velocity
	var bodyLinearVelocityLocal = body.global_basis.inverse() * body.linear_velocity
	var pointVelocityLocal = bodyLinearVelocityLocal + bodyAngularVelocityLocal.cross(grabLocal)
	
	var hingeAxisGlobal = hinge.global_basis.z 
	var hingeAxisLocal = (body.global_basis.inverse() * hingeAxisGlobal).normalized()
	
	var hingePositionLocal = body.to_local(hinge.global_position)
	var leverArm = grabLocal - hingePositionLocal
	var tangentLocal = hingeAxisLocal.cross(leverArm)
	if tangentLocal.length_squared() < 0.0001:
		return
		
	tangentLocal = tangentLocal.normalized()
	var tangentError = tangentLocal * errorLocal.dot(tangentLocal)
	var tangentVelocity = tangentLocal * pointVelocityLocal.dot(tangentLocal)

	var forceLocal = tangentError * springStrength - tangentVelocity * springDamping
	var forceGlobal = body.global_basis * forceLocal
	var offsetGlobal = body.global_basis * grabLocal
	body.apply_force(forceGlobal, offsetGlobal)

func endInteract(player):
	dragging = false
	handOffsetLocal = Vector3.ZERO
	player.setMouseFactors(1.0, 1.0)