extends KinematicBody2D

# Declare member variables here. Examples:
# var a = 2
# var b = "text"
export var speed: = Vector2(300.0, 1000.0)
export var gravity: = 4000.0

var velocity: = Vector2.ZERO



func length_Fuzzy(name: String ) -> float:
	var vectorLength = (to_global(get_node(name).position) - get_node(name).get_collision_point()).length()
	var number = [0,20,40,80,150]
	var values = [0,0,0]

# 					CLOSE

	if( vectorLength > number[0] && vectorLength <= number[1]):
		values[0] = (number[1] - vectorLength) / (number[1] - number[0] )
# 					MEDIUM
	if(vectorLength >= 0 && vectorLength <= number[2]):
		values[1] = (vectorLength - number[2]) / (number[2] - number[0])
	elif(vectorLength > number[2] && vectorLength <= number[3]):
		values[1] = (number[3] - vectorLength) / (number[3] - number[2])
#					FAR
	if(vectorLength >= number[3] && vectorLength <= number[4]):
		values[2] = (vectorLength - number[3]) / (number[4] - number[3])
	elif(vectorLength > number[4]):
		values[2] = 1
	return values
	pass




func fuzzy_logic(lewyDaleko:float , lewySrednio: float, lewyMocno: float, prostoDaleko:float , prostoSrednio: float, prostoMocno: float,prawyDaleko:float , prawySrednio: float, prawyMocno: float) -> int:

#Mocne lewo
  var ruleML1= min(lewyDaleko,min(prostoSrednio,prawyMocno))
  var ruleML2= min(lewyDaleko,min(prostoMocno,prawySrednio))
  var ruleML3= min(lewyDaleko,min(prostoMocno,prawyMocno))
  var ruleML4= min(lewyDaleko,min(prostoDaleko,prawyMocno))
  var ruleML5= min(lewySrednio,min(prostoMocno,prawyMocno))
  var ruleML6= min(lewyMocno,min(prostoMocno,prawyMocno))
  var ruleML7= min(lewySrednio,min(prostoSrednio,prawyMocno))
  var ruleML8= min(lewySrednio,min(prostoDaleko,prawyMocno))
#slabe lewo
  var ruleSL1= min(lewyDaleko,min(prostoMocno,prawyDaleko))
  var ruleSL2= min(lewyDaleko,min(prostoSrednio,prawySrednio))
  var ruleSL3= min(lewyDaleko,min(prostoDaleko,prawySrednio))
  var ruleSL6= min(lewySrednio,min(prostoMocno,prawySrednio))
#prosto
  var ruleNic1= min(lewyDaleko,min(prostoDaleko,prawyDaleko))
  var ruleNic2= min(lewyDaleko,min(prostoSrednio,prawyDaleko))
  var ruleNic3= min(lewySrednio,min(prostoDaleko,prawySrednio))
  var ruleNic4= min(lewySrednio,min(prostoSrednio,prawySrednio))
  var ruleNic5= min(lewyMocno,min(prostoSrednio,prawyMocno))
  var ruleNic6= min(lewyMocno,min(prostoDaleko,prawyMocno))
#slabe prawo
  var ruleLP1= min(lewySrednio,min(prostoDaleko,prawyDaleko))
  var ruleLP2= min(lewySrednio,min(prostoSrednio,prawyDaleko))
  var ruleLP3= min(lewyMocno,min(prostoSrednio,prawySrednio))
  var ruleLP4= min(lewyMocno,min(prostoMocno,prawySrednio))
  var ruleMP5= min(lewyMocno,min(prostoDaleko,prawySrednio))
#mocne prawo
  var ruleMP1= min(lewySrednio,min(prostoMocno,prawyDaleko))
  var ruleMP2= min(lewyMocno,min(prostoDaleko,prawyDaleko))
  var ruleMP3= min(lewyMocno,min(prostoSrednio,prawyDaleko))
  var ruleMP4= min(lewyMocno,min(prostoMocno,prawyDaleko))
#srodek ciezkosci
  var mocnoLewy = max ( ruleML1, max ( ruleML2, max ( ruleML3,max( ruleML4, max( ruleML5,max(ruleML7,max(ruleML8,ruleML6 ) ) ) ))))
  var slabyLewy = max(ruleSL1,max(ruleSL2,max(ruleSL3,ruleSL6)))
  var nic = max(ruleNic1,max(ruleNic2,max(ruleNic3,max(ruleNic4,max(ruleNic5,ruleNic6)))))
  var slabyPrawy = max(ruleLP1,max(ruleLP2,ruleLP3))
  var mocnyPrawy = max(ruleMP1,max(ruleMP2,max(ruleMP3,max(ruleLP4,max(ruleMP5,ruleMP4)))))
  var licznik= mocnoLewy * (-30) + slabyLewy * (-15) + (nic * 0) + (slabyPrawy * 15) + (mocnyPrawy * 30)
  var mianownik= mocnoLewy+slabyLewy+nic+slabyPrawy+mocnyPrawy
  var skret = 0
  print(mocnoLewy,slabyLewy,nic,slabyPrawy,mocnyPrawy)

  skret = licznik/mianownik

  print(" ", skret)
  return skret

  pass


# Called when the node enters the scene tree for the first time.
func _physics_process(delta: float) -> void:
	var timeScale: = delta * 200


	#print(path, " ", steering)


	var numbersRight = length_Fuzzy("left")
	var numbersLeft = length_Fuzzy("right")
	var numbersStraight = length_Fuzzy("middle")

	var  kat = fuzzy_logic(numbersLeft[0],numbersLeft[1],numbersLeft[2],numbersStraight[2],numbersStraight[1],numbersStraight[0],numbersRight[0],numbersRight[1],numbersRight[2])


	var rot:=  self.get_rotation()


	rotate(deg2rad(kat))



	move_and_collide(Vector2.RIGHT.rotated(rot) * timeScale )





	#if (RotateValueLeft >  RotateValueRight && abs(RotateValueLeft - RotateValueRight) > 1.25):
		#rotate(deg2rad(timeScale * RotateValueLeft))
	#elif(RotateValueLeft <  RotateValueRight && abs(RotateValueLeft - RotateValueRight) > 1.25):
		#rotate(-deg2rad(timeScale * RotateValueRight))
	#else:
		#pass

	# 					ZLE FUZZY LOGIC!
	#if(steering == 1 ):
	#	rotate(deg2rad(timeScale * 1.5))
	#elif(steering == -1):
	#	rotate(-deg2rad(timeScale * 1.5))
	#move_and_collide(Vector2.RIGHT.rotated(rot) * timeScale )



# 						Gracz
#	if(Input.get_action_strength("ui_right") > 0 && Input.get_action_strength("ui_up") > 0):
#		rotate(deg2rad(timeScale))
#	if(Input.get_action_strength("ui_left") > 0 && Input.get_action_strength("ui_up") > 0):
#		rotate(-deg2rad(timeScale))
#	if(Input.get_action_strength("ui_up") > 0):
#		move_and_collide(Vector2.RIGHT.rotated(rot) *5)


func _ready() -> void:
	get_node("left").set_collide_with_bodies(true)
	get_node("middle").set_collide_with_bodies(true)
	get_node("right").set_collide_with_bodies(true)
	pass




# Called every frame. 'delta' is the elapsed time since the previous frame.
#func _process(delta):
#	pass