import cv2
import time
from detect_bag import detect_bag
#这个要注意放在Feeding同一目录下是这样调用EMV的
from EMV.EMV import RelayController

THRESHOLD_X = 537       # 到位阈值
THRESHOLD_maxX = 1430  # 掉出滚筒阈值

relay_controller = RelayController()

# ==================================================
# 不同料包的滚筒控制逻辑
# ==================================================
def handle_bag_motor(cls, status_bool, status_text):
    """
    滚筒控制总逻辑：
    - 没检测到料包 → 不发信号
    - 未到位 → 不发信号
    - 掉出滚筒 → 报警（不再操作滚筒）
    - 到位：
        bag   → 立刻停止滚筒
        bag35 → 持续正转1.5s反转1.5秒 → 停止
    """

    # 没检测到料包
    if cls is None:
        return

    # 掉出滚筒（最高优先级）
    if status_text == "料包掉出滚筒":
        print("料包掉出滚筒 → 报警 / 停机")
        relay_controller.close(conveyor2=True)
        relay_controller.close(conveyor2_reverse=True)
        return

    # 未到位 → 什么都不做
    if status_bool is not True:
        return

    # ================== 到位 + 分类 ==================
    if cls == "bag":
        print("[bag] 到位 → 立刻停止滚筒")
        relay_controller.close(conveyor2=True)

    elif cls == "bag35":
        print("[bag35] 到位 → 持续正转滚筒1.5秒 后，反转滚筒 1.5 秒 到原位置→ 停止滚筒")
        time.sleep(1.5)
        #不确定能不能直接正转变成反转，加一个停止吧
        relay_controller.close(conveyor2=True)
        relay_controller.open(conveyor2_reverse=True)
        time.sleep(1.5)
        relay_controller.close(conveyor2_reverse=True)

    else:
        # 预留扩展
        return


# ==================================================
# 料袋状态判断
# ==================================================
def bag_judgment(img, return_conf=True, return_vis=False):
    """
    判断图片中的料袋状态
    """
    cls = None
    conf = None
    min_x = None
    vis_img = None

    # ================== 唯一检测调用 ==================
    if return_vis:
        cls, conf, min_x, vis_img = detect_bag(img, return_vis=True)
    else:
        cls, conf, min_x = detect_bag(img, return_vis=False)

    # ================== 状态判断 ==================
    if min_x is None:
        status_bool = None
        status_text = "没有料袋"

    elif min_x > THRESHOLD_maxX:
        status_bool = False
        status_text = "料包掉出滚筒"

    elif THRESHOLD_X <= min_x <= THRESHOLD_maxX:
        status_bool = True
        status_text = "料袋到位"

    else:
        status_bool = False
        status_text = "料袋未到位"

    # ================== 滚筒控制 ==================
    handle_bag_motor(cls, status_bool, status_text)

    # ================== 返回 ==================
    if not return_conf:
        conf = None
    if not return_vis:
        vis_img = None

    return status_bool, status_text, conf, min_x, vis_img


# ====================== 测试 ======================
if __name__ == "__main__":
    IMG_PATH = "./test_image/3.jpg"
    img = cv2.imread(IMG_PATH)
    if img is None:
        raise FileNotFoundError(f"图片无法读取: {IMG_PATH}")
    #这里面包含 handle_bag_motor滚筒控制，只要你记得后面机械臂抓完包之后要打开滚筒，Feeding里self.relay_controller.open(conveyor2=True)
    status_bool, status_text, conf, min_x, vis_img = bag_judgment(
        img,
        return_conf = True,
        return_vis = False
    )

    print(
        f"判断结果: {status_bool}, "
        f"中文状态: {status_text}, "
        f"conf={conf}, min_x={min_x}"
    )

    if vis_img is not None:
        cv2.imshow("Vis", vis_img)
        cv2.waitKey(0)
        cv2.destroyAllWindows()