extends CharacterBody2D

#Tutorial: https://www.youtube.com/watch?v=mJ1ZfGDTMCY  t=15s

signal car_finished(playerid,finalTime)

const COLLISIONMASK_FINISH=3 #set in road_overlay
const COLLISIONMASK_CHECKPOINT=4 #set in road_overlay

var checkpoints :Array[String]=[] #gets set on car creation
var checkpointtimes :Array[float]=[]

const ROAD_R_NAME="road_r"
const ROAD_L_NAME="road_l"
const CAR_NAME="CharacterBody_Car"


const COLLISION_SLOWDOWN_CAR=1.01
const COLLISION_SLOWDOWN_WALL=1.2

const MIN_CRASH_VELOCITY=10

const STANDSTILLSPEED=0.5
var wheel_base = 60*0.5

var engine_power = 350
#var engine_power = 1000 #crazy mode
var applied_engine_power=0

var friction = -0.5
var drag = -0.0005
var braking = -200
#var braking = -600 #crazy mode
var max_speed_reverse = 100
var slip_speed = 150
var traction_fast = 0.05 #traction when above slip_speed
var traction_slow = 0.5


#Automatic Steering settings

var steering_speed_slow = 100 #speed for slow steering
var steering_angle_slow = 50 #maximum angle slow speed
var steering_distance_far_slow=256
var steering_distance_close_slow=16

var steering_speed_fast = 400 #speed for fast steering
#var steering_speed_fast = 3*400 #speed for fast steering #crazy mode
var steering_angle_fast = 1 #maximum angle fast speed
var steering_distance_far_fast=256
var steering_distance_close_fast=64

# resetCar
var resetcar_stoppedspeed = 50 #activate timer when below this speed
var resetcar_movingspeed=resetcar_stoppedspeed+10 #stop timer when above this speed
var resetcar_distance=128 #196 is roughly when car is in the middle of a two wide road
var resetcar_steerangle=120

# other car avoidance
var avoid_car_distance_far=100
var avoid_car_distance_close=50
var avoid_car_steeringangle_far=0
var avoid_car_steeringangle_close=50
var avoid_car_mininum_space=50 #how much space RayCast_FL/R must have for avoidance to start. 
var avoidance_steeringangle:float=0 #actual set steering angle to avoid car
var avoidance_turndirection:int=0

#Effects
const BURNOUT_KEEP_ON_AFTER_START=1


#Variables
var running=false
var acceleration = Vector2.ZERO

var steer_direction=0

var autoreset=false

var autosteer_enabled=false


var burnout:float=0 #at 0 is off

@onready var ray_cast_fl: RayCast2D = $RayCast_FL
@onready var ray_cast_fr: RayCast2D = $RayCast_FR


@onready var reset_timer: Timer = $resetTimer

@onready var ray_cast_car: RayCast2D = $RayCast_Car #for tracking markers

@onready var collision_shape: CollisionShape2D = $CollisionShape2D
@onready var collision_enable_timer: Timer = $collisionEnableTimer

@onready var enginesound: Node = $Enginesound
@onready var sfx: Node = $SFX



var playerid=0
var finalTime=-1

func _ready() -> void:
	collision_shape.disabled=true #disable collisions on start. also to avoid collision when initially setting position
	finalTime=-1
	

