extends Interactable
class_name Pickupable

@export var springStrength: float = 150.0
@export var springDamping: float = 50.0
@export var rotateStrength: float = 10.0
@export var rotateDamping: float = 2.0

@export var maxLiftForce: float = 600.0
@export var maxTorque: float = 40.0
@export var maxThrowImpulse: float = 100.0

@export var throwSpeed: float = 10.0
@export var maxDistance: float = 4.0
@export var body: RigidBody3D
@export var snapAnchor: Marker3D
@export var visualMeshNode: MeshInstance3D

@export_range(0.01, 1000.0, 0.01) var bodyMass: float = 1.0

var dragging: bool = false
var rotating: bool = false
var playerRef

var holdDistance: float
var rotationOffset: Basis
var localCenterOffset: Vector3

var currentSocket: Socket = null

func _ready():
	if body:
		body.mass = bodyMass

func beginInteract(player, hitPosition: Vector3):
	playerRef = player
	dragging = true

	var holdPoint = player.hand
	holdDistance = holdPoint.global_position.distance_to(hitPosition)
	rotationOffset = holdPoint.global_basis.inverse() * body.global_basis
	localCenterOffset = body.global_basis.inverse() * (body.global_position - hitPosition)

func updateInteract(player):
	if !dragging:
		return
	
	if Input.is_action_just_pressed("action") and currentSocket != null:
		if currentSocket.attachedObject == self:
			currentSocket.detachObject()
		else:
			if currentSocket.attachObject(self):
				endInteract(player)
				return
	
	body.sleeping = false

	# Toggle rotation mode
	if Input.is_action_just_pressed("rotate"):
		rotating = true
		playerRef.setMouseFactors(0.0, 0.0)

	if Input.is_action_just_released("rotate"):
		rotating = false
		playerRef.setMouseFactors(1.0, 1.0)

	var holdPoint = player.hand

	# Release if too far away
	if body.global_position.distance_to(player.global_position) > maxDistance:
		endInteract(player)
		return

	if rotating:
		var previewTargetBasis = holdPoint.global_basis * rotationOffset
		var previewCurrentQuat = body.global_basis.get_rotation_quaternion()
		var previewTargetQuat = previewTargetBasis.get_rotation_quaternion()
		
		var previewDiff = previewTargetQuat * previewCurrentQuat.inverse()
		if previewDiff.w < 0.0:
			previewDiff = -previewDiff
			
		if previewDiff.get_angle() < 1.0: 
			var delta = player.interactionMouseDelta
			var weightPenalty = 1.0 / (1.0 + (body.mass * 0.1))
			
			var rot_x = deg_to_rad(-delta.x * 0.35 * weightPenalty)
			var rot_y = deg_to_rad(-delta.y * 0.35 * weightPenalty)
			
			rotationOffset = Basis(Vector3.UP, rot_x) * rotationOffset
			rotationOffset = Basis(Vector3.RIGHT, rot_y) * rotationOffset

	# Angular spring
	var targetBasis = holdPoint.global_basis * rotationOffset
	var currentQuat = body.global_basis.get_rotation_quaternion()
	var targetQuat = targetBasis.get_rotation_quaternion()
	var diff = targetQuat * currentQuat.inverse()

	if diff.w < 0.0:
		diff = -diff

	var angle = diff.get_angle()
	if angle > 0.001:
		var angularAcceleration = (diff.get_axis() * angle * rotateStrength * 10.0) - (body.angular_velocity * rotateDamping * 10.0)
		var invI = body.get_inverse_inertia_tensor()
		var torque = Vector3.ZERO
		
		if abs(invI.determinant()) > 0.00001:
			var I = invI.inverse()
			torque = I * angularAcceleration
		else:
			torque = angularAcceleration * body.mass * 0.01 
		torque = torque.limit_length(maxTorque)
		body.apply_torque(torque)

	var targetGrabPos = holdPoint.global_position + (-holdPoint.global_basis.z * holdDistance)
	var targetCenterPos = targetGrabPos + (targetBasis * localCenterOffset)
	var posError = targetCenterPos - body.global_position
	var force = (posError * springStrength) - (body.linear_velocity * springDamping)
	force *= body.mass 
	force = force.limit_length(maxLiftForce)
	body.apply_central_force(force)

func endInteract(player):
	dragging = false
	rotating = false
	player.setMouseFactors(1.0, 1.0)
	playerRef = null

func throw():
	if !dragging:
		return

	var direction = -playerRef.camera.global_basis.z
	var throwImpulse = direction * throwSpeed * body.mass
	throwImpulse = throwImpulse.limit_length(maxThrowImpulse)
	body.apply_impulse(throwImpulse)
	endInteract(playerRef)