extends CharacterBody3D


signal switch_to_ship

enum StatePlayer {NONE = 0, FLY = 1, SWIM = 2, WALK = 3, WALK_WATER=4}
enum AnimationSelected {IDLE = 0, RUN = 1, RUN_BACKWARD = 2, SITTING_GROUND_IDLE = 3,
						STRAFE_LEFT_WALK = 4, STRAFE_RIGHT_WALK = 5,
						WALK = 6, WALK_BACKWARD = 7}


# Constant
const SPEED_WALK_UP = 5.0
const SPEED_WALK_UP_STRAFE = 3.0
const SPEED_WALK_STRAFE = 2.5
const SPEED_WALK_DOWN_STRAFE = 0.6
const SPEED_WALK_DOWN = 0.8

const DIFF_RUN_WALK = 2.0
const SPEED_RUN_UP = SPEED_WALK_UP * DIFF_RUN_WALK
const SPEED_RUN_UP_STRAFE = SPEED_WALK_UP_STRAFE * DIFF_RUN_WALK
const SPEED_RUN_STRAFE = SPEED_WALK_STRAFE * DIFF_RUN_WALK
const SPEED_RUN_DOWN_STRAFE = SPEED_WALK_DOWN_STRAFE * DIFF_RUN_WALK
const SPEED_RUN_DOWN = SPEED_WALK_DOWN * DIFF_RUN_WALK

const ZOOM_STEP_Y = 0.05
const ZOOM_STEP_Z = 0.1
const ZOOM_MIN_Z = 0.2
const ZOOM_MAX_Z = 3.0
const ZOOM_DEFAULT = 1.0

const CAMERA_TPS_ROTATE_STEP_X = 0.5
const CAMERA_TPS_ROTATE_STEP_Y = 0.5

const CAMERA_FPS_ROTATE_STEP_X = 10.0
const CAMERA_FPS_ROTATE_STEP_Y = 10.0

const SPEED_ROTATE = PI
const PI_2 = PI / 2.0
const JUMP_FORCE = 4.5
const STEP_FORCE = 50.0

const MUL_SPEED_FLY = 2.0
const MUL_SPEED_SWIM = 0.5
const FACTOR_WALK = 1.0
const FACTOR_WALK_WATER = 0.3
const FACTOR_FLY = 2.0
const FACTOR_SWIM = 0.4
const MUL_SPEED_STEP = 0.2
const SPEED_COLLIDED = 0.0001

const MOVE_CAMERA_Y_STEP = 0.8
const MOVE_CAMERA_Y_MIN = -2.0
const MOVE_CAMERA_Y_MAX = 2.0
const MOVE_CAMERA_Y_OFFSET = 0.01

# Get the gravity from the project settings to be synced with RigidDynamicBody nodes.
var gravity = ProjectSettings.get_setting("physics/3d/default_gravity")
var starting_point = Vector2(DisplayServer.window_get_size().x / 2, DisplayServer.window_get_size().y / 2)
# Camera position
var camera_rotate_y = 0.0
var camera_rotate_x = 0.0
var camera_position_y = 0.0
# Player position
var player_rotate_y = 0.0
var player_rotate_x = 0.0
# Activate reconciliation between camera & player
var reconciliate_rotate_camera_player:bool = false
var move_camera_back_player:bool = false
# Player run
var is_run:bool = false
var zoom:float = ZOOM_DEFAULT
var tps:bool = true
var animation_object:AnimationPlayer = null

var anim_idle:String = "CHAR_idle_bored"
var anim_run:String = "CHAR_run"
var anim_run_backward:String = "CHAR_run backward"
var anim_sitting_ground_idle:String = "CHAR_sitting_ground_idle"
var anim_strafe_left_walk:String = "CHAR_strafe_left_walk"
var anim_strafe_right_walk:String = "CHAR_strafe_right_walk"
var anim_walk:String = "CHAR_walk"
var anim_walk_backward:String = "CHAR_walk_backward"

var anim_fly_idle:String = "CHAR_fly"
var anim_fly_run:String = "CHAR_fly"
var anim_fly_run_backward:String = "CHAR_fly"
var anim_fly_sitting_ground_idle:String = "CHAR_fly_idle"
var anim_fly_strafe_left_walk:String = "CHAR_fly"
var anim_fly_strafe_right_walk:String = "CHAR_fly"
var anim_fly_walk:String = "CHAR_fly"
var anim_fly_walk_backward:String = "CHAR_fly"

var anim_swim_idle:String = "CHAR_swim_idle"
var anim_swim_run:String = "CHAR_swim_slow"
var anim_swim_run_backward:String = "CHAR_swim_idle"
var anim_swim_sitting_ground_idle:String = "CHAR_swim_idle"
var anim_swim_strafe_left_walk:String = "CHAR_swim_idle"
var anim_swim_strafe_right_walk:String = "CHAR_swim_idle"
var anim_swim_walk:String = "CHAR_swim_slow"
var anim_swim_walk_backward:String = "CHAR_swim_idle"

var ANIM_WALK = [anim_idle, 
					anim_run,
					anim_run_backward,
					anim_sitting_ground_idle,
					anim_strafe_left_walk,
					anim_strafe_right_walk,
					anim_walk,
					anim_walk_backward
					]

var ANIM_FLY = [anim_fly_idle, 
					anim_fly_run,
					anim_fly_run_backward,
					anim_fly_sitting_ground_idle,
					anim_fly_strafe_left_walk,
					anim_fly_strafe_right_walk,
					anim_fly_walk,
					anim_fly_walk_backward
					]

var ANIM_SWIM = [anim_swim_idle, 
					anim_swim_run,
					anim_swim_run_backward,
					anim_swim_sitting_ground_idle,
					anim_swim_strafe_left_walk,
					anim_swim_strafe_right_walk,
					anim_swim_walk,
					anim_swim_walk_backward
					]

var current_anim:String = anim_idle
var player_sit:bool = false
var player_automove:bool = false