func _physics_process(delta: float) -> void:
	
	acceleration=Vector2.ZERO
	check_markers()
	get_input(delta)
	apply_friction()
	calculate_steering(delta)
	velocity +=acceleration*delta
	#velocity = transform.x * 200
	#vel = move_and_slide()
	
	if burnout>0:
		$Burnout_Left.emitting=true
		$Burnout_Right.emitting=true
	else:
		$Burnout_Left.emitting=false
		$Burnout_Right.emitting=false
	
	move_and_slide()
	
	for i in get_slide_collision_count():
		var collision = get_slide_collision(i)
		if $".".name==collision.get_collider().name: #collided with another car
			sfx.crashCarToCar(velocity.length())
			#collision.get_collider_velocity().length()
		#	velocity-=COLLISION_SLOWDOWN_CAR
		if ROAD_R_NAME==collision.get_collider().name or ROAD_L_NAME==collision.get_collider().name: #collided with road
			if (velocity.length()>MIN_CRASH_VELOCITY):
				velocity/=COLLISION_SLOWDOWN_WALL
				#sfx.crashBarrier(velocity.length())
				sfx.crashBarrier(velocity.length())
				
				
	
	#if get_slide_collision_count()>0:
	#	velocity/=2
	#for i in get_slide_collision_count():
	#	var collision = get_slide_collision(i)
	#	print("Collided with: ", collision.get_collider().name)
	
	if velocity.length() < resetcar_stoppedspeed and autosteer_enabled: #moving slow, possibly crash?
		if reset_timer.is_stopped():
			reset_timer.start()
	if velocity.length() > resetcar_movingspeed:
		reset_timer.stop()
		
	if running:
		enginesound.setCarSpeed(velocity.length())
		enginesound.setCarAcceleration(applied_engine_power/engine_power,delta)
	else: #at start or end of round
		enginesound.setCarSpeed(burnout*500)
		enginesound.setCarAcceleration(1.0,delta)
	#print("Car Accel="+str(acceleration.length()) +"/"+str(engine_power))


func _on_reset_timer_timeout() -> void:
	autoreset=true
	
func apply_friction():
	if velocity.length() < STANDSTILLSPEED: #standstill
		velocity=Vector2.ZERO
	var friction_force=velocity*friction
	var drag_force=velocity*velocity.length()*drag
	
	acceleration+=drag_force+friction_force
	
func get_input(delta:float):
	
	# Check RayCast to Road
	const distance_inf=1000
	var distance_fl=distance_inf
	var distance_fr=distance_inf
	var distance_guide_l=0
	var distance_guide_r=0
	
	if ray_cast_fl.is_colliding():
		var origin=ray_cast_fl.global_transform.origin
		var collision_point = ray_cast_fl.get_collision_point()
		distance_fl = origin.distance_to(collision_point)
		var collision_object=ray_cast_fl.get_collider()
		if collision_object.name==ROAD_R_NAME:
			distance_fl*=2 #fake distance when wrong side of road
			
		#print("DistanceFL "+str(distance_fl))
	if ray_cast_fr.is_colliding():
		var origin=ray_cast_fr.global_transform.origin
		var collision_point = ray_cast_fr.get_collision_point()
		distance_fr = origin.distance_to(collision_point)
		var collision_object=ray_cast_fr.get_collider()
		if collision_object.name==ROAD_L_NAME:
			distance_fr*=2 #fake distance when wrong side of road
		#print("DistanceFR "+str(distance_fr))
		
		
		
	var distance_min= min(distance_fl,distance_fr)
	
	var turndirection = 1 if distance_fl<distance_fr else -1
	var steering_angle=constrain( remap(velocity.length(),steering_speed_fast,steering_speed_slow,steering_angle_fast,steering_angle_slow),steering_angle_fast,steering_angle_slow) #set maximum steering_angle based on speed
	
	var steering_distance_far=constrain( remap(velocity.length(),steering_speed_fast,steering_speed_slow,steering_distance_far_fast,steering_distance_far_slow),steering_distance_far_fast,steering_distance_far_slow)
	var steering_distance_close=constrain( remap(velocity.length(),steering_speed_fast,steering_speed_slow,steering_distance_close_fast,steering_distance_close_slow),steering_distance_close_fast,steering_distance_close_slow)
	
	steer_direction=0 #0=straight
	if autosteer_enabled:
		if distance_min<steering_distance_far: #wall close, start steering away
			steer_direction += turndirection*deg_to_rad(constrain(remap(distance_min,steering_distance_far,steering_distance_close,0,steering_angle), 0,steering_angle))

	'''
	if Input.is_action_pressed("ui_left"):
		autosteer_enabled=false
		steer_direction=-1
	elif Input.is_action_pressed("ui_right"):
		autosteer_enabled=false
		steer_direction=1
	'''
	
	applied_engine_power=0
	
	var key_accelerator_pressed=Input.is_action_pressed(Gamestate.userinput_prefix+str(playerid))
	if key_accelerator_pressed:
		if running:
			#velocity = transform.x * 500
			applied_engine_power=engine_power
			
			
			if not autosteer_enabled: #start autosteer when accelerate is pressed
				autosteer_enabled=true 
				collision_enable_timer.start()
			
			if autoreset:
				autoreset=false
				
	if not running and finalTime==-1 and key_accelerator_pressed: #at start
		burnout=min(1.0,burnout+1.0*delta)
	else:
		burnout=max(0,burnout-1.0/BURNOUT_KEEP_ON_AFTER_START*delta)
	
	
	acceleration = transform.x * applied_engine_power
		
	if autoreset and running:
		acceleration = transform.x * braking #drive backwards
		
		if distance_min>=resetcar_distance: #nothing in front of car
			steer_direction=resetcar_steerangle #keep steering so turn around if standing in the middle of a track
		else:
			steer_direction*=-1 #invert steering
	else:
		if steer_direction>1:
			resetcar_steerangle=max(-resetcar_steerangle,+resetcar_steerangle) #calculate steering direction for next autoreset
		if steer_direction<1:
			resetcar_steerangle=min(-resetcar_steerangle,+resetcar_steerangle) #calculate steering direction for next autoreset
		

