extends CharacterBody3D

# 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 SPEED_RUN_UP = SPEED_WALK_UP * 2.0
const SPEED_RUN_UP_STRAFE = SPEED_WALK_UP_STRAFE * 2.0
const SPEED_RUN_STRAFE = SPEED_WALK_STRAFE * 2.0
const SPEED_RUN_DOWN_STRAFE = SPEED_WALK_DOWN_STRAFE * 2.0
const SPEED_RUN_DOWN = SPEED_WALK_DOWN * 2.0

const ZOOM_Y = 0.05
const ZOOM_Z = 0.1
const ZOOM_STEP = 0.1
const ZOOM_MIN = 0.2
const ZOOM_MAX = 2.0

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

# 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
# Player position
var player_rotate_y = 0.0
#var debug:bool = false
# Activate reconciliation between camera & player
var reconciliate_rotate_camera_player:bool = true
# Player run
var is_run:bool = false
var zoom:float = 1.0
var tps:bool = true


func _init():
	pass


func _ready():
# Place the mouse at the center of the screen
	get_viewport().warp_mouse(starting_point)


func _input(event):
	# Managed zoom (FPS or TPS)
	if Input.is_action_pressed("camera_zoom_in"):
		if tps:
			if zoom > ZOOM_MIN:
				zoom -= ZOOM_STEP
				var zoom3D:Vector3 = Vector3(0.0, -ZOOM_Y, -ZOOM_Z)
				$camera_root/horizontal_root/vertical_root/Camera3D_TPS.translate_object_local(zoom3D)
			if zoom < ZOOM_MIN:
				$camera_root/Camera3D_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
	elif Input.is_action_pressed("camera_zoom_out"):
		if not tps:
			# Reposition camera TPS back to player
#			$camera_root/horizontal_root.rotate_y( player_rotate_y - camera_rotate_y )
#			camera_rotate_y = player_rotate_y
			reconciliate_rotate_camera_player = true
			tps = true
		zoom += ZOOM_STEP
		$camera_root/horizontal_root/vertical_root/Camera3D_TPS.make_current()
		if zoom >= ZOOM_MAX:
			zoom = ZOOM_MAX
		else:
			var zoom3D:Vector3 = Vector3(0.0, ZOOM_Y, ZOOM_Z)
			$camera_root/horizontal_root/vertical_root/Camera3D_TPS.translate_object_local(zoom3D)

	# If right mouse button is pressed and mouse moves, pan horizontally camera
	# and rotate vertically
	if tps:
		if Input.is_action_just_pressed ( "ui_strafe" ):
			reconciliate_rotate_camera_player = false
			Input.set_mouse_mode(Input.MOUSE_MODE_HIDDEN)
		elif Input.is_action_just_released ( "ui_strafe" ):
			reconciliate_rotate_camera_player = true
			Input.set_mouse_mode(Input.MOUSE_MODE_VISIBLE)
	if Input.is_action_just_pressed( "move_run" ):
		is_run = true
	elif Input.is_action_just_released( "move_run" ):
		is_run = false
#	if Input.is_action_just_released("ui_cut"):
#		print("debug on")
#		debug = true
#	if Input.is_action_just_released("ui_copy"):
#		print("debug off")
#		debug = false

	if Input.is_mouse_button_pressed( 2 ):
		if event is InputEventMouseMotion:
			if tps:
				camera_rotate_y -= event.relative.x *0.01
				if camera_rotate_y > PI:
					camera_rotate_y -= TAU
				elif camera_rotate_y <= -PI:
					camera_rotate_y += TAU
				$camera_root/horizontal_root.rotate_y( -event.relative.x *0.01 )

				var new_camera_rotate_x = camera_rotate_x + event.relative.y * 0.01 
				if new_camera_rotate_x <= PI_2 and new_camera_rotate_x >= - PI_2:
					$camera_root/horizontal_root/vertical_root.rotate_x( event.relative.y * 0.01 )
					camera_rotate_x = new_camera_rotate_x
			else:
				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
				camera_rotate_y = player_rotate_y
				rotate_y( -event.relative.x *0.01 )
				#$camera_root/horizontal_root.rotate_y( event.relative.x *0.01 )


func _physics_process(delta):
	var input_dir: Vector2
	var input_x: float
	var input_y: float
	var speed: float
	var move_up:bool = false
	var move_down:bool = false
	var move_strafe: bool = false

	# Add the gravity.
	if not is_on_floor():
		motion_velocity.y -= gravity * delta

	# Handle Jump.
#	if Input.is_action_just_pressed("ui_accept") and is_on_floor():
#		motion_velocity.y = JUMP_FORCE

	# Get the input direction and handle the movement/deceleration.
	if Input.is_action_pressed("ui_strafe"):
		if Input.is_action_pressed("ui_left"):
			input_x = 1.0
			move_strafe = true
		elif Input.is_action_pressed("ui_right"):
			input_x = -1.0
			move_strafe = true
	else:
		var y = 0
		if Input.is_action_pressed("ui_right"):
			y -= 1
		if Input.is_action_pressed("ui_left"):
			y += 1
		if y != 0:
			var dt = y * delta * SPEED_ROTATE
			if tps:
				camera_rotate_y += dt
				if camera_rotate_y > PI:
					camera_rotate_y -= TAU
				elif camera_rotate_y <= -PI:
					camera_rotate_y += TAU
				$camera_root/horizontal_root.rotate_y( dt )
			else:
				player_rotate_y += dt
				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( dt )
		# Disable vector on ui_right/ui_left (used to rotate and not strafe)
		input_x = 0
	if Input.is_action_pressed("ui_up"):
		input_y = 1.0
		move_up = true
	elif Input.is_action_pressed("ui_down"):
		input_y = -1.0
		move_down = true

	if is_run:
		if move_strafe:
			if move_up:
				speed = SPEED_RUN_UP_STRAFE
			elif move_down:
				speed = SPEED_RUN_DOWN_STRAFE
			else:
				speed = SPEED_RUN_STRAFE
		elif move_up:
			speed = SPEED_RUN_UP
		else:
			speed = SPEED_RUN_DOWN
	else:
		if move_strafe:
			if move_up:
				speed = SPEED_WALK_UP_STRAFE
			elif move_down:
				speed = SPEED_WALK_DOWN_STRAFE
			else:
				speed = SPEED_WALK_STRAFE
		elif move_up:
			speed = SPEED_WALK_UP
		else:
			speed = SPEED_WALK_DOWN

	var direction = (transform.basis * Vector3(input_x, 0, input_y)).normalized()
	if direction:
		motion_velocity.x = direction.x * speed
		motion_velocity.z = direction.z * speed
	else:
		motion_velocity.x = move_toward(motion_velocity.x, 0, speed)
		motion_velocity.z = move_toward(motion_velocity.z, 0, speed)

	move_and_slide()


func _process( delta ):
#	if debug:
#		print(reconciliate_rotate_camera_player, ", ", camera_rotate_y,", ", player_rotate_y)

	if reconciliate_rotate_camera_player:
		var diff = camera_rotate_y - player_rotate_y

		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:
			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
			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