var state_player:StatePlayer = StatePlayer.WALK
var animation_selected:AnimationSelected = AnimationSelected.IDLE
var current_state:StatePlayer = StatePlayer.NONE
var current_animation_selected:AnimationSelected = AnimationSelected.IDLE
var level_water:float
var heigh_underwater_swim = 0.4
var heigh_underwater_walk = 1.0
var stop_move:bool = false
var max_collided_speed = 0.0
@onready var camera_fps:Camera3D = $camera_root/Camera3D_FPS_WALK
@onready var height_step:Vector3 = $RayCastStep.get_position() - $RayCastGround.get_position()

var on_ship:bool = false
var near_boat:Node3D = null

var jump_disabled:bool = false

# Use to calculate collision for camera
var distance_cam:float = 0.0
var angle_rotate_camera_x:float = sin(CAMERA_TPS_ROTATE_STEP_X)

# Action InProgress
var is_camera_move_up:bool = false
var is_camera_move_down:bool = false
var is_camera_move_left:bool = false
var is_camera_move_right:bool = false

var offset_camera_target:float =0.0
var offset_camera_current:float =0.0
var offset_camera_walk:float = 0.0
var offset_camera_fly:float = 0.0
var offset_camera_swim:float = 0.0


func switch_state(new_state):
	if state_player == new_state:
		return
	state_player = new_state
	match state_player:
		StatePlayer.WALK_WATER:
			Common.msg_debug("switch camera WALK (WALK_WATER)")
			camera_fps.rotate_x( -player_rotate_x )
			player_rotate_x = 0
			if get_viewport().get_camera_3d() == camera_fps:
				camera_fps = $camera_root/Camera3D_FPS_WALK
				camera_fps.make_current()
				tps = false
			else:
				camera_fps = $camera_root/Camera3D_FPS_WALK
			$CollisionWalk.disabled = false
			$CollisionFly.disabled = true
			$CollisionSwim.disabled = true
			offset_camera_target = offset_camera_walk
		StatePlayer.WALK:
			Common.msg_debug("switch camera WALK")
			camera_fps.rotate_x( -player_rotate_x )
			player_rotate_x = 0
			if get_viewport().get_camera_3d() == camera_fps:
				camera_fps = $camera_root/Camera3D_FPS_WALK
				camera_fps.make_current()
				tps = false
			else:
				camera_fps = $camera_root/Camera3D_FPS_WALK
			$CollisionWalk.disabled = false
			$CollisionFly.disabled = true
			$CollisionSwim.disabled = true
			offset_camera_target = offset_camera_walk
		StatePlayer.FLY:
			Common.msg_debug("switch camera FLY")
			camera_fps.rotate_x( -player_rotate_x )
			player_rotate_x = 0
			if get_viewport().get_camera_3d() == camera_fps:
				camera_fps = $camera_root/Camera3D_FPS_FLY
				camera_fps.make_current()
				tps = false
			else:
				camera_fps = $camera_root/Camera3D_FPS_FLY
			$CollisionWalk.disabled = true
			$CollisionFly.disabled = false
			$CollisionSwim.disabled = true
			offset_camera_target = offset_camera_fly
		StatePlayer.SWIM:
			Common.msg_debug("switch camera SWIM")
			camera_fps.rotate_x( -player_rotate_x )
			player_rotate_x = 0
			if get_viewport().get_camera_3d() == camera_fps:
				camera_fps = $camera_root/Camera3D_FPS_SWIM
				camera_fps.make_current()
				tps = false
			else:
				camera_fps = $camera_root/Camera3D_FPS_SWIM
			$CollisionWalk.disabled = true
			$CollisionFly.disabled = true
			$CollisionSwim.disabled = false
			offset_camera_target = offset_camera_swim


func search_animation( obj:Node , root:String = "/") -> bool:
	var ret:bool = false
	for i in obj.get_children():
		print("DEBUG - search_animation: " + root + String(i.get_name()))
		if i.get_name() == "AnimationPlayer" or i.get_name() == "animation_player":
			animation_object = i
			return true
		else:
			ret = search_animation(i, root + String(i.get_name()) + "/" )
			if ret == true:
				return ret
	return false


func print_list_animation():
	for key in animation_object.get_animation_list():
		print('Anim detected: ', key)


func switch_animation(name:String):
	#print(">", name)
	if name != current_anim and animation_object.has_animation(name):
		current_anim = name
		animation_object.play( name )
		if not animation_object.is_connected("animation_finished",  self._on_AnimationPlayer_animation_finished.bind(name)):
			animation_object.connect("animation_finished", self._on_AnimationPlayer_animation_finished.bind(name))


func select_animation(animation:AnimationSelected):
	if current_state != StatePlayer.NONE:
		if current_state == state_player and current_animation_selected == animation:
			return
	current_state = state_player
	current_animation_selected = animation
	match current_state:
		StatePlayer.WALK_WATER:
			switch_animation(ANIM_WALK[current_animation_selected])
		StatePlayer.WALK:
			switch_animation(ANIM_WALK[current_animation_selected])
		StatePlayer.FLY:
			switch_animation(ANIM_FLY[current_animation_selected])
		StatePlayer.SWIM:
			switch_animation(ANIM_SWIM[current_animation_selected])


func _on_AnimationPlayer_animation_finished(name:String):
	print("End animation: " , name)


func _init():
	print("wall_min_slide_angle: ", get_wall_min_slide_angle())
	print("get_floor_max_angle: ", get_floor_max_angle())
	print("get_floor_snap_length: ", get_floor_snap_length())
	# set_wall_min_slide_angle(1.0)
	#set_floor_max_angle(4.0)
	# set_floor_snap_length(0.4)
	pass

func offset_y(obj1:Node3D, obj2:Node3D):
	return obj2.get_global_transform().origin.y - obj1.get_global_transform().origin.y

func _ready():
	# Place the mouse at the center of the screen
	get_viewport().warp_mouse(starting_point)
	#$RayCastGround.rotate_x( -get_floor_max_angle() )
	search_animation($Mesh/character, "/")
	print_list_animation()
	switch_state(StatePlayer.WALK)
	offset_camera_fly = offset_y($camera_root/Camera3D_FPS_WALK, $camera_root/Camera3D_FPS_FLY)
	offset_camera_swim = offset_y($camera_root/Camera3D_FPS_WALK, $camera_root/Camera3D_FPS_SWIM)
	Common.msg_debug("offset_camera_fly: " + str(offset_camera_fly))
	Common.msg_debug("offset_camera_swim: " + str(offset_camera_swim))
	heigh_underwater_walk = $Water_Walk_Max_Level.get_global_transform().origin.y
	Common.msg_debug("heigh_underwater_walk: " + str(heigh_underwater_walk))
	heigh_underwater_swim = $Water_Swim_Max_Level.get_global_transform().origin.y
	Common.msg_debug("heigh_underwater_swim: " + str(heigh_underwater_swim))
