extends RigidBody

var thrust_plan : Plan = Plan.new()
var is_local : bool = false
var running_plan : bool = false
var plan_time : float = 0.0
var last_translation : Vector3 = get_translation()
var last_rotation : Vector3 = get_rotation()
var initial_translation : Vector3 = get_translation()
var initial_rotation : Vector3 = get_rotation()
var initial_velocity : Vector3 = Vector3(0,0,0)
var initial_rotational_velocity : Vector3 = Vector3(0,0,0)
var enable_physics : bool = false

var round_time : float = 0.0
var end_of_round_translation : Vector3 = get_translation()
var end_of_round_rotation : Vector3 = get_rotation()

onready var arrow_obj : Spatial = $ArrowContainer
onready var arrow_scale_obj : Spatial = $ArrowContainer/ArrowScaleContainer
onready var rotate_arrow_obj : Spatial = $RotateArrowContainer
onready var rotate_arrow_scale_obj : Spatial = $RotateArrowContainer/RotateArrowScaleContainer

onready var turn_end_banner : TextureRect = $TurnEndBanner

const TIME_STEP : float = 0.01
const MAX_LIN_THRUST : float = 1.0
const MAX_ROT_THRUST : float = 1.0

func init() -> void:
	last_rotation = get_rotation()
	last_translation = get_translation()
	initial_rotation = get_rotation()
	initial_translation = get_translation()
	enable_physics = true
	get_parent().get_parent().console_print("Player is set to local? " + String(is_local))
	if is_local:
		$MainPanel.visible = true
		$ThrustPanel.visible = true

func send_thrust_plan() -> void:
	for element in thrust_plan.current_elements:
		get_parent().get_parent().console_print('Sending data = ' + String(element.time) + String(element.linear_thrust) + String(element.rotational_thrust))
		rpc_id(1, "add_thrust_element", element.time, element.linear_thrust, element.rotational_thrust)

remote func add_thrust_element(time: float, linear_thrust : Vector3, rotational_thrust : Vector3) -> void:
	if is_local:
		return
	var element : ThrustElement = ThrustElement.new()
	element.time = time
	element.linear_thrust = linear_thrust
	element.rotational_thrust = rotational_thrust
	get_parent().get_parent().console_print(String(is_local) + ": Adding thrust element with the following params: time = " + String(time) + ", lin = " + String(linear_thrust) + ", rot = " + String(rotational_thrust) + ".")
	thrust_plan.add_element(element)

remote func r_end_turn() -> void:
	self._on_PlayButton_pressed()
	if is_local:
		$MainPanel/EndTurnButton.disabled = false
		$MainPanel/TimeBar.value = 0
		$TurnEndBanner.visible = true
		$TurnEndBanner/AnimationPlayer.play()
		return
	thrust_plan.new_turn()

func _on_TimeBar_value_changed(value : float) -> void:
	var min_time : float = thrust_plan.current_time()
	get_parent().get_parent().console_print(String(is_local) + ": Current plan time is " + String(min_time))
	if value <= min_time:
		$MainPanel/TimeBar.value = min_time

func _on_AddButton_pressed() -> void:
	var new_thrust_element : ThrustElement = ThrustElement.new()
	new_thrust_element.linear_thrust = get_linear_vector()
	new_thrust_element.rotational_thrust = get_rotational_vector()
	new_thrust_element.time = $MainPanel/TimeBar.value - thrust_plan.current_time()
	if (thrust_plan.add_element(new_thrust_element)):
		get_parent().get_parent().console_print(String(is_local) + ": Added thrust element with " + String(new_thrust_element.time) + String(new_thrust_element.rotational_thrust) + String(new_thrust_element.linear_thrust))
	else:
		get_parent().get_parent().console_print(String(is_local) + ": Failed to add thrust element")

func _on_RemoveButton_pressed() -> void:
	thrust_plan.remove_last_element()

func _on_ClearButton_pressed() -> void:
	thrust_plan.remove_all_elements()

func _on_EndTurnButton_pressed() -> void:
	$MainPanel/EndTurnButton.disabled = true
	send_thrust_plan()
	rpc_id(1, "end_turn")
	thrust_plan.new_turn()
	round_time += 5
	# TODO: Add the rest functionality

