import queue
import threading
import pygame
import time
import sys
from  lerobot.teleoperators import Teleoperator,make_teleoperator_from_config
from .config import MixConfig
"""
混合控制器。
"""
class Mix(Teleoperator):
    config_class = MixConfig
    name = "mix"
    def __init__(self, config: MixConfig):
        self.config = config
        self.teleopList = {}
        for name,teleop in self.config.teleopList.items():
            self.teleopList[name] = make_teleoperator_from_config(teleop)
    @property
    def action_features(self):
        action_features = {}
        for name,teleop in self.teleopList.items():
            for i,item in enumerate(teleop.action_features()["names"].keys()):
                action_features[f"{name}.{item}"] = i
        return {
            "dtype": "float32",
            "shape": (len(action_features),),
            "names": action_features,
        }

    @property
    def feedback_features(self):
        return {}

    @property
    def is_connected(self):
        return all([teleop.is_connected for teleop in self.teleopList.values()])

    def connect(self, calibrate = True):
        for teleop in self.teleopList.values():
            teleop.connect(calibrate)
    @property
    def is_calibrated(self):
        return True

    def calibrate(self):
        return all([teleop.calibrate() for teleop in self.teleopList.values()])

    def configure(self):
        pass

    def get_action(self):
        data = {}
        for index,teleop in self.teleopList.items():
            for key,item in teleop.get_action().items():
                data[f"{index}.{key}"] = item
        return data

    def send_feedback(self, feedback):
        pass

    def disconnect(self):
        for teleop in self.teleopList.values():
            teleop.disconnect()