godot arm ik

extends Spatial

onready var hand_r = $HandR
onready var hand_l = $HandL

#used for reload animations, this is the point the left hand goes to to grab an ammo clip
export(NodePath) var reload_clip_point_path
onready var reload_clip_point = get_node(reload_clip_point_path)
onready var reload_clip_interim_point = reload_clip_point.get_node("ReloadClipPointInterim")
var grab_clip_weight = 0.0 # if weight is 1.0 hand is at reload point, if 0.0 at regular point

export(NodePath) var reload_magnet_path
onready var reload_magnet = get_node(reload_magnet_path)

export(NodePath) var magnet_l_path
export(NodePath) var magnet_r_path

var magnet_l : Spatial
var magnet_r : Spatial

var ref_hand_r : Spatial = null
var ref_hand_l : Spatial = null

export(NodePath) var skeleton_path
onready var skeleton = get_node(skeleton_path)

var upper_arm_l_ind = 0
var lower_arm_l_ind = 0
var hand_l_ind = 0
var upper_arm_r_ind = 0
var lower_arm_r_ind = 0
var hand_r_ind = 0

var arm_upper_length_l = 0.0
var arm_lower_length_l = 0.0
var arm_upper_length_r = 0.0
var arm_lower_length_r = 0.0

func _ready():
	magnet_l = get_node(magnet_l_path)
	magnet_r = get_node(magnet_r_path)

	upper_arm_l_ind = skeleton.find_bone("arm_upperl")
	lower_arm_l_ind = skeleton.find_bone("arm_lowerl")
	hand_l_ind = skeleton.find_bone("handl")
	upper_arm_r_ind = skeleton.find_bone("arm_upperr")
	lower_arm_r_ind = skeleton.find_bone("arm_lowerr")
	hand_r_ind = skeleton.find_bone("handr")

	var shoulder_pos_l = skeleton.get_bone_global_pose(upper_arm_l_ind).origin
	var elbow_pos_l = skeleton.get_bone_global_pose(lower_arm_l_ind).origin
	var hand_pos_l = skeleton.get_bone_global_pose(hand_l_ind).origin
	var shoulder_pos_r = skeleton.get_bone_global_pose(upper_arm_r_ind).origin
	var elbow_pos_r = skeleton.get_bone_global_pose(lower_arm_r_ind).origin
	var hand_pos_r = skeleton.get_bone_global_pose(hand_r_ind).origin

	arm_upper_length_l = shoulder_pos_l.distance_to(elbow_pos_l)
	arm_lower_length_l = hand_pos_l.distance_to(elbow_pos_l)
	arm_upper_length_r = shoulder_pos_r.distance_to(elbow_pos_r)
	arm_lower_length_r = hand_pos_r.distance_to(elbow_pos_r)

func update_hand_ref(_hand_r, _hand_l):
	ref_hand_r = _hand_r
	ref_hand_l = _hand_l

func _process(_delta):
	update_hand_transforms()

func update_hand_transforms():
	if !is_instance_valid(ref_hand_l) or !is_instance_valid(ref_hand_r):
		return
	hand_r.global_transform = ref_hand_r.global_transform
	if grab_clip_weight < 0.001:
		hand_l.global_transform = ref_hand_l.global_transform
	else:
		var ref_hand_pos = ref_hand_l.global_transform.origin
		var reload_point_pos = reload_clip_point.global_transform.origin
		var interim_reload_point_pos = reload_clip_interim_point.global_transform.origin
		if grab_clip_weight < 0.5:
			var weight = grab_clip_weight / 0.5
			hand_l.global_transform.origin = ref_hand_pos.linear_interpolate(interim_reload_point_pos, weight)
		else:
			var weight = (grab_clip_weight - 0.5) / 0.5
			hand_l.global_transform.origin = interim_reload_point_pos.linear_interpolate(reload_point_pos, weight)
		var lower_arm_transform = skeleton.get_bone_global_pose(lower_arm_l_ind)
		var t_x = skeleton.to_global(lower_arm_transform.basis.x)
		var t_y = skeleton.to_global(lower_arm_transform.basis.y)
		var t_z = skeleton.to_global(lower_arm_transform.basis.z)
		t_x = t_x - skeleton.global_transform.origin
		t_y = t_y - skeleton.global_transform.origin
		t_z = t_z - skeleton.global_transform.origin
		var goal_basis = Basis(t_x, -t_z, t_y)
		var start_basis = ref_hand_l.global_transform.basis
		var weight = clamp(grab_clip_weight, 0.0, 0.2) / 0.2
		#hand_l.global_transform.basis = start_basis.slerp(goal_basis, weight) # this is error prone for some reason
		hand_l.global_transform.basis = goal_basis
	update_arm_transforms()