func _update_arrow() -> void:
	var linear_thrust : Vector3 = get_linear_vector()
	var linear_thrust_normalized : Vector3 = linear_thrust.normalized()
	var linear_thrust_magnitude : float = linear_thrust.length()
	var phi : float 
	if linear_thrust_normalized.x == 0 and linear_thrust_normalized.z == 0:
		phi = PI/2
	else:
		phi = asin(linear_thrust_normalized.x / sqrt(pow(linear_thrust_normalized.x, 2) + pow(linear_thrust_normalized.z,2)))
		
	if linear_thrust_normalized.z < 0:
		phi = PI - phi
	var theta : float = -asin(linear_thrust_normalized.y)
	arrow_obj.rotation = Vector3(0,0,0)
	arrow_obj.rotate(Vector3(1,0,0), theta)
	arrow_obj.rotate(Vector3(0,1,0), phi)
	arrow_scale_obj.scale = Vector3(linear_thrust_magnitude, linear_thrust_magnitude, linear_thrust_magnitude)

func _update_rotate_arrow() -> void:
	var rotational_thrust : Vector3 = get_rotational_vector()
	var rotational_thrust_normalized : Vector3 = rotational_thrust.normalized()
	var rotational_thrust_magnitude : float = rotational_thrust.length()
	var phi : float 
	if rotational_thrust_normalized.x == 0 and rotational_thrust_normalized.z == 0:
		phi = PI/2
	else:
		phi = asin(rotational_thrust_normalized.x / sqrt(pow(rotational_thrust_normalized.x, 2) + pow(rotational_thrust_normalized.z,2)))
		
	if rotational_thrust_normalized.z < 0:
		phi = PI - phi
	var theta : float = -asin(rotational_thrust_normalized.y)
	rotate_arrow_obj.rotation = Vector3(0,0,0)
	rotate_arrow_obj.rotate(Vector3(1,0,0), theta)
	rotate_arrow_obj.rotate(Vector3(0,1,0), phi)
	rotate_arrow_scale_obj.scale = Vector3(rotational_thrust_magnitude, rotational_thrust_magnitude, rotational_thrust_magnitude)

func get_linear_vector() -> Vector3:
	return Vector3($ThrustPanel/XLin.value, $ThrustPanel/YLin.value, $ThrustPanel/ZLin.value) * MAX_LIN_THRUST

func get_rotational_vector() -> Vector3:
	return Vector3($ThrustPanel/XRot.value, $ThrustPanel/YRot.value, $ThrustPanel/ZRot.value) * MAX_ROT_THRUST

func _on_XLin_value_changed(value : float) -> void:
	self._update_arrow()
	if get_linear_vector().length() < 1.0:
		return
	$ThrustPanel/XLin.value = sign($ThrustPanel/XLin.value) * \
		sqrt(1 - pow($ThrustPanel/YLin.value, 2) - pow($ThrustPanel/ZLin.value, 2))

func _on_XRot_value_changed(value : float) -> void:
	self._update_rotate_arrow()
	if get_rotational_vector().length() < 1.0:
		return
	$ThrustPanel/XRot.value = sign($ThrustPanel/XRot.value) * \
		sqrt(1 - pow($ThrustPanel/YRot.value, 2) - pow($ThrustPanel/ZRot.value, 2))

func _on_YLin_value_changed(value : float) -> void:
	self._update_arrow()
	if get_linear_vector().length() < 1.0:
		return
	$ThrustPanel/YLin.value = sign($ThrustPanel/YLin.value) * \
		sqrt(1 - pow($ThrustPanel/XLin.value, 2) - pow($ThrustPanel/ZLin.value, 2))

func _on_YRot_value_changed(value : float) -> void:
	self._update_rotate_arrow()
	if get_rotational_vector().length() < 1.0:
		return
	$ThrustPanel/YRot.value = sign($ThrustPanel/YRot.value) * \
		sqrt(1 - pow($ThrustPanel/XRot.value, 2) - pow($ThrustPanel/ZRot.value, 2))

func _on_ZLin_value_changed(value : float) -> void:
	self._update_arrow()
	if get_linear_vector().length() < 1.0:
		return
	$ThrustPanel/ZLin.value = sign($ThrustPanel/ZLin.value) * \
		sqrt(1 - pow($ThrustPanel/YLin.value, 2) - pow($ThrustPanel/XLin.value, 2))

func _on_ZRot_value_changed(value : float) -> void:
	self._update_rotate_arrow()
	if get_rotational_vector().length() < 1.0:
		return
	$ThrustPanel/ZRot.value = sign($ThrustPanel/ZRot.value) * \
		sqrt(1 - pow($ThrustPanel/YRot.value, 2) - pow($ThrustPanel/XRot.value, 2))

func _get_current_plan_element(all_elements) -> PlanElement:
	var summed_time : float = 0.0
	for element in all_elements:
		summed_time += element.time
		if summed_time > plan_time:
			return element
	return null

func _get_all_thrust_elements():
	var all_elements = []
	for element in thrust_plan.elements:
		all_elements.append(element)
	for element in thrust_plan.current_elements:
		all_elements.append(element)
	return all_elements

