extends Node
export var grab_ik_curve: Curve
export var grab_ik_curve_multiplier: float = 0.14
export var cinematic_cam: PackedScene

# Declare member variables here. Examples:
# var a = 2
# var b = "text"


# Called when the node enters the scene tree for the first time.
var cinematic_camera

func _ready():
	pass # Replace with function body.

var active_states = []
var locked_actors = {}

class Grabbing:
	signal enable_cinematic_camera(where, look)
	signal update_cinematic_camera(where, look)
	signal disable_cinematic_camera
	signal end_stage
	var actor_master: KinematicBody
	var actor_slave: KinematicBody
	var slave_space: RID
	var master_space: RID
	var finished = false
	var follow_node
	var follow_node_full
	var state = 0
	var ik: SkeletonIK
	var ik_curve_pos = 0.0
	var grab_ik_curve: Curve
	var curve_delta_mul: float = 0.16
	func start(act_master: KinematicBody, act_slave: KinematicBody, curve: Curve, delta_mul: float):
		actor_master = act_master
		actor_slave = act_slave
		grab_ik_curve = curve
		curve_delta_mul = delta_mul
		var skel_master = actor_master.get_meta("skeleton")
		assert(skel_master is Skeleton)
		ik = SkeletonIK.new()
		ik.root_bone = "J_Bip_R_UpperArm"
		ik.tip_bone = "J_Bip_R_Hand"
		skel_master.add_child(ik)
		
#		actor_master.set_meta("orchestrated", true)
#		actor_slave.set_meta("orchestrated", true)
#		characters.animation_node_travel(actor_master, "grab")
#		characters.animation_node_travel(actor_slave, "grabbed")
		reset_character(actor_master)
		reset_character(actor_slave)
		var mx = actor_master.global_transform
		var sx = actor_slave.global_transform
		var m = mx.interpolate_with(sx, 0.5)
		var cam_where = m.origin + actor_master.global_transform.basis[0] * 1.1
		cam_where += Vector3.UP * 2.0
		var cam_to = m.origin + Vector3.UP * 1.4
		emit_signal("enable_cinematic_camera", cam_where, cam_to)
	func reset_character(b):
		assert(b)
		b.remove_meta("cmdqueue")
		b.remove_meta("cmdq_walk")
		b.remove_meta("climb")
		characters.animation_node_travel(b, "locomotion")
		characters.set_walk_speed(b, 0.0, 0)
		
	func update(delta):
		var mx = actor_master.global_transform
		var sx = actor_slave.global_transform
		var m = mx.interpolate_with(sx, 0.5)
		var cam_where = m.origin + actor_master.global_transform.basis[0] * 1.1
		cam_where += Vector3.UP * 2.0
		var cam_to = m.origin + Vector3.UP * 1.4
		emit_signal("update_cinematic_camera", cam_where, cam_to)
	func physics_update(delta):
		var offt = actor_slave.global_transform.origin - actor_slave.global_transform.basis[2] * 0.45
		match state:
			0:
				reset_character(actor_master)
				reset_character(actor_slave)
				actor_master.set_meta("orchestrated", true)
				actor_slave.set_meta("orchestrated", true)
				actor_master.add_collision_exception_with(actor_slave)
				actor_slave.add_collision_exception_with(actor_master)
				slave_space = PhysicsServer.body_get_space(actor_slave.get_rid())
				PhysicsServer.body_set_space(actor_slave.get_rid(), RID())
				master_space = PhysicsServer.body_get_space(actor_master.get_rid())
				PhysicsServer.body_set_space(actor_master.get_rid(), RID())
				state = 1
			1:
