import asyncio
from utils.logging import LoggerSingleton
from fastapi import FastAPI
from robot.controller import RobotController, Cell
from utils.config import ConfigParser
from vision.datamatrix import DataMatrixReader

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

    async def run(self):
        self.logger.info("Connecting to robot controller")
        await self.controller.connect()

        while True:
            # Main robot control loop
            while True:
                slot = self.controller.get_next_free_slot()
                if not slot:
                    self.logger.debug("No free slots available")
                    break
                # Pick and place new cell
                cell_id = self.vision.read_datamatrix()
                if not cell_id:
                    self.logger.debug("Failed to read datamatrix")
                    break
                self.logger.info(f"Processing cell {cell_id}")
                cell = Cell(cell_id)
                await self.controller.pick_cell_from_feeder()
                await self.controller.insert_cell_to_slot(cell, slot)
            
            # Check for completed measurements
            if self.controller.work_queue:
                completed_slot = self.controller.work_queue.pop(0)
                cell_id = await self.controller.collect_cell_from_slot(completed_slot)
                cell = Cell(cell_id)
                await self.controller.sort_cell(cell)

            await asyncio.sleep(0.1)

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