#	var obj1 = $camera_root/horizontal_root/vertical_root/Camera3D_TPS/MouseUp.get_global_transform().origin
#	var obj2 = $camera_root.get_global_transform().origin
#	distance_cam = obj2.distance_to(obj1)
#	Common.msg_debug("distance: " + str(distance_cam))
#	Common.msg_debug(str(sin(CAMERA_TPS_ROTATE_STEP_X)))
#	update_collision_exception()


func update_collision(target:RayCast3D, obj:Node3D):
	Common.msg_debug("child:" + str(obj))
	target.add_exception(obj)
	for child in obj.get_children():
		if child is Node3D:
			update_collision(target, child)


func update_collision_exception():
	var target:RayCast3D = $camera_root/horizontal_root/vertical_root/Camera3D_TPS/MouseColl
	target.clear_exceptions()
	target.add_exception($CollisionWalk)
	update_collision(target, $Mesh)
	target.force_raycast_update()
#	target.add_exception($Mesh)
#	for child in $Mesh.get_children():
#		Common.msg_debug("child:" + str(child))
#		target.add_exception(child)


func get_distance(obj1: Node3D, obj2: Node3D) -> float:
	return  obj1.get_global_transform().origin.distance_to(obj2.get_global_transform().origin)


func get_distance_vector(obj1: Vector3, obj2: Vector3) -> float:
	return  obj1.distance_to(obj2)


func _input(event):
	if tps:
		if Input.is_action_pressed("INPUT_VIEW_FRONT", true) and tps:
			# Move camera position on front
			switch_camera_tps_front()
		elif Input.is_action_pressed("INPUT_VIEW_BACK", true) and tps:
			# Move camera position on back
			switch_camera_tps_back()
		if Input.is_action_pressed("INPUT_VIEW_ZOOM_IN", true):
			camera_zoom_in()
		elif Input.is_action_pressed("INPUT_VIEW_ZOOM_OUT", true):
			camera_zoom_out()
		if Input.is_action_just_pressed("INPUT_VIEW_CAMERA_FPS_TPS", true):
			switch_camera_fps()
		if Input.is_action_just_pressed("INPUT_VIEW_CAMERA_MOVE_PLAYER_FOLLOW"):
			# Mouse Button Right (start)
			pass
		elif Input.is_action_just_released("INPUT_VIEW_CAMERA_MOVE_PLAYER_FOLLOW"):
			# Mouse Button Right (stop)
			pass
		elif Input.is_action_just_pressed("INPUT_VIEW_CAMERA_MOVE_ONLY"):
			# Mouse Button Left (start)
			pass
		elif Input.is_action_just_released("INPUT_VIEW_CAMERA_MOVE_ONLY"):
			# Mouse Button Left (stop)
			pass
		elif Input.is_action_pressed("INPUT_VIEW_CAMERA_MOVE_PLAYER_FOLLOW") or Input.is_action_pressed("INPUT_VIEW_CAMERA_MOVE_ONLY"):
			if event is InputEventMouseMotion:
				move_camera(event.relative.y * 0.01 , - event.relative.x *0.01)
		if Input.is_action_just_pressed("INPUT_VIEW_CAMERA_MOVE_PLAYER_FOLLOW"):
			reconciliate_rotate_camera_player = true
			Input.set_mouse_mode(Input.MOUSE_MODE_HIDDEN)
		elif Input.is_action_just_released("INPUT_VIEW_CAMERA_MOVE_PLAYER_FOLLOW"):
			# reconciliate_rotate_camera_player = false # continue move camera, and stop when it's arrive in final position
			Input.set_mouse_mode(Input.MOUSE_MODE_VISIBLE)
	else:
		if Input.is_action_pressed("INPUT_VIEW_CAMERA_MOVE_PLAYER_FOLLOW") or Input.is_action_pressed("INPUT_VIEW_CAMERA_MOVE_ONLY"):
			if event is InputEventMouseMotion:
				camera_rotate_y = player_rotate_y
				player_rotate_y += event.relative.x *0.01
				if player_rotate_y > PI:
					player_rotate_y -= TAU
				elif player_rotate_y <= -PI:
					player_rotate_y += TAU
				rotate_y( -event.relative.x *0.01 )
				var new_camera_rotate_x = player_rotate_x + event.relative.y * 0.01
				if new_camera_rotate_x <= PI_2 and new_camera_rotate_x >= - PI_2:
					camera_fps.rotate_x( event.relative.y * 0.01 )
					player_rotate_x = new_camera_rotate_x
		if Input.is_action_pressed("INPUT_VIEW_ZOOM_OUT", true):
			camera_zoom_out()
		elif Input.is_action_just_pressed("INPUT_VIEW_CAMERA_FPS_TPS", true):
			switch_camera_tps()

	# Action on both state FPS or TPS
	if Input.is_action_just_pressed("INPUT_ACTION_SIT_DOWN", true):
		player_sit = not player_sit
	elif Input.is_action_just_pressed("INPUT_ACTION_AUTO_UP", true):
		player_automove = not player_automove
	elif Input.is_action_just_pressed("INPUT_ACTION_FLY", true):
		match state_player:
			StatePlayer.WALK:
				switch_state(StatePlayer.FLY)
			StatePlayer.FLY:
				switch_state(StatePlayer.WALK)
	elif Input.is_action_just_released("INPUT_ACTION_RUN", true):
		is_run = !is_run


func _input_in_ship(event):
	if Input.is_action_just_pressed("INPUT_ACTION_ENTER_VEHICLE"):
		on_ship = false
		player_sit = false
		return


