extends RigidBody

const TIME_STEP : float = 0.01

var thrust_plan : Plan = Plan.new()
var is_ready : 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 enable_physics : bool = false

func init():
	last_rotation = get_rotation()
	last_translation = get_translation()
	enable_physics = true

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


func send_thrust_plan() -> void:
	get_parent().get_parent().console_print("Sending plan...")
	for element in thrust_plan.current_elements:
		rpc("add_thrust_element", element.time, element.linear_thrust, element.rotational_thrust)
	rpc("r_end_turn")
	thrust_plan.new_turn()


remote func end_turn() -> void:
	get_parent().get_parent().console_print("Setting ready status to true...")
	is_ready = true


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 = []
	all_elements = thrust_plan.elements
	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:
		translation = last_translation
		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 = translation
		last_rotation = rotation
		running_plan = false
		return
	add_central_force(self.transform.basis.xform(current_thrust_element.linear_thrust))
	add_torque(self.transform.basis.xform(current_thrust_element.rotational_thrust))
	plan_time += delta