extends Spatial

export var keep_input_true = true

var velocity = Vector3()
var direction = Vector3()

export var character_height = 1.7

# Walk
export var MAX_SPEED = 2.5
export var MAX_RUNNING_SPEED = 5.0
export var MAX_WALK_SPEED = 1.0
export var CROUCH_MULT = 0.5
var ACCEL = 20
var DEACCEL = 20
var air_control = 0.3

# Jump
var jump_height = 3.5

# Fly
const FLY_SPEED = 40
const FLY_ACCEL = 4

# Crouch
var crouching = false
var crouch_height_diff = 0.8

# Stair
var stair_detection = true
export var stair_speed_mult = 1.0
var stair_detect_static = true
var stair_detect_rigid = false
var stair_step_max_height = 0.25
var stair_check_extent = 0.7
var stair_smoothen_duration = 0.0

var stair_nodes = []
var known_stair_nodes = []
var stair_check_query = PhysicsShapeQueryParameters.new()
var stair_check_shape = BoxShape.new()
var is_on_stair = false
var on_stair_duration = 1.0
var on_stair_timer = 0.0

# Ledge grabbing
var ledge_grab_distance = 0.45
var ledge_grab_max_height = 2.0 #From feet
var ledge_grab_min_height = 0.5
var ledge_grab_horizontal = 0.5
var ledge_angle_max = PI/2

var ledge_hold_duration = 0.4
var ledge_up_force = 3

var ledge_grabbing = false
var ledge_hold_time = 0

var ledge_grab_query = PhysicsShapeQueryParameters.new()
var ledge_grab_shape = BoxShape.new()

# Input
var move_forward = false
var move_backward = false
var move_left = false
var move_right = false
var move_jump = false
var move_sprint = false
var move_walk = false
var move_crouch_toggle = false
var move_ledge_grab = false

func _ready():
	ledge_grab_query.set_shape(ledge_grab_shape)
	ledge_grab_query.exclude = [get_parent()]

	stair_check_query.set_shape(stair_check_shape)
	stair_check_query.exclude = [get_parent()]

func input(property, val):
	if get(property) != null:
		set(property, val)

func move_walk(delta):
	#if move_forward: Debug.msg(move_forward, "move_forward", "%s's movement" % get_parent().name, 0.06)
	#if move_backward: Debug.msg(move_backward, "move_backward", "%s's movement" % get_parent().name, 0.06)
	#if move_left: Debug.msg(move_left, "move_left", "%s's movement" % get_parent().name, 0.06)
	#if move_right: Debug.msg(move_right, "move_right", "%s's movement" % get_parent().name, 0.06)
	#if move_jump: Debug.msg(move_jump, "move_jump", "%s's movement" % get_parent().name, 0.06)
	#if move_sprint: Debug.msg(move_sprint, "move_sprint", "%s's movement" % get_parent().name, 0.06)
	#if move_walk: Debug.msg(move_walk, "move_walk", "%s's movement" % get_parent().name, 0.06)
	#if move_crouch_toggle: Debug.msg(move_crouch_toggle, "move_crouch_toggle", "%s's movement" % get_parent().name, 0.06)
	#if move_ledge_grab: Debug.msg(move_ledge_grab, "move_ledge_grab", "%s's movement" % get_parent().name, 0.06)

	direction = Vector3()

	var aim = get_parent().get_global_transform().basis
	aim.z.y = 0

	if !ledge_grabbing:
		if move_forward:
			direction -= aim.z
		if move_backward:
			direction += aim.z
		if move_left:
			direction -= aim.x
		if move_right:
			direction += aim.x
		if move_jump and get_parent().is_on_floor():
			move_jump = false
			if !hitting_head():
				velocity.y = jump_height
				#Debug.msg("Jumped!", "", "%s's movement" % get_parent().name)

		if move_ledge_grab:
			ledge_grabbing = check_ledge()
	else:
		if move_ledge_grab:
			if check_ledge(true):
				ledge_hold_time += delta
				#Debug.msg(ledge_hold_duration - ledge_hold_time, "Hold timer: ", "%s's movement" % get_parent().name)
			else:
				ledge_grabbing = false
				ledge_hold_time = 0
		else:
			ledge_grabbing = false
			ledge_hold_time = 0

	if move_crouch_toggle:
		move_crouch_toggle = false
		if crouching:
			if !hitting_head():
				set_crouch(false)
		else:
			set_crouch(true)

	#Debug.msg(crouching, "Crouching", "%s's movement" % get_parent().name)

	direction = direction.normalized()

	if !ledge_grabbing:
		velocity -= Global.gravity * Global.gravity_vector * delta
	elif ledge_hold_time >= ledge_hold_duration:
		velocity.y = ledge_up_force
	else:
		velocity.y = 0

	var temp_velocity = velocity
	temp_velocity.y = 0

	if stair_detection:
		check_stair()

	if on_stair_timer >= 0:
		on_stair_timer -= delta
	else:
		is_on_stair = false

	#Debug.msg(is_on_stair, "On Stair", "%s's movement" % get_parent().name)

	var speed
	if move_sprint:
		speed = MAX_RUNNING_SPEED
	elif move_walk:
		speed = MAX_WALK_SPEED
	else:
		speed = MAX_SPEED

	if crouching:
		speed *= CROUCH_MULT

	if is_on_stair:
		speed *= stair_speed_mult

	var target = direction * speed

	var acceleration
	if direction.dot(temp_velocity) > 0:
		acceleration = ACCEL
	else:
		acceleration = DEACCEL

	if !get_parent().is_on_floor():
		acceleration *= air_control

	temp_velocity = temp_velocity.linear_interpolate(target, acceleration * delta)

	velocity.x = temp_velocity.x
	velocity.z = temp_velocity.z

	velocity = get_parent().move_and_slide(velocity, Vector3(0, 1, 0))

	if stair_detection:
		clear_stairs()

	var slides = get_parent().get_slide_count()

	for ss in slides:
		var col = get_parent().get_slide_collision(ss).collider
		if col.get("sleeping") != null:
			if col.sleeping:
				col.set_sleeping(false)
				col.emit_signal("sleeping_state_changed")
				#Debug.msg("Woke up!", "", col.name)

	if !keep_input_true:
		move_forward = false
		move_backward = false
		move_left = false
		move_right = false
		move_jump = false
		move_sprint = false
		move_walk = false
		move_crouch_toggle = false
		move_ledge_grab = false

