import asyncio
from robot_control.src.utils.logging import LoggerSingleton
from robot_control.src.robot.controller import RobotController
from robot_control.src.utils.config import ConfigParser
from robot_control.src.vision.datamatrix import DataMatrixReader

class LoaderSystem:
    def __init__(self):
        self.config = ConfigParser()
        self.logger = LoggerSingleton.get_logger(self.config)
        self.controller = RobotController(self.config)
        cam = self.config.get_vision_config().get("camera_id")
        self.vision = DataMatrixReader(cam)
        self.logger.info("Initializing LoaderSystem")
        self.vision.initialize()

    async def run(self):
        await self.controller.connect()

        # Main robot control loop
        while True:
            await asyncio.sleep(0.1) # avoid busy loop

            #Check for free slots loop
            while True:
                slot = self.controller.get_next_free_slot()
                if not slot:
                    break
                # Pick and place new cell
                cell_id = self.vision.read_datamatrix()
                if not cell_id:
                    self.logger.debug("No cell detected")
                    break
                self.logger.info(f"Processing cell {cell_id}")
                cell = self.controller.add_cell(cell_id)
                try:
                    await self.controller.pick_cell_from_feeder()
                    await self.controller.insert_cell_to_slot(cell, slot)
                except Exception as e:
                    self.logger.error(f"Failed to process cell {cell_id}: {str(e)}")
                    break
            
            # Check for completed measurements and sort cell
            await self.controller.process_finished_measurement()


if __name__ == "__main__":
    loader_system = LoaderSystem()
    asyncio.run(loader_system.run())