func switch_camera_fps():
	#Common.msg_debug("switch_camera_fps")
	camera_fps.rotate_x( -player_rotate_x )
	player_rotate_x = 0
	camera_fps.make_current()
	tps = false
	reconciliate_rotate_camera_player = false
	Input.set_mouse_mode(Input.MOUSE_MODE_VISIBLE)
	$camera_root/horizontal_root.rotate_y( player_rotate_y - camera_rotate_y )
	camera_rotate_y = player_rotate_y


func switch_camera_tps():
	#Common.msg_debug("switch_camera_tps")
	tps = true
	reconciliate_rotate_camera_player = true
	# Recover rotate Y
	$camera_root/horizontal_root.rotate_y( player_rotate_y - camera_rotate_y )
	camera_rotate_y = player_rotate_y
	# Recover rotate X
	$camera_root/horizontal_root/vertical_root.rotate_x( player_rotate_x - camera_rotate_x )
	camera_rotate_x = player_rotate_x
	# Recover zoom
	var zoomorign = zoom
	var curzoom = zoom - ZOOM_MIN_Z
	var fac = curzoom / ZOOM_STEP_Z
	var zoom3D:Vector3 = Vector3(0.0, -fac * ZOOM_STEP_Y, -curzoom)
	zoom = ZOOM_MIN_Z
	$camera_root/horizontal_root/vertical_root/Camera3D_TPS.translate_object_local(zoom3D)
	# Position Camera on Y
	var pos:Vector3 = $camera_root.get_position()
	pos.y -= camera_position_y
	$camera_root.set_position( pos )
	camera_position_y = 0.0
	# Move slowly the camera to detect collision (and stop camera)
	$camera_root/horizontal_root/vertical_root/Camera3D_TPS.make_current()
	var last_zoom = 0.0
	while zoom < zoomorign and  zoom != last_zoom:
		last_zoom = zoom
		camera_zoom_out()


func switch_camera_tps_front():
	# Recover rotate Y
	$camera_root/horizontal_root.rotate_y( player_rotate_y - camera_rotate_y )
	camera_rotate_y = player_rotate_y
	# Recover rotate X
	$camera_root/horizontal_root/vertical_root.rotate_x( player_rotate_x - camera_rotate_x )
	camera_rotate_x = player_rotate_x
	# Recover zoom
	var curzoom = zoom - ZOOM_MIN_Z
	var fac = curzoom / ZOOM_STEP_Z
	var zoom3D:Vector3 = Vector3(0.0, -fac * ZOOM_STEP_Y, -curzoom)
	zoom = ZOOM_MIN_Z
	$camera_root/horizontal_root/vertical_root/Camera3D_TPS.translate_object_local(zoom3D)
	# Position Camera on Y
	var pos:Vector3 = $camera_root.get_position()
	pos.y -= camera_position_y
	$camera_root.set_position( pos )
	camera_position_y = 0.0
	# Move slowly the camera to detect collision (and stop camera)
	var last_zoom = 0.0
	while zoom < ZOOM_DEFAULT and  zoom != last_zoom:
		last_zoom = zoom
		camera_zoom_out()


func switch_camera_tps_back():
	$camera_root/horizontal_root.rotate_y( player_rotate_y - camera_rotate_y + PI )
	camera_rotate_y = player_rotate_y + PI
	if camera_rotate_y > PI:
		camera_rotate_y -= TAU
	elif camera_rotate_y <= -PI:
		camera_rotate_y += TAU
	# Recover rotate X
	$camera_root/horizontal_root/vertical_root.rotate_x( player_rotate_x - camera_rotate_x )
	camera_rotate_x = player_rotate_x
	# Recover zoom
	var curzoom = zoom - ZOOM_MIN_Z
	var fac = curzoom / ZOOM_STEP_Z
	var zoom3D:Vector3 = Vector3(0.0, -fac * ZOOM_STEP_Y, -curzoom)
	zoom = ZOOM_MIN_Z
	$camera_root/horizontal_root/vertical_root/Camera3D_TPS.translate_object_local(zoom3D)
	# Position Camera on Y
	var pos:Vector3 = $camera_root.get_position()
	pos.y -= camera_position_y
	$camera_root.set_position( pos )
	camera_position_y = 0.0
	# Move slowly the camera to detect collision (and stop camera)
	var last_zoom = 0.0
	while zoom < ZOOM_DEFAULT and zoom != last_zoom:
		last_zoom = zoom
		camera_zoom_out()


func camera_zoom_in():
	if zoom > ZOOM_MIN_Z:
		zoom -= ZOOM_STEP_Z
		var zoom3D:Vector3 = Vector3(0.0, -ZOOM_STEP_Y, -ZOOM_STEP_Z)
		$camera_root/horizontal_root/vertical_root/Camera3D_TPS.translate_object_local(zoom3D)
		#detect_camera_collision()
	if zoom < ZOOM_MIN_Z:
		Common.msg_debug("camera_zoom_in:" + str(zoom) + " < " + str(ZOOM_MIN_Z))
		switch_camera_fps()


func camera_zoom_out():
	if not tps:
		switch_camera_tps()
	
	var coll:RayCast3D = $camera_root/horizontal_root/vertical_root/Camera3D_TPS/Back
	coll.force_raycast_update()
	if ! coll.is_colliding():
		zoom += ZOOM_STEP_Z
		if zoom >= ZOOM_MAX_Z:
			zoom = ZOOM_MAX_Z
		else:
			var zoom3D:Vector3 = Vector3(0.0, ZOOM_STEP_Y, ZOOM_STEP_Z)
			$camera_root/horizontal_root/vertical_root/Camera3D_TPS.translate_object_local(zoom3D)
		#detect_camera_collision()


func loop_collision_x(col:RayCast3D, maxloop:int, angle_x:float ):
	var maxloop_origin = maxloop
	col.force_raycast_update()
	if not col.is_colliding():
		return
	while maxloop > 0:
		var new_camera_rotate_x = camera_rotate_x + angle_x
		if angle_x != 0.0 and new_camera_rotate_x <= PI_2 and new_camera_rotate_x >= - PI_2:
			$camera_root/horizontal_root/vertical_root.rotate_x( angle_x )
			camera_rotate_x = new_camera_rotate_x
		maxloop -= 1
		col.force_raycast_update()
		if not col.is_colliding():
			loop_collision_x_step_2(col, 100, -angle_x/100.0)
			return