func _physics_process(delta : float) -> void:
	if !enable_physics:
		return
	if !running_plan:
		arrow_obj.visible = true
		rotate_arrow_obj.visible = true
		disable_all_engine_particles()
		set_translation(last_translation)
		set_rotation(last_rotation)
		return
	var all_thrust_elements = _get_all_thrust_elements()
	var current_thrust_element : ThrustElement = _get_current_plan_element(all_thrust_elements)
	if current_thrust_element == null:
		last_translation = get_translation()
		last_rotation = get_rotation()
		running_plan = false
		return
	arrow_obj.visible = false
	rotate_arrow_obj.visible = false
	add_central_force(self.transform.basis.xform(current_thrust_element.linear_thrust))
	add_torque(self.transform.basis.xform(current_thrust_element.rotational_thrust))
	update_engine_particles(current_thrust_element.linear_thrust, current_thrust_element.rotational_thrust)
	plan_time += delta

func disable_all_engine_particles() -> void:
	var engine_particles = [
		$"EngineParticleContainer/S-X,R+Y",
		$"EngineParticleContainer/S+X,R-Y",
		$"EngineParticleContainer/S-X,R-Y",
		$"EngineParticleContainer/S+X,R+Y",
		$"EngineParticleContainer/S-Y,R+Z,R+X",
		$"EngineParticleContainer/S-Y,R-Z,R+X",
		$"EngineParticleContainer/S-Y,R-Z,R-X",
		$"EngineParticleContainer/S-Y,R+Z,R-X",
		$"EngineParticleContainer/S+Y,R-Z,R+X",
		$"EngineParticleContainer/S+Y,R+Z,R+X",
		$"EngineParticleContainer/S+Y,R-Z,R-X",
		$"EngineParticleContainer/S+Y,R+Z,R-X",
		$EngineParticleContainer/SZ,
		$EngineParticleContainer/SZ2
	]
	for ep in engine_particles:
		ep.emitting = false