func calculate_steering(delta:float):
	var rear_wheel = position - transform.x *wheel_base/2.0
	var front_wheel = position + transform.x *wheel_base/2.0
	rear_wheel += velocity*delta
	front_wheel += velocity.rotated(steer_direction)*delta
	var new_heading = (front_wheel-rear_wheel).normalized()
	
	var traction = traction_slow
	if velocity.length() > slip_speed:
		traction = traction_fast
		
	
	var d = new_heading.dot(velocity.normalized())
	
	if d > 0:
		velocity = velocity.lerp(new_heading * velocity.length(), traction)
		if d<0.85 and velocity.length()>slip_speed and applied_engine_power>engine_power/2:
			sfx.sliding(velocity.length())
			$PositionFrontLeftTire.setSliding(true)
			$PositionFrontRightTire.setSliding(true)
		elif d>0.98: #keep trails a bit longer
			$PositionFrontLeftTire.setSliding(false)
			$PositionFrontRightTire.setSliding(false)
			
	elif d<0:
		velocity = - new_heading * min(velocity.length(),max_speed_reverse)
	rotation = new_heading.angle()
	
func getRound():
	var i=getNextCPindex()/checkpoints.size()
	return i
	

	
func check_markers():
	if ray_cast_car.is_colliding():
		#print("Marker: "+str(ray_cast_car.get_collider()))
		#if ray_cast_car.get_collision_mask_value(COLLISIONMASK_FINISH):
		#	print("Player "+str(playerid)+" Finished")
		#if ray_cast_car.get_collision_mask_value(COLLISIONMASK_CHECKPOINT):
		var rcc_collidername=ray_cast_car.get_collider().name
		if rcc_collidername=="area_finish":
			$label_round.showRounds(getRound())
			#print("Player "+str(playerid)+" drove through Finish")
			if getNextCPindex()==-1 and finalTime==-1: #all checkpoints have times and did not finish = Finished
				print("Player "+str(playerid)+" Finished")
				print("Final Time: "+str(Gamestate.getTimeElapsed()))
				running=false
				finalTime=Gamestate.getTimeElapsed()
				car_finished.emit(playerid,finalTime)
				
		elif rcc_collidername.begins_with("area_cp"):
			var nextcp_i=getNextCPindex()
			var checkpoint_i=checkpoints.find(rcc_collidername)
			if checkpoint_i>=0 and nextcp_i>=0: #found and there is a next checkpoint time free
				if (nextcp_i%checkpoints.size())==checkpoint_i: #this cp is next cp
					checkpointtimes[nextcp_i]=Gamestate.getTimeElapsed()
					#print("Player "+str(playerid)+" Checkpoint "+str(ray_cast_car.get_collider().name))
					#print("New CP array "+str(checkpointtimes))

func constrain(val,a,b):
	var vmin=min(a,b)
	var vmax=max(a,b)
	return min(vmax,max(vmin,val))

func getNextCPindex():
	#returns index of first 0 value in times array
	#-1 if all cps have times
	#[10.2,15.5,12.2,0,0,0,0] -> 3
	var i=0
	for cpt in checkpointtimes:
		if cpt==0:
			return i
		i+=1
	return -1

func _on_collision_enable_timer_timeout() -> void:
	collision_shape.disabled=false