func loop_collision_x_step_2(col:RayCast3D, maxloop:int, angle_x:float ):
	var maxloop_origin = maxloop
	col.force_raycast_update()
	if col.is_colliding():
		return
	while maxloop > 0:
		var new_camera_rotate_x = camera_rotate_x + angle_x
		if angle_x != 0.0 and new_camera_rotate_x <= PI_2 and new_camera_rotate_x >= - PI_2:
			$camera_root/horizontal_root/vertical_root.rotate_x( angle_x )
			camera_rotate_x = new_camera_rotate_x
		maxloop -= 1
		col.force_raycast_update()
		if col.is_colliding():
			loop_collision_x_step_3(col, 10, -angle_x/10.0)
			return


func loop_collision_x_step_3(col:RayCast3D, maxloop:int, angle_x:float ):
	var maxloop_origin = maxloop
	col.force_raycast_update()
	if not col.is_colliding():
		return
	while maxloop > 0:
		var new_camera_rotate_x = camera_rotate_x + angle_x
		if angle_x != 0.0 and new_camera_rotate_x <= PI_2 and new_camera_rotate_x >= - PI_2:
			$camera_root/horizontal_root/vertical_root.rotate_x( angle_x )
			camera_rotate_x = new_camera_rotate_x
		maxloop -= 1
		col.force_raycast_update()
		if not col.is_colliding():
			return


func loop_collision_y(col:RayCast3D, maxloop:int, angle_y:float ):
	var maxloop_origin = maxloop
	var new_camera_rotate_y
	col.force_raycast_update()
	if not col.is_colliding():
		return
	if tps:
		var diff = camera_rotate_y - player_rotate_y
		if diff <= 0.01 and diff >= -0.01:
			move_camera_back_player = true
	while maxloop > 0:
		new_camera_rotate_y = camera_rotate_y + angle_y
		if new_camera_rotate_y > PI:
			new_camera_rotate_y -= TAU
		elif new_camera_rotate_y <= -PI:
			new_camera_rotate_y += TAU
		$camera_root/horizontal_root.rotate_y( angle_y )
		camera_rotate_y = new_camera_rotate_y
		maxloop -= 1
		col.force_raycast_update()
		if not col.is_colliding():
			loop_collision_y_step_2(col, 20, -angle_y/10.0)
			return

func loop_collision_y_step_2(col:RayCast3D, maxloop:int, angle_y:float ):
	var maxloop_origin = maxloop
	col.force_raycast_update()
	if col.is_colliding():
		return
	while maxloop > 0:
		var new_camera_rotate_y = camera_rotate_y + angle_y
		if new_camera_rotate_y > PI:
			new_camera_rotate_y -= TAU
		elif new_camera_rotate_y <= -PI:
			new_camera_rotate_y += TAU
		$camera_root/horizontal_root.rotate_y( angle_y )
		camera_rotate_y = new_camera_rotate_y
		maxloop -= 1
		col.force_raycast_update()
		if col.is_colliding():
			loop_collision_y_step_3(col, 10, -angle_y/10.0)
			return

func loop_collision_y_step_3(col:RayCast3D, maxloop:int, angle_y:float ):
	var maxloop_origin = maxloop
	col.force_raycast_update()
	if not col.is_colliding():
		return
	while maxloop > 0:
		var new_camera_rotate_y = camera_rotate_y + angle_y
		if new_camera_rotate_y > PI:
			new_camera_rotate_y -= TAU
		elif new_camera_rotate_y <= -PI:
			new_camera_rotate_y += TAU
		$camera_root/horizontal_root.rotate_y( angle_y )
		camera_rotate_y = new_camera_rotate_y
		maxloop -= 1
		col.force_raycast_update()
		if not col.is_colliding():
			return


func detect_camera_collision_x(angle_x):
	var dis = get_distance($camera_root/horizontal_root/vertical_root, $camera_root/horizontal_root/vertical_root/Camera3D_TPS)
	if angle_x >= 0.0:
		var coll:RayCast3D = $camera_root/horizontal_root/vertical_root/Camera3D_TPS/Up
		var dey:float = dis * sin(angle_x) 
		#Common.msg_debug("dis:" + str(dis) + " angle_x:" + str(angle_x) + " deY:" + str(dey))
		var pos:Vector3 = Vector3(0.0, dey + 0.2 , 0.0)
		coll.set_target_position(pos)
		loop_collision_x(coll, 20, -0.01 )
	if angle_x <= 0.0:
		var coll:RayCast3D = $camera_root/horizontal_root/vertical_root/Camera3D_TPS/Down
		var dey:float = dis * sin(angle_x)
		#Common.msg_debug("dis:" + str(dis) + " angle_x:" + str(angle_x) + " deY:" + str(dey))
		var pos:Vector3 = Vector3(0.0, dey - 0.2 , 0.0)
		coll.set_target_position(pos)
		loop_collision_x(coll, 20, +0.01 )


func detect_camera_collision_y(angle_y):
	var dis = get_distance($camera_root/horizontal_root/vertical_root, $camera_root/horizontal_root/vertical_root/Camera3D_TPS)
	if angle_y >= 0.0:
		var coll:RayCast3D = $camera_root/horizontal_root/vertical_root/Camera3D_TPS/Right
		var dey:float = dis * sin(angle_y) 
		#Common.msg_debug("dis:" + str(dis) + " angle_y:" + str(angle_y) + " deY:" + str(dey))
		var pos:Vector3 = Vector3(dey + 0.2, 0.0 , 0.0)
		coll.set_target_position(pos)
		loop_collision_y(coll, 20, -0.01 )
	if angle_y <= 0.0:
		var coll:RayCast3D = $camera_root/horizontal_root/vertical_root/Camera3D_TPS/Left
		var dey:float = dis * sin(angle_y)
		#Common.msg_debug("dis:" + str(dis) + " angle_y:" + str(angle_y) + " deY:" + str(dey))
		var pos:Vector3 = Vector3(dey - 0.2, 0.0 , 0.0)
		coll.set_target_position(pos)
		loop_collision_y(coll, 20, +0.01 )


