"""
E.Drake - ENGN 2220
Jan 21, 2026

TESTING GEOMETRY
"""
import matplotlib
matplotlib.use('QtAgg')
import matplotlib.pyplot as plt
from PyQt6 import QtWidgets
import sys
import config
#print(str(config.GEO_DIR))
sys.path.insert(0,str(config.GEO_DIR))
sys.path.insert(0,str(config.PATH_DIR))
from geo_current import geo_current

import jupedsim as jps
from matplotlib.patches import Circle

def main_loop():
    geometry,doors = geo_current(full_plot = True)
    dk_3, d4_6, d7_12, d_exit = doors
    
    model = jps.CollisionFreeSpeedModel()
    sim = jps.Simulation(model=model,geometry=geometry)
    exit_id = sim.add_exit_stage(d_exit)
    journey = jps.JourneyDescription([exit_id])
    journey_id = sim.add_journey(journey)
    total_sim_time = 60.0
    
    doorways = {
        0: dk_3,
        1: d4_6,
        2: d7_12,
    }

    # Spawn times for each door (seconds)
    spawn_schedule = {
        "door_1": [0.0, 5.0, 10.0],   # Agents at t=0, 5, 10
        "door_2": [2.0, 7.0],
        "door_3": [3.0],
    }
    events = []
    for door_name, times in spawn_schedule.items():
        for t in times:
            events.append((t, doors[door_name]))
    events.sort(key=lambda x: x[0])

    event_index = 0
    while sim.elapsed_time() < total_sim_time:
        current_time = sim.elapsed_time()
        # Process all events whose time has come
        while event_index < len(events) and events[event_index][0] <= current_time:
            _, door_pos = events[event_index]
            agent_params = jps.CollisionFreeSpeedModelAgentParameters(
                position=door_pos,
                journey_id=journey_id,
                stage_id=exit_id,
                radius=0.2,
                )
            sim.add_agent(agent_params)
            event_index += 1
        sim.iterate()


if __name__ == "__main__":
    app = QtWidgets.QApplication(sys.argv)
    main_loop()
    plt.show(block=False)
    sys.exit(app.exec())