#				actor_slave.get_parent().remove_child(actor_slave)
#				actor_master.add_child(actor_slave)
				var skel_master = actor_master.get_meta("skeleton")
				assert(skel_master is Skeleton)
				var skel_slave = actor_slave.get_meta("skeleton")
				assert(skel_slave is Skeleton)
				follow_node_full = skel_slave.get_node("neck/marker_neck_grab")
				follow_node = Position3D.new()
				skel_master.add_child(follow_node)
				follow_node.global_transform = follow_node_full.global_transform
				ik.target_node = ik.get_path_to(follow_node)
				ik.override_tip_basis = true
				ik.interpolation = 0.0
				ik_curve_pos = 0.0
				
				actor_master.global_transform = Transform(actor_slave.global_transform.basis.rotated(Vector3.UP, PI), offt)
				var master_xform = actor_master.global_transform.looking_at(actor_slave.global_transform.origin, Vector3.UP)
				var slave_xform = actor_slave.global_transform.looking_at(actor_master.global_transform.origin, Vector3.UP)
				var orientation = Transform(actor_slave.global_transform.basis, Vector3())
				actor_slave.set_meta("orientation", orientation)
				var master_orientation = Transform(actor_master.global_transform.basis, Vector3())
				actor_master.set_meta("orientation", master_orientation)
				characters.animation_node_travel(actor_master, "grab")
				characters.animation_node_travel(actor_slave, "grabbed")
				ik.start(true)
				state = 3
			3:
				ik.interpolation = grab_ik_curve.interpolate_baked(ik_curve_pos)
				ik_curve_pos += delta * curve_delta_mul
				follow_node.global_transform = follow_node_full.global_transform
				ik.start(true)
				if ik.interpolation >= 1.0:
					state = 4
			4:
				var m_anim: AnimationTree = actor_master.get_meta("animation_tree")
				var m_state: AnimationNodeStateMachinePlayback = m_anim["parameters/state/playback"]
				var m_l = m_state.get_current_length()
				var m_p = m_state.get_current_play_position()
				var s_anim: AnimationTree = actor_slave.get_meta("animation_tree")
				var s_state: AnimationNodeStateMachinePlayback = s_anim["parameters/state/playback"]
				var s_l = m_state.get_current_length()
				var s_p = m_state.get_current_play_position()
				if m_p >= m_l && s_p >= s_l:
					state = 5
					
#			2:
#				actor_slave.global_transform = Transform(actor_master.global_transform.basis.rotated(Vector3.UP, PI), offt)
#				var orientation = Transform(actor_slave.global_transform.basis, Vector3())
#				actor_slave.set_meta("orientation", orientation)
#				state = 3
#			3:
#				actor_slave.global_transform = Transform(actor_master.global_transform.basis.rotated(Vector3.UP, PI), offt)
#				var orientation = Transform(actor_slave.global_transform.basis, Vector3())
#				actor_slave.set_meta("orientation", orientation)
#				state = 4
	func finish():
		pass

func enable_camera(where, to):
	var current_cam = get_viewport().get_camera()
	var player = current_cam.get_meta("player")
	if !cinematic_camera:
		cinematic_camera = cinematic_cam.instance()
		var cam: Camera = cinematic_camera.get_node("rot_y/rot_x/SpringArm/camera_offset/Camera")
		var cam_offset = cinematic_camera.get_node("rot_y/rot_x/SpringArm/camera_offset")
		cam.set_meta("player", player)
		cam.set_meta("current_cam", current_cam)
		cinematic_camera.set_meta("cam", cam)
		cinematic_camera.set_meta("offset", cam_offset)
		get_viewport().add_child(cinematic_camera)
		cinematic_camera.global_transform.origin = where
#		cam_offset.global_transform.origin = where
		cam.current = true
		cam.look_at(to, Vector3.UP)
func update_camera(where, to):
	var cam = cinematic_camera.get_meta("cam")
	var cam_offset = cinematic_camera.get_meta("offset")
	cam_offset.global_transform.origin = where
	cam.look_at(to, Vector3.UP)
	

func grab(actor_master, actor_slave):
	var gr = Grabbing.new()
	gr.connect("enable_cinematic_camera", self, "enable_camera")
	gr.connect("update_cinematic_camera", self, "update_camera")
	gr.start(actor_master, actor_slave, grab_ik_curve, grab_ik_curve_multiplier)
	active_states.push_back(gr)

func _physics_process(delta):
	var completed = []
	for e in active_states:
		e.physics_update(delta)
		if e.finished:
			completed.push_back(e)
	for d in completed:
		active_states.erase(d)
func _process(delta):
	var completed = []
	for e in active_states:
		e.update(delta)
		if e.finished:
			completed.push_back(e)
	for d in completed:
		active_states.erase(d)