func rotate_camera_y(angle_y):
	var coll:RayCast3D
	if angle_y >= 0.0:
		coll = $camera_root/horizontal_root/vertical_root/Camera3D_TPS/Right
	else:
		coll = $camera_root/horizontal_root/vertical_root/Camera3D_TPS/Left
	if coll.is_colliding():
		return
	camera_rotate_y -= angle_y
	if camera_rotate_y > PI:
		camera_rotate_y -= TAU
	elif camera_rotate_y <= -PI:
		camera_rotate_y += TAU
	$camera_root/horizontal_root.rotate_y( -angle_y )
	detect_camera_collision_y(-angle_y)


func move_camera(angle_x, angle_y):
	var new_camera_rotate_x = camera_rotate_x + angle_x
	if angle_x != 0.0 and new_camera_rotate_x <= PI_2 and new_camera_rotate_x >= - PI_2:
		$camera_root/horizontal_root/vertical_root.rotate_x( angle_x )
		camera_rotate_x = new_camera_rotate_x
		detect_camera_collision_x(angle_x)
	rotate_camera_y(angle_y)


func move_camera_y(delta_y:float):
	var posTarget:Vector3 = $camera_root.get_position()
	if delta_y >= 0.0:
		var coll:RayCast3D = $camera_root/horizontal_root/vertical_root/Camera3D_TPS/Up
		var pos:Vector3 = Vector3(0.0, delta_y+0.2 , 0.0)
		coll.set_target_position(pos)
		coll.force_raycast_update()
		if coll.is_colliding():
			return true
	else:
		var coll:RayCast3D = $camera_root/horizontal_root/vertical_root/Camera3D_TPS/Down
		var pos:Vector3 = Vector3(0.0, delta_y-0.2 , 0.0)
		coll.set_target_position(pos)
		coll.force_raycast_update()
		if coll.is_colliding():
			return true
	posTarget.y += delta_y
	camera_position_y += delta_y
	$camera_root.set_position(posTarget)
	detect_camera_collision_y(delta_y)
	return false


func move_camera_y_by_step(delta_y:float, step_y:float):
	var cur:float = 0.0
	var coll:bool = false
	if delta_y >= 0.0:
		if delta_y < step_y:
			step_y = delta_y
			coll = move_camera_y(step_y)
			cur += step_y
		else:
			while !coll and cur < delta_y + step_y:
				coll = move_camera_y(step_y)
				cur += step_y
			if delta_y - cur < step_y:
				step_y = delta_y - cur
				coll = move_camera_y(step_y)
				cur += step_y
	else:
		if delta_y > -step_y:
			step_y = delta_y
			coll = move_camera_y(step_y)
			cur += step_y
		else:
			while !coll and cur > delta_y - step_y:
				coll = move_camera_y(-step_y)
				cur -= step_y
			if delta_y - cur > -step_y:
				step_y = delta_y - cur
				coll = move_camera_y(step_y)
				cur += step_y
	return cur


func move_camera_y_one_step(delta_y:float, step_y:float):
	var cur:float = 0.0
	var coll:bool = false
	if delta_y >= 0.0:
		if delta_y < step_y:
			step_y = delta_y
			coll = move_camera_y(step_y)
			cur += step_y
		else:
			if !coll:
				coll = move_camera_y(step_y)
				cur += step_y
			elif delta_y - cur < step_y:
				step_y = delta_y - cur
				coll = move_camera_y(step_y)
				cur += step_y
	else:
		if delta_y > -step_y:
			step_y = delta_y
			coll = move_camera_y(step_y)
			cur += step_y
		else:
			if !coll:
				coll = move_camera_y(-step_y)
				cur -= step_y
			elif delta_y - cur > -step_y:
				step_y = delta_y - cur
				coll = move_camera_y(step_y)
				cur += step_y
	return cur


func _physics_process(delta):
	if (state_player == StatePlayer.WALK or state_player == StatePlayer.WALK_WATER) and (not is_on_floor()):
		velocity.y -= gravity * delta
	if tps:
		var camx = (Input.get_action_strength("INPUT_VIEW_CAMERA_UP", true) - Input.get_action_strength("INPUT_VIEW_CAMERA_DOWN", true)) * CAMERA_TPS_ROTATE_STEP_X * delta
		var camy = (Input.get_action_strength("INPUT_VIEW_CAMERA_LEFT", true) - Input.get_action_strength("INPUT_VIEW_CAMERA_RIGHT", true)) * CAMERA_TPS_ROTATE_STEP_Y * delta
		move_camera(camx, camy)
		var delta_y = (Input.get_action_strength("INPUT_VIEW_UP", true) -  Input.get_action_strength("INPUT_VIEW_DOWN", true)) * MOVE_CAMERA_Y_STEP * delta
		#Common.msg_debug("step y:" + str(delta_y))
		if delta_y != 0.0 and camera_position_y + delta_y <= MOVE_CAMERA_Y_MAX and camera_position_y + delta_y >= MOVE_CAMERA_Y_MIN:
			# move_camera_y(delta_y)
			move_camera_y_by_step(delta_y, 0.1 )
			# var cur = move_camera_y_by_step(delta_y, 0.1 )
			# Common.msg_debug("cur:" + str(cur))
	else:
		var camx = (Input.get_action_strength("INPUT_VIEW_CAMERA_UP", true) - Input.get_action_strength("INPUT_VIEW_CAMERA_DOWN", true)) * CAMERA_FPS_ROTATE_STEP_X * delta
		var camy = (Input.get_action_strength("INPUT_VIEW_CAMERA_LEFT", true) - Input.get_action_strength("INPUT_VIEW_CAMERA_RIGHT", true)) * CAMERA_FPS_ROTATE_STEP_Y * delta
		var dy = camy * delta * SPEED_ROTATE
		player_rotate_y += dy
		if player_rotate_y > PI:
			player_rotate_y -= TAU
		elif player_rotate_y <= -PI:
			player_rotate_y += TAU
		camera_rotate_y = player_rotate_y
		rotate_y( dy )
		var dx = - camx * delta * SPEED_ROTATE
		var new_camera_rotate_x = player_rotate_x + dx
		if new_camera_rotate_x <= PI_2 and new_camera_rotate_x >= - PI_2:
			camera_fps.rotate_x( dx )
			player_rotate_x = new_camera_rotate_x
		if state_player == StatePlayer.WALK_WATER:
			if get_position().y <= level_water - heigh_underwater_walk:
				Common.msg_debug("get_position.y:" + str(get_position().y) + " <= [heigh_underwater_walk] " + str( level_water - heigh_underwater_walk))
				switch_state(StatePlayer.SWIM)
				return
		if (state_player == StatePlayer.WALK or state_player == StatePlayer.WALK_WATER):
			pass