func update_engine_particles(linear_thrust : Vector3, rotational_thrust : Vector3) -> void:
	var engine_particles : Particles = $"EngineParticleContainer/S-X,R-Y"
	if linear_thrust.x < 0 or rotational_thrust.y > 0:
		engine_particles.emitting = true
		engine_particles.scale = Vector3.ONE * (max(-linear_thrust.x / MAX_LIN_THRUST, 0)+max(rotational_thrust.y / MAX_ROT_THRUST, 0)) * 0.005
	else:
		engine_particles.emitting = false
	
	engine_particles = $"EngineParticleContainer/S+X,R+Y"
	if linear_thrust.x > 0 or rotational_thrust.y < 0:
		engine_particles.emitting = true
		engine_particles.scale = Vector3.ONE * (max(linear_thrust.x / MAX_LIN_THRUST, 0)+max(-rotational_thrust.y / MAX_ROT_THRUST, 0)) * 0.005
	else:
		engine_particles.emitting = false
	
	engine_particles = $"EngineParticleContainer/S-X,R+Y"
	if linear_thrust.x < 0 or rotational_thrust.y < 0:
		engine_particles.emitting = true
		engine_particles.scale = Vector3.ONE * (max(-linear_thrust.x / MAX_LIN_THRUST, 0)+max(-rotational_thrust.y / MAX_ROT_THRUST, 0)) * 0.005
	else:
		engine_particles.emitting = false
	
	engine_particles = $"EngineParticleContainer/S+X,R-Y"
	if linear_thrust.x > 0 or rotational_thrust.y > 0:
		engine_particles.emitting = true
		engine_particles.scale = Vector3.ONE * (max(linear_thrust.x / MAX_LIN_THRUST, 0)+max(rotational_thrust.y / MAX_ROT_THRUST, 0)) * 0.005
	else:
		engine_particles.emitting = false
	
	engine_particles = $"EngineParticleContainer/S-Y,R+Z,R+X"
	if linear_thrust.y < 0 or rotational_thrust.z > 0 or rotational_thrust.x > 0:
		engine_particles.emitting = true
		engine_particles.scale = Vector3.ONE * (max(-linear_thrust.y / MAX_LIN_THRUST, 0)+max(rotational_thrust.z / MAX_ROT_THRUST, 0)+max(rotational_thrust.x / MAX_ROT_THRUST, 0)) * 0.01/3
	else:
		engine_particles.emitting = false
	
	engine_particles = $"EngineParticleContainer/S-Y,R-Z,R+X"
	if linear_thrust.y < 0 or rotational_thrust.z < 0 or rotational_thrust.x > 0:
		engine_particles.emitting = true
		engine_particles.scale = Vector3.ONE * (max(-linear_thrust.y / MAX_LIN_THRUST, 0)+max(-rotational_thrust.z / MAX_ROT_THRUST, 0)+max(rotational_thrust.x / MAX_ROT_THRUST, 0)) * 0.01/3
	else:
		engine_particles.emitting = false
	
	engine_particles = $"EngineParticleContainer/S-Y,R-Z,R-X"
	if linear_thrust.y < 0 or rotational_thrust.z < 0 or rotational_thrust.x < 0:
		engine_particles.emitting = true
		engine_particles.scale = Vector3.ONE * (max(-linear_thrust.y / MAX_LIN_THRUST, 0)+max(-rotational_thrust.z / MAX_ROT_THRUST, 0)+max(-rotational_thrust.x / MAX_ROT_THRUST, 0)) * 0.01/3
	else:
		engine_particles.emitting = false
	
	engine_particles = $"EngineParticleContainer/S-Y,R+Z,R-X"
	if linear_thrust.y < 0 or rotational_thrust.z > 0 or rotational_thrust.x < 0:
		engine_particles.emitting = true
		engine_particles.scale = Vector3.ONE * (max(-linear_thrust.y / MAX_LIN_THRUST, 0)+max(rotational_thrust.z / MAX_ROT_THRUST, 0)+max(-rotational_thrust.x / MAX_ROT_THRUST, 0)) * 0.01/3
	else:
		engine_particles.emitting = false
	
	engine_particles = $"EngineParticleContainer/S+Y,R-Z,R+X"
	if linear_thrust.y > 0 or rotational_thrust.z < 0 or rotational_thrust.x > 0:
		engine_particles.emitting = true
		engine_particles.scale = Vector3.ONE * (max(linear_thrust.y / MAX_LIN_THRUST, 0)+max(-rotational_thrust.z / MAX_ROT_THRUST, 0)+max(rotational_thrust.x / MAX_ROT_THRUST, 0)) * 0.01/3
	else:
		engine_particles.emitting = false
	
	engine_particles = $"EngineParticleContainer/S+Y,R+Z,R+X"
	if linear_thrust.y > 0 or rotational_thrust.z > 0 or rotational_thrust.x > 0:
		engine_particles.emitting = true
		engine_particles.scale = Vector3.ONE * (max(linear_thrust.y / MAX_LIN_THRUST, 0)+max(rotational_thrust.z / MAX_ROT_THRUST, 0)+max(rotational_thrust.x / MAX_ROT_THRUST, 0)) * 0.01/3
	else:
		engine_particles.emitting = false
	
	engine_particles = $"EngineParticleContainer/S+Y,R-Z,R-X"
	if linear_thrust.y > 0 or rotational_thrust.z < 0 or rotational_thrust.x < 0:
		engine_particles.emitting = true
		engine_particles.scale = Vector3.ONE * (max(linear_thrust.y / MAX_LIN_THRUST, 0)+max(-rotational_thrust.z / MAX_ROT_THRUST, 0)+max(-rotational_thrust.x / MAX_ROT_THRUST, 0)) * 0.01/3
	else:
		engine_particles.emitting = false
	
	engine_particles = $"EngineParticleContainer/S+Y,R+Z,R-X"
	if linear_thrust.y > 0 or rotational_thrust.z > 0 or rotational_thrust.x < 0:
		engine_particles.emitting = true
		engine_particles.scale = Vector3.ONE * (max(linear_thrust.y / MAX_LIN_THRUST, 0)+max(rotational_thrust.z / MAX_ROT_THRUST, 0)+max(-rotational_thrust.x / MAX_ROT_THRUST, 0)) * 0.01/3
	else:
		engine_particles.emitting = false
	
	engine_particles = $EngineParticleContainer/SZ
	var engine_particles2 : Particles = $EngineParticleContainer/SZ2
	if linear_thrust.z != 0:
		engine_particles.emitting = true
		engine_particles2.emitting = true
		var modz : float = linear_thrust.z
		if modz < 0:
			modz = -modz
		engine_particles.scale = Vector3.ONE * (modz / MAX_ROT_THRUST) * 0.02
		engine_particles2.scale = Vector3.ONE * (modz / MAX_ROT_THRUST) * 0.02
	else:
		engine_particles.emitting = false
		engine_particles2.emitting = false

func play_current_plan():
	pass

func play_all_plans():
	get_parent().play_full_plans()

func play_full_plan():
	set_linear_velocity(initial_velocity)
	set_angular_velocity(initial_rotational_velocity)
	set_translation(initial_translation)
	set_rotation(initial_rotation)
	running_plan = true
	plan_time = 0.0

func _on_PlayButton_pressed():
	play_all_plans()