extends Player

class_name BasePathPlayer

var MAX_SPEED:float = 0.2

var current_speed:float = 0.0

export var speed_factor:float = 0.001
export var brake_factor:float = 0.8


# nodes
var timer:Timer
var path:Path
var follow:PathFollow

var lane:int

var has_next:bool = false
var is_resetable:bool = false
var buffered_curve:Curve3D
var buffered_translate:Vector3 = Vector3(0,0,0)
var buffered_rotation_axis:Vector3 = Vector3(0,0,0)
var last_rotation_axis:Vector3 = Vector3(0,0,0)
var last_rotation_phi:float = 0
var buffered_rotation_phi:float = 0
var torque_penalty:Vector3
var force_penalty:Vector3

const lane_offset = [-0.085, 0.085, -0.255, 0.255 ]

func _ready():
	timer = get_node("ResetTimer")
	path = get_node("Path")
	path.curve = path.curve.duplicate()
	follow = get_node("Path/PathFollow")
	
func _physics_process(delta):
	if is_network_master():
		if not is_out:
			
			follow.set_offset(follow.get_offset() + current_speed)
			
			# curve
			if has_next && (path.curve.get_point_count() == 0 || follow.get_unit_offset() >= 1.0):
				has_next = false
				path.curve.clear_points()
				if path.curve == null:
					path.curve = Curve3D.new()
				for i in range(buffered_curve.get_point_count()):
					var _pos = buffered_curve.get_point_position(i)
					var _in = buffered_curve.get_point_in(i)
					var _out = buffered_curve.get_point_out(i)
					if buffered_rotation_phi != 0:
						_pos = _pos.rotated(buffered_rotation_axis, buffered_rotation_phi)
						_in = _in.rotated(buffered_rotation_axis, buffered_rotation_phi)
						_out = _out.rotated(buffered_rotation_axis, buffered_rotation_phi)
					path.curve.add_point(_pos + buffered_translate, _in, _out)
					path.curve.set_point_tilt(i, buffered_curve.get_point_tilt(i))
				
				follow.set_offset(0.001)
				if get_road() != null:
					road_index = get_road().get_index()
			
				print(buffered_translate)
				print(buffered_curve.get_baked_length())
				print(path.curve.get_baked_length())
			if get_road() != null:
				var road = get_road()
				var penalty_index = road.penalty_index(follow.get_offset(), current_speed)
				
				if penalty_index >= 0:
					torque_penalty = road.get_torque_penalty(penalty_index)
					force_penalty = road.get_force_penalty(penalty_index)
					is_out = true
					is_resetable = false
					timer.start()
			
			# position
			check_position()
		else:
			var race_car = get_race_car()
			race_car.get_node("ray").set_enabled(false)
			if force_penalty.length() != 0:
				race_car.apply_central_impulse(force_penalty * current_speed)
			if torque_penalty.length() != 0:
				race_car.axis_lock_angular_x = false
				race_car.axis_lock_angular_y = false
				race_car.axis_lock_angular_z = false
				race_car.apply_torque_impulse(torque_penalty * current_speed)
	else:
		position = slave_position

func get_race_car():
	return get_node("Path/PathFollow/raceCar")

func get_road():
	return get_race_car().road
			
func _on_raceCar_road_entered(road):
	buffered_curve = road.get_lane_curve(lane)
	print(road.get_name())
	print(buffered_curve.get_baked_length())
	
	buffered_rotation_axis = last_rotation_axis
	buffered_rotation_phi = last_rotation_phi
							
	if path.curve.get_point_count() > 0:
		buffered_translate = Util.Curve_get_last_point(path.curve)
		
	if road.get_end_rotation_phi() != 0:
		last_rotation_axis = (last_rotation_axis + road.get_end_rotation_axis()).normalized()
		last_rotation_phi += road.get_end_rotation_phi()
		
	has_next = true

master func check_position():
	position = Vector2(road_index, follow.get_offset())
	rset("slave_position", position)
	
master func reset():
	if is_resetable:
		is_out = false
		is_resetable = false
		has_next = false
		current_speed = 0
		
		var race_car = get_race_car()
		# todo PATH
		
		var route = get_node("/root/world/route")
		var road = race_car.road
		
		while road.reset_index != 0:
			road = route.get_road(road.get_index() - road.reset_index)
			
		path.curve.clear_points()
		
		force_penalty = Vector3(0,0,0)
		torque_penalty = Vector3(0,0,0)
		
		var road_before = route.get_road(road.get_index() - 1)
		if road_before == null:
			road_before = road
		
		#buffered_translate = road_before.get_curve().get_point_position(road_before.get_curve().get_point_count() - 1)
		last_rotation_axis = road_before.get_rotation().normalized()
		last_rotation_phi = road_before.get_rotation().length()
		
		follow.set_transform(road.get_transform())
		
		race_car.road = null
		race_car.collider = null
		race_car.linear_velocity = Vector3(0,0,0)
		race_car.angular_velocity = Vector3(0,0,0)
		race_car.axis_lock_angular_x = true
		race_car.axis_lock_angular_y = true
		race_car.axis_lock_angular_z = true
		race_car.transform = Transform()
		race_car.get_node("ray").set_enabled(true)
			
func set_start(new_lane:int):
	lane = new_lane
	follow.set_h_offset(lane_offset[lane])

func _on_ResetTimer_timeout():
	is_resetable = true