import logging
from types import SimpleNamespace

# 模拟 Constant
class Constant:
    str_feed_take = "[测试] 开始抓料流程"
    str_feed_take_success = "[测试] ✅ 抓料成功"
    str_feed_takePhoto_fail = "[测试] ❌ 抓料点位获取失败"

# 模拟 FeedStatus 和 CatchStatus
class FeedStatus:
    FTake = "FTake"

class CatchStatus:
    CNone = "CNone"
    CTake = "CTake"
    COk = "COk"

class DetectStatus:
    DOk = "DOk"

# 模拟 Position 类
class Position:
    def __init__(self, x=0, y=0, z=0, a=0, b=0, c=0):
        self.X = x
        self.Y = y
        self.Z = z
        self.A = a
        self.B = b
        self.C = c

    def get_position(self):
        return self

    def compare(self, other, is_action=False):
        return (
            abs(self.X - other.X) < 0.1 and
            abs(self.Y - other.Y) < 0.1 and
            abs(self.Z - other.Z) < 0.1
        )

# 模拟 Detect 类
class Detect:
    def __init__(self):
        self.detect_position = None
        self.position_index = 0
        self.detect_status = DetectStatus.DOk
        # 模拟配置点位（类似 list.ini）
        self.positions = {
            0: Position(100, 200, 300, 0, 0, 0),
            1: Position(150, 250, 350, 0, 0, 0),
            2: Position(200, 300, 400, 0, 0, 0),
            3: Position(100, 200, 300, 0, 0, 0),
            4: Position(150, 250, 350, 0, 0, 0),
            5: Position(200, 300, 400, 0, 0, 0),

        }

    def run(self):
        """根据当前索引加载点位"""
        pos = self.positions.get(self.position_index, None)
        if pos:
            self.detect_position = pos
            return True
        else:
            print(f"[❌ 失败] 索引 {self.position_index} 没有对应点位")
            return False

# 模拟 FeedLine 类
class FeedLine:
    def __init__(self):
        self.drop_position = Position()

    def get_take_position(self):
        return SimpleNamespace(get_position=lambda: Position(100, 200, 300))

    def set_drop_position(self, position):
        self.drop_position = position

# 模拟 RelayController
class RelayController:
    def get_all_device_status(self, *args):
        return {"SENSOR2": True}

    def open(self, conveyor2=False):
        pass

# 模拟主类，用于测试
class TestRunner:
    def __init__(self):
        self.feedStatus = FeedStatus.FTake
        self.catch = SimpleNamespace(catch_status=CatchStatus.CNone)
        self.detect = Detect()
        self.debug_run_count = 0
        self.sensor2_ready = True
        self.relay_controller = RelayController()

        # feedConfig 模拟
        self.feedConfig = SimpleNamespace(feedLine=FeedLine())

        # real_position 模拟
        self.real_position = Position(100, 200, 300)

    def log_signal(self, level, msg):
        logging.log(level, msg)

    def next_position(self):
        self.log_signal(logging.INFO, "[调试] 模拟移动到下一个位置")

    def run_test(self):
        if self.feedStatus == FeedStatus.FTake:
            self.log_signal(logging.INFO, Constant.str_feed_take)
            take_position = self.feedConfig.feedLine.get_take_position()
            if not take_position or not take_position.get_position():
                self.log_signal(logging.ERROR, Constant.str_feed_takePhoto_fail)
                return

            if not take_position.get_position().compare(self.real_position, is_action=True):
                self.log_signal(logging.INFO, "🟡 机器人尚未到达抓料点位")
                return

            self.log_signal(logging.INFO, "🟢 机器人已到达抓料点位")

            # 传感器等待逻辑（模拟已触发）
            sensors = self.relay_controller.get_all_device_status('sensors')
            sensor2_value = sensors.get("SENSOR2", False)
            if not sensor2_value:
                self.log_signal(logging.INFO, "⏳ 等待传感器2料包信号...")
                return

            self.log_signal(logging.INFO, "✅ 传感器2检测到料包到位，开始执行抓取")

            # 执行抓取动作
            if self.catch.catch_status == CatchStatus.CNone:
                self.catch.catch_status = CatchStatus.CTake
            if self.catch.catch_status == CatchStatus.CTake:
                self.log_signal(logging.INFO, "正在执行抓料动作...")
                self.catch.catch_status = CatchStatus.COk
            if self.catch.catch_status == CatchStatus.COk:
                self.log_signal(logging.INFO, Constant.str_feed_take_success)
                if not self.sensor2_ready:
                    self.log_signal(logging.INFO, "抓取完成，重新启动 conveyor2")
                    self.relay_controller.open(conveyor2=True)

                self.catch.catch_status = CatchStatus.CNone
                self.detect.detect_status = DetectStatus.DOk
                self.log_signal(logging.INFO, "修改抓取点")
                print(f"[调试] 即将丢包点位索引: {self.detect.position_index}")
                # ✅ 索引递增
                self.detect.position_index += 1
                print(f"[调试] 下一个要加载的索引: {self.detect.position_index}")

                # ✅ 加载新点位
                if not self.detect.run():
                    self.log_signal(logging.ERROR, "❌ 加载新点位失败，停止流程")
                    return

                # ✅ 设置丢包点
                self.feedConfig.feedLine.set_drop_position(self.detect.detect_position)

                self.debug_run_count += 1
                self.next_position()
                return
            else:
                self.log_signal(logging.ERROR, Constant.str_feed_takePhoto_fail)

# ========================
# 主程序入口
# ========================
if __name__ == "__main__":
    logging.basicConfig(level=logging.INFO, format='%(asctime)s [%(levelname)s] %(message)s')

    runner = TestRunner()
    print("🔧 开始测试索引递增和点位加载逻辑...\n")

    for i in range(5):
        print(f"\n🔄 第 {i+1} 次测试")
        runner.run_test()