func hitting_head():
	var ray = get_parent().get_world().direct_space_state.intersect_ray(get_parent().global_transform.origin, get_parent().global_transform.origin + Vector3(0, crouch_height_diff + 0.05, 0), [get_parent()])
	Debug.msg(!ray.empty(), "Hitting Head", "%s's movement" % get_parent().name)
	return !ray.empty()

func set_crouch(val):
	crouching = val

	if val:
		character_height -= crouch_height_diff

		get_node("../BodyShape").shape.height -= crouch_height_diff
		#get_node("../BodyShape").translation.y -= crouch_height_diff / 2
		get_node("../Head").translation.y -= crouch_height_diff / 2

		get_node("../BodyMesh").mesh.mid_height -= crouch_height_diff
		#get_node("../BodyMesh").translation.y -= crouch_height_diff / 2
		get_node("../HeadMesh").translation.y -= crouch_height_diff / 2
	else:
		character_height += crouch_height_diff

		get_node("../BodyShape").shape.height += crouch_height_diff
		#get_node("../BodyShape").translation.y += crouch_height_diff / 2
		get_node("../Head").translation.y += crouch_height_diff / 2

		get_node("../BodyMesh").mesh.mid_height += crouch_height_diff
		#get_node("../BodyMesh").translation.y += crouch_height_diff / 2
		get_node("../HeadMesh").translation.y += crouch_height_diff / 2

func check_ledge(check_only = false):
	var space = get_parent().get_world().direct_space_state
	var feetpos = get_parent().global_transform
	feetpos.origin -= Vector3(0, character_height / 2, 0)

	var min_height = ledge_grab_min_height
	if check_only:
		min_height = 0.0

	var heading = Vector2(get_parent().global_transform.basis.z.x, get_parent().global_transform.basis.z.z).angle_to(Vector2(-1, 0))

	#Check if there's actually a ledge to grab first, return false if there isn't
	var extents = Vector3(ledge_grab_horizontal, ledge_grab_max_height - min_height, ledge_grab_distance) / 2 #(hor, vert, depth)
	var shape_transform = feetpos
	shape_transform.origin += Vector3(ledge_grab_distance / 2, extents.y + min_height, 0).rotated(Vector3(0, 1, 0), heading)

	ledge_grab_query.transform = shape_transform
	ledge_grab_shape.extents = extents

	var intersect = space.intersect_shape(ledge_grab_query)

	if !ledge_grabbing or ledge_hold_time >= ledge_hold_duration:
		if check_only:
			Debug.draw_cube(shape_transform, extents * 2, "grabbounds", Color(1, 1, 0, 0.15))
		else:
			Debug.draw_cube(shape_transform, extents * 2, "grabbounds", Color(0, 0, 1, 0.15))

	var free = true

	if !intersect.empty():
		for ii in intersect:
			if ii.collider != self:
				free = false
				#Debug.msg(true, ii.collider.name, "Ledge", 1.05)
				break

	if free:
		return false
	elif check_only:
		return true

	shape_transform.origin += Vector3(0, (ledge_grab_max_height - ledge_grab_min_height + 0.2) / 2, 0)
	extents.y = 0.1

	ledge_grab_query.transform = shape_transform
	ledge_grab_shape.extents = extents

	intersect = space.intersect_shape(ledge_grab_query)

	if intersect.empty():
		Debug.draw_cube(shape_transform, extents * 2, "grabbounds", Color(0, 1, 0, 0.25))
	else:
		Debug.draw_cube(shape_transform, extents * 2, "grabbounds", Color(1, 0, 0, 0.25))
		return false

	var ray_pos = Vector3(get_parent().global_transform.origin.x, shape_transform.origin.y, get_parent().global_transform.origin.z)
	var ray = space.intersect_ray(get_parent().global_transform.origin, ray_pos, [self])

	if ray.empty():
		Debug.draw_ray(get_parent().global_transform.origin, ray_pos, "grabray", Color(0,1,0,1))
		return true
	else:
		Debug.draw_ray(get_parent().global_transform.origin, ray_pos, "grabray", Color(1,0,0,1))

	return false

