extends Interactable
class_name Door

@export var body: RigidBody3D
@export var springStrength: float = 200.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
@export var hingeAngularLowerLimit: float = -120

@export var snapAngleThreshold: float = 1
@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 handOffset: 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 _physics_process(_delta):
	if dragging:
		return

	var angle = get_hinge_angle()
	var ang_vel = get_hinge_ang_vel()

	if abs(rad_to_deg(angle)) > snapAngleThreshold:
		return

	if abs(ang_vel) > snapVelocityThreshold:
		return

	# Strong damping near closed position
	var damping = springDamping * 4.0

	# PD controller torque toward angle 0
	var torque = -angle * springStrength - ang_vel * damping

	var axis = hinge.global_transform.basis.y
	body.apply_torque(axis * torque)

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

func updateInteract(player):
	if !dragging:
		return

	var worldGrab = body.to_global(grabLocal)

	handOffset += player.camera.global_basis.x * player.interactionMouseDelta.x * mouseForce
	handOffset += -player.camera.global_basis.y * player.interactionMouseDelta.y * mouseForce
	handOffset = handOffset.limit_length(0.3)
	
	var targetHand = player.hand.global_position + handOffset

	var error = targetHand - worldGrab

	var handDistance = player.hand.global_position.distance_to(worldGrab)
	if handDistance > maxGrabDistance:
		player.stopInteraction()
		return

	var offset = worldGrab - body.global_position

	var pointVelocity = body.linear_velocity + body.angular_velocity.cross(offset)

	var hingeAxis = body.global_basis.y

	var tangent = hingeAxis.cross(offset)
	if tangent.length_squared() < 0.0001:
		return
	tangent = tangent.normalized()

	var tangentError = tangent * error.dot(tangent)
	var tangentVelocity = tangent * pointVelocity.dot(tangent)

	var force = tangentError * springStrength - tangentVelocity * springDamping

	body.apply_force(force, offset)

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

#func get_hinge_angle() -> float:
	#var body_basis = body.global_transform.basis
	#var hinge_basis = hinge.global_transform.basis
#
	## Body forward vector in world space
	#var forward = body_basis.z
#
	## Convert forward vector into hinge local space
	#var local_forward = hinge_basis.inverse() * forward
#
	## Angle around hinge axis (Y)
	#return atan2(local_forward.x, local_forward.z)

func get_hinge_angle() -> float:
	var hinge_axis = hinge.global_transform.basis.y.normalized()

	# Door forward direction
	var fwd = body.global_transform.basis.z.normalized()

	# Reference forward direction (door closed direction)
	var ref = hinge.global_transform.basis.z.normalized()

	# Remove hinge axis component (project onto hinge plane)
	var fwd_proj = (fwd - hinge_axis * fwd.dot(hinge_axis)).normalized()
	var ref_proj = (ref - hinge_axis * ref.dot(hinge_axis)).normalized()

	# Signed angle
	var angle = atan2(
		hinge_axis.dot(ref_proj.cross(fwd_proj)),
		ref_proj.dot(fwd_proj)
	)

	return angle


func get_hinge_ang_vel() -> float:
	var axis = hinge.global_transform.basis.y.normalized()
	return body.angular_velocity.dot(axis)