extends Interactable
class_name Lever

@export var body: RigidBody3D
@export var hinge: HingeJoint3D

@export var angleOn: float = -45.0
@export var angleOff: float = 0.0

@export var snapStrength: float = 200.0
@export var maxGrabDistance: float = 4.0
@export var mouseForce: float = 0.015

var dragging := false
var grabLocal := Vector3.ZERO
var handOffset := Vector3.ZERO
var targetAngle := 0.0

func _ready():
	hinge.set_param(HingeJoint3D.PARAM_LIMIT_LOWER, deg_to_rad(angleOn))
	hinge.set_param(HingeJoint3D.PARAM_LIMIT_UPPER, deg_to_rad(angleOff))
	targetAngle = angleOff


func _physics_process(delta):
	if dragging:
		return

	var angle = rad_to_deg(get_hinge_angle())

	# Snap to nearest end
	var mid = (angleOn + angleOff) * 0.5
	if angle < mid:
		targetAngle = angleOn
	else:
		targetAngle = angleOff

	var angleError = deg_to_rad(targetAngle) - get_hinge_angle()

	# HARD SNAP — no spring
	var axis = hinge.global_transform.basis.x
	var torque = snapStrength * sign(angleError)

	body.apply_torque(axis * torque)


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


func updateInteract(player):
	if !dragging:
		return

	var worldGrab = body.to_global(grabLocal)

	# Only vertical mouse movement (Y)
	handOffset += -player.camera.global_basis.y * player.interactionMouseDelta.y * mouseForce
	handOffset = handOffset.limit_length(0.25)

	var targetHand = player.hand.global_position + handOffset
	var error = targetHand - worldGrab

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

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

	# Correct hinge axis (X)
	var hingeAxis = body.global_basis.x
	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 * snapStrength
	body.apply_force(force, offset)


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


func get_hinge_angle() -> float:
	var body_basis = body.global_transform.basis
	var hinge_basis = hinge.global_transform.basis

	# X-axis rotation → use UP vector
	var up = body_basis.y
	var local_up = hinge_basis.inverse() * up

	return atan2(local_up.z, local_up.y)


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