extends RigidBody3D

var drag_coefficient := 2.0
var static_drag := 5.0
var max_thrust_n := 100.0

var max_aoa := deg_to_rad(5.0)
var drag_coefficient_wing := 2.0
var max_aoa_drag_coefficient := 4.0

@onready var thrust_point := $ThrustPoint.position as Vector3
@onready var left_wing_point := $LeftWing.position as Vector3
@onready var right_wing_point := $RightWing.position as Vector3

var im := ImmediateMesh.new()

func draw_line(from: Vector3, to: Vector3, color: Color):
	im.surface_set_color(color)
	im.surface_add_vertex(from)
	im.surface_add_vertex(to)

func get_point_velocity (point :Vector3)->Vector3:
	return linear_velocity + angular_velocity.cross(point - global_transform.origin)

func _ready() -> void:
	var mi := MeshInstance3D.new()
	var mat := StandardMaterial3D.new()
	mat.shading_mode = BaseMaterial3D.SHADING_MODE_UNSHADED
	mat.vertex_color_use_as_albedo = true
	mi.material_override = mat
	mi.mesh = im
	add_child(mi)
	mi.top_level = true
	mi.global_transform = Transform3D()
func _physics_process(delta: float) -> void:
	PhysicsServer3D.body_set_param(get_rid(), PhysicsServer3D.BODY_PARAM_FRICTION, -200000000.0)
	im.clear_surfaces()
	im.surface_begin(Mesh.PRIMITIVE_LINES)
	var pos := global_transform * thrust_point
	var left_wing_pos := global_transform * left_wing_point
	var right_wing_pos := global_transform * right_wing_point
	var global_forward := global_basis * Vector3.FORWARD
	var global_back := -global_forward
	
	var movement_dir := linear_velocity.normalized()
	if movement_dir.is_normalized():
		apply_central_force((-movement_dir) * linear_velocity.length_squared() * drag_coefficient)
	
	var thrust_amount := Input.get_action_strength("thrust_forward")
	apply_force(global_forward * max_thrust_n * thrust_amount, pos - global_position)
	
	var input_vec := Vector2()
	input_vec.x = Input.get_action_strength("roll_right") - Input.get_action_strength("roll_left")
	input_vec.y = Input.get_action_strength("pitch_down") - Input.get_action_strength("pitch_up")
	
	var left_wing_input := clamp(input_vec.y + input_vec.x, -1.0, 1.0) as float
	var right_wing_input := clamp(input_vec.y - input_vec.x, -1.0, 1.0) as float
	
	run_wing(left_wing_pos, left_wing_input, global_basis * Vector3.UP)
	run_wing(right_wing_pos, right_wing_input, global_basis * Vector3.UP)
	
	im.surface_end()
func run_wing(wing_global_pos: Vector3, wing_input: float, wing_global_up: Vector3):
	var local_wing_velocity := get_point_velocity(wing_global_pos)
	var wing_vel_along_wing_normal := local_wing_velocity.project(wing_global_up)
	var wing_force_pos := wing_global_pos - global_position
	var wing_movement_dir := wing_vel_along_wing_normal.normalized()
	if wing_movement_dir.is_normalized():
		var drag_force := (-wing_movement_dir) * (wing_vel_along_wing_normal.length_squared() * drag_coefficient_wing)
		apply_force(drag_force, wing_force_pos)
		draw_line(wing_global_pos, wing_global_pos + drag_force, Color.BLUE)
	apply_force(wing_input * wing_global_up * 15.0, wing_force_pos)
	draw_line(wing_global_pos, wing_global_pos + local_wing_velocity, Color.RED)
