extends Node


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


# Called when the node enters the scene tree for the first time.
var cam
var fps_cam
var motion = Vector2()
func _ready():
	var root = get_parent()
	process_priority = 200



# Called every frame. 'delta' is the elapsed time since the previous frame.
#func _process(delta):
#	pass
func _physics_process(delta):
	var orientation: Transform
	var root = get_parent()
	if !root.has_meta("cam"):
		return
	cam = root.get_meta("cam")
	assert(cam)
	fps_cam = root.get_meta("fps_cam")
	assert(fps_cam)
	var animtree = root.get_meta("animation_tree")
	if !animtree:
		return
	if root.has_meta("orientation"):
		orientation = root.get_meta("orientation")
	if !orientation:
		return
	var running = false
	assert(cam)
	assert(fps_cam)

	var motion_target = Vector2(Input.get_action_strength("walk_right") - Input.get_action_strength("walk_left"),
			Input.get_action_strength("walk_front") - Input.get_action_strength("walk_back"))
	motion = motion.linear_interpolate(motion_target, 10.0 * delta)
	var xmotion = motion
	xmotion.x = clamp(xmotion.x, -0.4, 0.4)
#	if root.has_meta("group_behavior"):
#		if motion_target.y < 0:
#			behavior.destroy_group_behavior(root.get_meta("group_behavior"))
	if !root.has_meta("vehicle") && !root.has_meta("cmdqueue") && !root.has_meta("group_behavior"):
		if Input.is_action_just_pressed("activate"):
			markers.activate_marker()
		if Input.is_action_pressed("run"):
			running = true
		var cam_z = Vector3()
		var cam_x = Vector3()
		if cam.current:
			cam_z = cam.global_transform.basis.z
			cam_x = cam.global_transform.basis.x
		cam_z.y = 0
		cam_z = cam_z.normalized()
		cam_x.y = 0
		cam_x = cam_x.normalized()
		var target = cam_x * motion.x - cam_z * motion.y
		if target.length() > 0.001 && cam.current:
			var q_from = Quat(orientation.basis)
			var q_to = Quat(Transform().looking_at(target, Vector3.UP).basis)
			orientation.basis = Basis(q_from.slerp(q_to, delta * 10.0))
			root.set_meta("orientation", orientation)
		var fwd_probe = characters.forward_probe(root, 0.5, 0.05, 1.0, 0.1)
		if fwd_probe > 0.1 && fwd_probe < 0.7:
			var cmdq = []
			if root.has_meta("cmdqueue"):
				cmdq = root.get_meta("cmdqueue")
			cmdq.push_back(["climb", "1"])
			root.set_meta("cmdqueue", cmdq)

		animtree["parameters/state/playback"].travel("locomotion")
		var strafing = 0
		var linear = 0
		if fps_cam.current:
			strafing = motion.x
			linear = motion.y
		else:
			strafing = 0
			linear = motion.length()
		if running:
			animtree["parameters/state/locomotion/loc/blend_position"] = Vector2(linear * 3.0, strafing * 2.0)
		else:
			animtree["parameters/state/locomotion/loc/blend_position"] = Vector2(linear * 0.5, strafing)
	elif root.has_meta("vehicle"):
		if Input.is_action_just_pressed("activate"):
			markers.activate_marker()
		var vehicle: VehicleBody = root.get_meta("vehicle")
		var driver = vehicle.get_meta("driver")
		if driver == root:
			if vehicle.parked:
				vehicle.parked = false
				vehicle.mode = vehicle.MODE_RIGID
			if abs(xmotion.y) < 0.001:
				vehicle.brake = 1000
				vehicle.steering = -xmotion.x * 0.7
			else:
				vehicle.brake = 0
				if vehicle.linear_velocity.length() < 16.6:
					vehicle.engine_force = 4000 * xmotion.y
				else:
					vehicle.engine_force = 500 * xmotion.y
				var accel = vehicle.angular_velocity * 350.0 * delta
				vehicle.steering = -xmotion.x * 0.7