"""
main.py - Entry point using object-oriented Robot and Input abstractions
"""
from threading import Thread
from multiprocessing import Queue
import time

import config as cfg
from Robot import Robot, HardwareBackend, PyBulletBackend
from simulation import Simulation
from EspCommunication import ESP32Communication
from ArduinoCommunication import ArduinoCommunication
from inputs.PygameController import controller_loop


def create_robot_instance() -> Robot:
    if cfg.sim:
        sim_instance = Simulation()
        backend = PyBulletBackend(sim_instance)
    else:
        comm = ArduinoCommunication() if cfg.arduinoConnection else ESP32Communication()
        comm.start()
        backend = HardwareBackend(comm)

    return Robot(backend=backend)


def robot_control_loop(robot: Robot, control_queue: Queue):
    robot.reset_to_init()
    time.sleep(1)

    tick = 0.05
    next_time = time.time()

    while True:
        next_time += tick

        # Drain the queue to get the latest command frame
        latest_cmd = None
        while not control_queue.empty():
            latest_cmd = control_queue.get_nowait()

        if latest_cmd is not None:
            robot.robot_state = latest_cmd["state"]
            robot.vector_dirmov = latest_cmd["dirmov"]

        # Run state machine tick
        robot.tick()

        sleep_time = next_time - time.time()
        if sleep_time > 0:
            time.sleep(sleep_time)


if __name__ == "__main__":
    controller_queue = Queue()
    
    my_robot = create_robot_instance()

    # Start Pygame input thread
    input_thread = Thread(target=controller_loop, args=(controller_queue,), daemon=True)
    robot_thread = Thread(target=robot_control_loop, args=(my_robot, controller_queue), daemon=True)

    input_thread.start()
    robot_thread.start()

    robot_thread.join()