func update_arm_transforms():
	var weight = clamp(grab_clip_weight, 0.0, 0.5) / 0.5
	var magnet_l_pos = magnet_l.global_transform.origin.linear_interpolate(reload_magnet.global_transform.origin, grab_clip_weight)
	update_arm_transform(upper_arm_l_ind, lower_arm_l_ind, hand_l_ind, arm_upper_length_l, arm_lower_length_l, hand_l, magnet_l_pos)
	#update_arm_transform(upper_arm_l_ind, lower_arm_l_ind, hand_l_ind, arm_upper_length_l, arm_lower_length_l, hand_l, magnet_l.global_transform.origin)
	update_arm_transform(upper_arm_r_ind, lower_arm_r_ind, hand_r_ind, arm_upper_length_r, arm_lower_length_r, hand_r, magnet_r.global_transform.origin, false)

func update_arm_transform(upper_arm_ind, lower_arm_ind, hand_ind, upper_arm_len, lower_arm_len, goal_hand, magnet_position, is_left=true):
	# bunch of math for doing IK on the arms
	var shoulder_pos = skeleton.get_bone_global_pose(upper_arm_ind).origin
	var goal_hand_pos = skeleton.to_local(goal_hand.global_transform.origin)
	var dis_from_shoulder_to_goal_pos = shoulder_pos.distance_to(goal_hand_pos)
	var angles = SSS_calc(upper_arm_len, lower_arm_len, dis_from_shoulder_to_goal_pos)
	var y = shoulder_pos.direction_to(goal_hand_pos)
	var tmp_z = shoulder_pos.direction_to(skeleton.to_local(magnet_position))
	var x = -y.cross(tmp_z).normalized()
	var z = x.cross(y).normalized()


	var angle = angles.B
	if angle != 0 and is_left:
		angle = PI - angle
	if !is_left:
		x = -x
		z = -z
		angle = -(PI - angle)

	y = y.rotated(x, angle)
	z = z.rotated(x, angle)

	var upper_arm_y = y

	#arm will point along y of basis, bicep faces towards z, x is towards inside
	var new_upper_arm_transform = Transform()
	new_upper_arm_transform.basis = Basis(z, y, -x)
	new_upper_arm_transform.origin = shoulder_pos
	new_upper_arm_transform.scaled(skeleton.scale)
	skeleton.set_bone_global_pose_override(upper_arm_ind, new_upper_arm_transform, 1, true)

	var new_lower_arm_transform = Transform()
	#var elbow_pos = skeleton.get_bone_global_pose(lower_arm_ind).origin

	if is_left:
		angle = PI - angles.C
	else:
		var tmp = y
		y = z
		z = -tmp
		angle = -3*PI/2 + angles.C

	y = -y.rotated(x, angle).normalized()
	z = -z.rotated(x, angle)

	if is_left:
		var t = clamp(cur_y_angle/90, 0.0, 1.0)
		z = z.rotated(y, -t * deg2rad(30))
		x = x.rotated(y, -t * deg2rad(30))

	new_lower_arm_transform.basis = Basis(z, y, -x)
	new_lower_arm_transform.origin = shoulder_pos + upper_arm_y * upper_arm_len
	skeleton.set_bone_global_pose_override(lower_arm_ind, new_lower_arm_transform, 1, true)

	var new_hand_transform = Transform()
	var hand_global_pos = goal_hand.global_transform.origin
	new_hand_transform.origin = goal_hand_pos
	var t_x = skeleton.to_local(hand_global_pos + goal_hand.global_transform.basis.x)
	var t_y = skeleton.to_local(hand_global_pos + goal_hand.global_transform.basis.y)
	var t_z = skeleton.to_local(hand_global_pos + goal_hand.global_transform.basis.z)
	t_x = t_x - goal_hand_pos
	t_y = t_y - goal_hand_pos
	t_z = t_z - goal_hand_pos
	if is_left:
		new_hand_transform.basis = Basis(t_x, t_z, -t_y)
	else:
		new_hand_transform.basis = Basis(t_x, -t_z, t_y)
	skeleton.set_bone_global_pose_override(hand_ind, new_hand_transform, 1, true)
	#$DebugPoint.transform.origin = new_hand_transform.origin


func SSS_calc(side_a, side_b, side_c):
	if side_c >= side_a + side_b:
		return {"A": 0, "B": 0, "C": 0}
	var angle_a = law_of_cos(side_b, side_c, side_a)
	var angle_b = law_of_cos(side_c, side_a, side_b) + PI
	var angle_c = PI - angle_a - angle_b

	return {"A": angle_a, "B": angle_b, "C": angle_c}

func law_of_cos(a, b, c):
	if 2 * a * b == 0:
		return 0
	return acos( (a * a + b * b - c * c) / ( 2 * a * b) )

var cur_y_angle = 0.0
func update_turn_info(y_angle: float):
	cur_y_angle = y_angle

func set_grab_clip_weight(weight: float):
	grab_clip_weight = weight