@tool
extends Node

# Define a class to hold each sensor-conveyor setup
class SensorConveyorSetup:
	var sensor: DiffuseSensor
	var conveyor: BeltConveyor
	var hold_triggered: bool = false
	var hold_timer: float = 0.0
	var conveyor_direction: int = 1
	var prev_sensor_output: bool = false
	var raised_position: float
	var lowered_position: float
	var hold_duration: float
	var speed: float
	
	func _init(p_sensor: DiffuseSensor, p_conveyor: BeltConveyor, p_raised_pos: float, p_lowered_pos: float, p_hold_duration: float, p_speed: float):
		sensor = p_sensor
		conveyor = p_conveyor
		raised_position = p_raised_pos
		lowered_position = p_lowered_pos
		hold_duration = p_hold_duration
		speed = p_speed

@onready var conveyor1: BeltConveyor = $"../Conveyor3"
@onready var conveyor2: BeltConveyor = $"../Conveyor4"
@onready var conveyor3: BeltConveyor = $"../Conveyor5"
@onready var conveyor4: BeltConveyor = $"../Conveyor6"

@onready var diffuse_sensor_1: DiffuseSensor = $"../DiffuseSensor15"
@onready var diffuse_sensor_2: DiffuseSensor = $"../DiffuseSensor16"
@onready var diffuse_sensor_3: DiffuseSensor = $"../DiffuseSensor17"
@onready var diffuse_sensor_4: DiffuseSensor = $"../DiffuseSensor23"

var time = 0 
var tag_group = "TagGroup0"

# Array to hold all sensor-conveyor setups
var setups: Array[SensorConveyorSetup] = []

# Track multiple selected conveyors for multiple boxes
var pending_activations: Array[Dictionary] = []  # Array of {conveyor_index: int, direction: int}

func _ready() -> void:
	time = 0
	SimulationEvents.simulation_started.connect(_on_simulation_started)
	
	# Initialize setups - you can add more setups here
	# SensorConveyorSetup(sensor, conveyor, raised_pos, lowered_pos, hold_duration, speed)
	setups.append(SensorConveyorSetup.new(diffuse_sensor_1, conveyor1, 5.1, 4.8, 1.5, 8))
	setups.append(SensorConveyorSetup.new(diffuse_sensor_2, conveyor2, 5.1, 4.8, 1.5, 8))
	setups.append(SensorConveyorSetup.new(diffuse_sensor_3, conveyor3, 5.1, 4.8, 1.5, 8))
	setups.append(SensorConveyorSetup.new(diffuse_sensor_4, conveyor4, 5.1, 4.8, 1.5, 8))

func _on_simulation_started() -> void:
	time = 0
	pass
	#OIPComms.register_tag(tag_group, "Chute_1.CTRL.STS.Enabled", 1)

func _physics_process(delta: float) -> void:
	time = int(time + snapped(delta, 0.01) * 100)
	
	# First sensor decides which conveyor should be active for new boxes
	var first_setup = setups[0]  # This is the first sensor setup
	var first_sensor_triggered = first_setup.prev_sensor_output and not first_setup.sensor.output
	
	# If first sensor triggered, add a new selection to the queue
	if first_sensor_triggered:
		var selected_conveyor_index = randi() % setups.size()  # Randomly pick conveyor 0, 1, or 2
		var random_direction = 1 if randi() % 2 == 0 else -1  # Randomly decide direction
		pending_activations.append({"conveyor_index": selected_conveyor_index, "direction": random_direction})
		print("New box: Selected conveyor ", selected_conveyor_index, " with direction ", random_direction)
	
	# Check each pending activation to see if its sensor is triggered
	var activations_to_remove = []
	for i in range(pending_activations.size()):
		var activation = pending_activations[i]
		var conveyor_index = activation["conveyor_index"]
		var direction = activation["direction"]
		var setup = setups[conveyor_index]
		
		# Check if this conveyor's sensor is triggered and conveyor is not already active
		var sensor_triggered = setup.prev_sensor_output and not setup.sensor.output
		if sensor_triggered and not setup.hold_triggered:
			# Activate this conveyor
			setup.hold_triggered = true
			setup.hold_timer = 0.0
			setup.conveyor.position.y = setup.raised_position
			setup.conveyor.speed = setup.speed * direction
			print("Activated conveyor ", conveyor_index, " with direction ", direction)
			# Mark for removal from pending list
			activations_to_remove.append(i)
	
	# Remove completed activations (in reverse order to maintain indices)
	for i in range(activations_to_remove.size() - 1, -1, -1):
		pending_activations.remove_at(activations_to_remove[i])
	
	# Process each setup (handle timing and cleanup)
	for setup in setups:
		if setup.hold_triggered:
			setup.hold_timer += delta
			setup.conveyor.position.y = setup.raised_position
			# Keep the same speed that was randomly assigned
			if setup.hold_timer >= setup.hold_duration:
				setup.hold_triggered = false
				setup.conveyor.position.y = setup.lowered_position
				setup.conveyor.speed = 0
		else:
			setup.conveyor.position.y = setup.lowered_position
			setup.conveyor.speed = 0
	
	# Update previous sensor state for all sensors
	for setup in setups:
		setup.prev_sensor_output = setup.sensor.output
	
	#print(time)

# Helper function to check if any conveyor is currently active
func _any_conveyor_active() -> bool:
	for setup in setups:
		if setup.hold_triggered:
			return true
	return false