func _process( delta ):
	var input_y: float = 0.0
	var input_x: float = 0.0
	var input_z: float = 0.0
	var move_up:bool = false
	var move_down:bool = false
	var move_strafe: bool = false
	var mulstep: float = 1.0
	var speed: float
	var y = 0
	var animation_selected:AnimationSelected = AnimationSelected.IDLE

	if offset_camera_current != offset_camera_target:
		var offset_diff = move_camera_y_one_step( offset_camera_target - offset_camera_current, MOVE_CAMERA_Y_OFFSET)
		offset_camera_current += offset_diff
	if reconciliate_rotate_camera_player:
		var diff = camera_rotate_y - player_rotate_y
		if not Input.is_action_pressed("INPUT_VIEW_CAMERA_MOVE_PLAYER_FOLLOW"):
			if diff <= 0.01 and diff >= -0.01:
				reconciliate_rotate_camera_player = false
				return
		if diff > PI:
			diff = camera_rotate_y - player_rotate_y - TAU
		elif diff < -PI:
			diff = camera_rotate_y - player_rotate_y + TAU
		var absdiff = diff
		if absdiff < 0.0:
			absdiff = -absdiff
		if absdiff <= 0.5 * delta:
			$Mesh.rotate_y( diff )
			$CollisionFly.rotate_y( diff )
			$CollisionFlyIdle.rotate_y( diff )
			$CollisionSwim.rotate_y( diff )
			$CollisionWalk.rotate_y( diff )
			$camera_root.rotate_y( diff )
			$RayCastGround.rotate_y( diff )
			$RayCastStep.rotate_y( diff )
			$camera_root/horizontal_root.rotate_y( -diff )
			player_rotate_y = camera_rotate_y
		else:
			if diff >= 0.0:
				diff = delta * SPEED_ROTATE
			else:
				diff = -delta * SPEED_ROTATE
			self.rotate_y( diff )
			$camera_root/horizontal_root.rotate_y( -diff )
			player_rotate_y += diff
			if player_rotate_y > PI:
				player_rotate_y -= TAU
			elif player_rotate_y <= -PI:
				player_rotate_y += TAU