func check_stair():
	var shape_safezone = 0.05
	var space = get_parent().get_world().direct_space_state
	var feetpos = get_parent().global_transform
	feetpos.origin -= Vector3(0, character_height / 2 - shape_safezone, 0)

	var feetray = space.intersect_ray(feetpos.origin + Vector3(0, 0.5, 0), feetpos.origin + Vector3(0, -1, 0), [get_parent()])
	Debug.draw_ray(feetpos.origin + Vector3(0, shape_safezone, 0), feetpos.origin + Vector3(0, -1, 0), "stairray", Color(1,0,1))

	var feetcollider
	if !feetray.empty():
		feetcollider = feetray.collider

		#print("%s Feet: %s" % [get_parent().name, feetray.collider.name])

	#First see if there's anything to check
	var extents = Vector3(stair_check_extent, stair_step_max_height - shape_safezone, stair_check_extent) / 2 #(hor, vert, depth)
	var shape_transform = feetpos
	shape_transform.origin += Vector3(0, stair_step_max_height / 2, 0)

	stair_check_query.transform = shape_transform
	stair_check_shape.extents = extents

	var intersect = space.intersect_shape(stair_check_query)

	if intersect.empty():
		Debug.draw_cube(shape_transform, extents * 2, "%sstairbounds1" % self, Color(1, 1, 1, 0.15))
		#print("%s No steps to check" % get_parent().name)
		return false

	var bodiestocheck = []
	for ii in intersect:
		bodiestocheck.append(ii.collider)

	#If there is, check if it's short enough
	extents = Vector3(stair_check_extent, character_height - stair_step_max_height - shape_safezone, stair_check_extent) / 2
	shape_transform.origin += Vector3(0, character_height / 2, 0)

	stair_check_query.transform = shape_transform
	stair_check_shape.extents = extents

	Debug.draw_cube(shape_transform, extents * 2, "%sstairbounds2" % self, Color(0, 1, 0, 0.15))

	var intersect2 = space.intersect_shape(stair_check_query)

	if !intersect2.empty():
		for ii2 in intersect2:
			while bodiestocheck.has(ii2.collider):
				bodiestocheck.erase(ii2.collider)

	if bodiestocheck.empty():
		Debug.draw_cube(shape_transform, extents * 2, "%sstairbounds3" % self, Color(1, 0, 0, 0.15))
		return false

	for bb in bodiestocheck:
		#print("Checking %s" % bb.name)
		if bb != feetcollider:
			if !stair_nodes.has(bb):
				var add = false
				if stair_detect_static and bb is StaticBody:
					add = true
				elif stair_detect_rigid and bb is RigidBody:
					add = true

				if add:
					get_parent().add_collision_exception_with(bb)
					stair_nodes.append(bb)

					if !known_stair_nodes.has(bb):
						known_stair_nodes.append(bb)

					if known_stair_nodes.has(feetcollider):
						is_on_stair = true
						on_stair_timer = on_stair_duration

					#Debug.msg(bb.name, "Added exception with", "%s's movement" % get_parent().name)

	return true

func clear_stairs():
	for ss in stair_nodes:
		get_parent().remove_collision_exception_with(ss)

	stair_nodes.clear()

func move_fly(delta):
	direction = Vector3()

	var aim = $Head.get_global_transform().basis

	if Input.is_action_pressed("move_forward"):
		direction -= aim.z
	if Input.is_action_pressed("move_backward"):
		direction += aim.z
	if Input.is_action_pressed("move_left"):
		direction -= aim.x
	if Input.is_action_pressed("move_right"):
		direction += aim.x
	if Input.is_action_pressed("move_jump"):
		direction += Vector3(0, 1, 0)

	direction = direction.normalized()

	var target = direction * FLY_SPEED

	velocity = velocity.linear_interpolate(target, FLY_ACCEL * delta)

	move_and_slide(velocity, Vector3(0, 1, 0))