extends VehicleBody

var countdown
var boosting = false
const booster_force = 2500
const brake_force = 50
const boost_length = 8
const turn_constant = 0.45
const roll_constant = 0.5
const pitch_constant = 0.9
const drag_constant = 0.4
const v_angle_max = 1.0472 #60 deg, also top and bottom of regular steering
const v_angle_min = -1.45626 #-85 deg
const roll_angle_max = 1.22173 #70 deg
var roll_dir = 0 #right = 1, left = -1
var pitch_dir = 0 #up = 1, down = -1
var v_angle
var roll_angle
var vel_slow
var parentvel = [Vector3.ZERO, Vector3.ZERO]
export var roll_curve : CurveTexture



func _ready():
	countdown = boost_length
	brake = 5
	set_network_master(-1)
	set_physics_process(true)
		#mode = RigidBody.MODE_STATIC

func trigger_boost():
	if not boosting:
		print("boosting")
		boosting = true

func _process(delta):
	if is_network_master():
		if boosting and countdown <= 0:
			print("stopped boosting")
			boosting = false
			countdown = boost_length
		
		if Input.is_action_just_pressed("boost"):
			trigger_boost()
		roll_dir =  Input.get_action_strength("roll_left") - Input.get_action_strength("roll_right")
		pitch_dir = Input.get_action_strength("pitch_up") - Input.get_action_strength("pitch_down")
		$rearwheel.brake = lerp($rearwheel.brake, Input.get_action_strength("brake")*brake_force, 0.05)

func _physics_process(delta):
	if is_network_master():
		v_angle = atan2(global_transform.basis.x.y,sqrt(global_transform.basis.x.z*global_transform.basis.x.z + global_transform.basis.x.x*global_transform.basis.x.x))
		roll_angle = global_transform.basis.x.cross(Vector3.UP).angle_to($rightaileron.global_transform.origin-$wingcenter.global_transform.origin)
		if $rightaileron.global_transform.origin.y < $leftaileron.global_transform.origin.y:
			roll_angle *= -1
		if boosting:
			add_force(global_transform.basis.x*booster_force,Vector3.ZERO)
			countdown -= delta
		
		#print(linear_velocity.length())
		#slow plane by drag and gravity
		if linear_velocity.length() > 16 and linear_velocity.angle_to(global_transform.basis.x) < 0.25:
			var v_dir = 1 if linear_velocity.y > 0 else -1
			vel_slow = v_dir*sqrt(abs(2*9.8*linear_velocity.y*delta)) + drag_constant + Input.get_action_strength("brake")*5
			add_force(-1*linear_velocity.normalized()*vel_slow*mass,Vector3.ZERO)
		

		
			#pass #"inherit" linear and angular velocity of what plane is landed on

func _integrate_forces(state):
	if is_network_master():
		#linear_velocity -= parentvel[0]; angular_velocity -= parentvel[1]
#		if $rightwheel.get_rpm() < 60 and $rightwheel.is_in_contact():
#			if $rightwheel/Area.get_overlapping_bodies()[0].has_method("get_linear_velocity"):
#				var ulv = $rightwheel/Area.get_overlapping_bodies()[0].linear_velocity
#				var uav = $rightwheel/Area.get_overlapping_bodies()[0].angular_velocity
#				var upos = $rightwheel/Area.get_overlapping_bodies()[0].global_transform.origin
#				ulv += (global_transform.origin - upos).rotated(uav.normalized(),uav.length()*state.get_step()) + global_transform.origin
#				parentvel = [ulv,uav]
#		else:
#			parentvel = [Vector3.ZERO, Vector3.ZERO]
		if linear_velocity.length() > 25:
			#linear_velocity = linear_velocity.normalized()*(linear_velocity.length()-vel_slow)
			var ang_vel_target = Vector3.ZERO
			#pitch plane
			if pitch_dir == 1: #up, rotate toward maximum vertical angle
				ang_vel_target += pitch_constant*(v_angle_max-v_angle)*global_transform.basis.z
			elif pitch_dir == -1: #down, rotate toward minimum vertical angle
				ang_vel_target += pitch_constant*(v_angle_min-v_angle)*global_transform.basis.z
			
			#roll (rotate around lengthwise axis)
			var is_returning = 3 if roll_dir == 0 else 1 #return to flat quicker
			ang_vel_target -= roll_constant*is_returning*(roll_dir*roll_curve.curve.interpolate(roll_angle_max-roll_angle))*global_transform.basis.x
			
			#turn (based on how much the plane is rolled (need to add
			ang_vel_target += turn_constant*Vector3.UP*roll_angle
			
			#apply angular velocity
			angular_velocity = ang_vel_target#lerp(angular_velocity,ang_vel_target,0.1)
			if linear_velocity.angle_to(global_transform.basis.x) < 0.25:
				set_linear_velocity(get_linear_velocity().slerp(transform.basis.x*linear_velocity.length(),0.1))
		#linear_velocity += parentvel[0]; angular_velocity += parentvel[1]
		rpc("_set_position", global_transform, linear_velocity)
	elif get_network_master() == -1 and get_tree().get_network_unique_id() == 1:
		rpc("_set_position", global_transform, linear_velocity)