#	elif move_camera_back_player:
#		var diff:float = camera_rotate_y - player_rotate_y
#		var step:float = 0.0
#		if diff < 0.0:
#			step = +0.1
#		else:
#			step = -0.1
#		rotate_camera_y(step)
#		diff = camera_rotate_y - player_rotate_y
#		if diff < 0.0:
#			diff = -diff
#		if diff < 0.01:
#			move_camera_back_player = false
	if Input.is_action_pressed("INPUT_ACTION_RIGHT"):
		y -= 1
	if Input.is_action_pressed("INPUT_ACTION_LEFT"):
		y += 1
	if y != 0:
		var angle_y = y * delta * SPEED_ROTATE
		if tps:
			if reconciliate_rotate_camera_player:
				camera_rotate_y += angle_y
				if camera_rotate_y > PI:
					camera_rotate_y -= TAU
				elif camera_rotate_y <= -PI:
					camera_rotate_y += TAU
				$camera_root/horizontal_root.rotate_y( angle_y )
				detect_camera_collision_y( angle_y)
			else:
				camera_rotate_y += angle_y
				if camera_rotate_y > PI:
					camera_rotate_y -= TAU
				elif camera_rotate_y <= -PI:
					camera_rotate_y += TAU
				player_rotate_y += angle_y
				if player_rotate_y > PI:
					player_rotate_y -= TAU
				elif player_rotate_y <= -PI:
					player_rotate_y += TAU
				rotate_y( angle_y )
				detect_camera_collision_y( angle_y)
		else:
			player_rotate_y += angle_y
			if player_rotate_y > PI:
				player_rotate_y -= TAU
			elif player_rotate_y <= -PI:
				player_rotate_y += TAU
			camera_rotate_y = player_rotate_y
			rotate_y( angle_y )
			#detect_camera_collision_y( angle_y) # Ignore detection, will try to reposition after 
	if Input.is_action_pressed("INPUT_ACTION_UP") or (Input.is_action_pressed("INPUT_VIEW_CAMERA_MOVE_PLAYER_FOLLOW") and Input.is_action_pressed("INPUT_VIEW_CAMERA_MOVE_ONLY")):
		input_y = 1.0
		input_z = 1.0
		move_up = true
	elif Input.is_action_pressed("INPUT_ACTION_DOWN"):
		player_automove = false
		input_y = -1.0
		input_z = -1.0
		move_down = true
	elif player_automove:
		input_y = 1.0
		input_z = 1.0
		move_up = true
	if Input.is_action_pressed("INPUT_ACTION_STRAFE_LEFT"):
		input_x = 1.0
		move_strafe = true
	elif Input.is_action_pressed("INPUT_ACTION_STRAFE_RIGHT"):
		input_x = -1.0
		move_strafe = true
	else:
		input_x = 0.0

	if state_player == StatePlayer.WALK_WATER or state_player == StatePlayer.WALK:
		input_z = 0.0
	else:
		if tps:
			input_z *= (-camera_rotate_x)
		else:
			input_z *= (-player_rotate_x)

	if is_run:
		if move_strafe:
			if move_up:
				speed = SPEED_RUN_UP_STRAFE
				animation_selected = AnimationSelected.RUN
			elif move_down:
				speed = SPEED_RUN_DOWN_STRAFE
				animation_selected = AnimationSelected.RUN_BACKWARD
			else:
				speed = SPEED_RUN_STRAFE
				if input_x > 0.0:
					animation_selected = AnimationSelected.STRAFE_LEFT_WALK
				else:
					animation_selected = AnimationSelected.STRAFE_RIGHT_WALK
		elif move_up:
			speed = SPEED_RUN_UP
			animation_selected = AnimationSelected.RUN
		else:
			speed = SPEED_RUN_DOWN
			if move_down:
				animation_selected = AnimationSelected.RUN_BACKWARD
			else:
				animation_selected = AnimationSelected.IDLE
	else:
		if move_strafe:
			if move_up:
				speed = SPEED_WALK_UP_STRAFE
				animation_selected = AnimationSelected.WALK
			elif move_down:
				speed = SPEED_WALK_DOWN_STRAFE
				animation_selected = AnimationSelected.WALK_BACKWARD
			else:
				speed = SPEED_WALK_STRAFE
				if input_x > 0.0:
					animation_selected = AnimationSelected.STRAFE_LEFT_WALK
				else:
					animation_selected = AnimationSelected.STRAFE_RIGHT_WALK
		elif move_up:
			speed = SPEED_WALK_UP
			animation_selected = AnimationSelected.WALK
		else:
			speed = SPEED_WALK_DOWN
			if move_down:
				animation_selected = AnimationSelected.WALK_BACKWARD
			else:
				animation_selected = AnimationSelected.IDLE
	if input_x == 0.0 and input_y == 0.0:
		if player_sit:
			animation_selected = AnimationSelected.SITTING_GROUND_IDLE
		else:
			animation_selected = AnimationSelected.IDLE
	match state_player:
		StatePlayer.WALK_WATER:
			Common.msg_debug("WALK_WATER ** level_water: " + str(level_water) + " get_position.y:" + str(get_position().y) + " > [heigh_underwater_walk] " + str( level_water - heigh_underwater_walk))
			if get_position().y > level_water - heigh_underwater_walk:
				#Common.msg_debug("get_position.y:" + str(get_position().y) + " > [heigh_underwater_walk] " + str( level_water - heigh_underwater_walk))
				mulstep = FACTOR_WALK_WATER
				if not is_on_floor():
					velocity.y -= gravity * delta
			else:
				switch_state(StatePlayer.SWIM)
				mulstep = FACTOR_SWIM
		StatePlayer.WALK:
			Common.msg_debug("WALK")
			mulstep = FACTOR_WALK
			if not is_on_floor():
				velocity.y -= gravity * delta
		StatePlayer.FLY:
			#Common.msg_debug("FLY")
			mulstep = FACTOR_FLY
		StatePlayer.SWIM:
			Common.msg_debug("SWIM - get_position.y:" + str(get_position().y) + " >= [heigh_underwater_swim] " + str( level_water - heigh_underwater_swim))
			if is_on_floor() and get_position().y >= level_water - heigh_underwater_swim:
				#Common.msg_debug("get_position.y:" + str(get_position().y) + " >= [heigh_underwater_swim] " + str( level_water - heigh_underwater_swim))
				switch_state(StatePlayer.WALK_WATER)
				mulstep = FACTOR_WALK_WATER
			else:
				mulstep = FACTOR_SWIM
	select_animation(animation_selected)

	var direction = (transform.basis * Vector3(input_x, input_z, input_y)).normalized()
	if direction:
		velocity.x = direction.x * speed * mulstep
		velocity.z = direction.z * speed * mulstep
		if not (state_player == StatePlayer.WALK_WATER or state_player == StatePlayer.WALK):
			velocity.y = direction.y * speed * mulstep
	else:
		velocity.x = move_toward(velocity.x, 0, speed * mulstep)
		velocity.z = move_toward(velocity.z, 0, speed * mulstep)
		if not (state_player == StatePlayer.WALK_WATER or state_player == StatePlayer.WALK):
			velocity.y = move_toward(velocity.y, 0, speed * mulstep)
	var save_velocity:Vector3 = velocity
	var collided = move_and_slide()
	if state_player == StatePlayer.SWIM:
		if collided == false and level_water <= get_position().y:
			Common.msg_debug("get_position.y:" + str(get_position().y) + " >= [0] " + str( level_water))
			# force always under water (if not collision)
			var tmp = get_position()
			tmp.y = level_water
			set_position(tmp)
		elif collided == true and get_position().y >= level_water - heigh_underwater_swim:
			Common.msg_debug("get_position.y:" + str(get_position().y) + " >= [heigh_underwater_swim] " + str( level_water - heigh_underwater_swim))
			switch_state(StatePlayer.WALK_WATER)
			if not is_on_floor():
				velocity.y -= gravity * delta
	elif state_player == StatePlayer.WALK:
		if collided and is_on_floor() and (input_x != 0.0 or input_y != 0.0) and !$RayCastStep.is_colliding():
			var delta_1:Vector3 = get_position_delta()
			var areawalk = delta_1.x * delta_1.x + delta_1.z * delta_1.z
			if (areawalk * 10.0 < max_collided_speed) or (is_run and (areawalk * 10.0 < max_collided_speed * DIFF_RUN_WALK )):
				var jump_step:Vector3 = height_step
				translate(jump_step)
				velocity = save_velocity - delta_1 # (save_velocity - delta_1) * MUL_SPEED_STEP
				move_and_slide()
				velocity.x = 0.0
				velocity.y = -STEP_FORCE
				velocity.z = 0.0
				collided = move_and_slide()
		else:
			var delta_1:Vector3 = get_position_delta()
			var areawalk = (delta_1.x * delta_1.x + delta_1.z * delta_1.z) / speed * mulstep
			if !is_run and max_collided_speed < areawalk:
				max_collided_speed = areawalk


func enter_underwater():
	# function called by Area3D (Water Object)
	level_water = get_position().y
	switch_state(StatePlayer.WALK_WATER)


func exit_underwater():
	# function called by Area3D (Water Object)
	switch_state(StatePlayer.WALK)


func enter_boat(boat:Node3D):
	print("enter_boat")
	near_boat = boat
	print(boat.name)


func exit_boat(boat:Node3D):
	print("exit_boat")
	if boat == near_boat:
		near_boat = null


func _on_timer_jump_timeout():
	jump_disabled = false