import time
from enum import Enum

import Constant
from COM.COM_Robot import RobotClient
from Util.util_time import CClockPulse, CTon
from EMV import *

class CatchStatus(Enum):
    CNone = 0
    CTake = 1
    CDrop = 2
    CShake = 3
    COk = 4

class Catch:
    def __init__(self, robotClient: RobotClient):
        self.robotClient = robotClient
        self.catch_status = CatchStatus.CNone
        self.shake_continue = CTon()
        self.drop_continue = CTon()
        self.take_continue = CTon()
        self.shakePulse = CClockPulse()
        self.shake_count = 5
        self.drop_count = 2
        self.is_send_command = False
        self.is_send_take_command = False
        self.shake_Q = False
    def run(self):
        if self.catch_status == CatchStatus.CNone:
            return

        if self.catch_status == CatchStatus.CTake:
            if not self.is_send_take_command:
                # 本身IO
                ##self.robotClient.sendIOControl(self.robotClient.con_ios[0],1)
                # 网络继电器
                open(1, 0, 0)
                self.is_send_take_command = True
            else:
                if self.take_continue.Q(True,self.robotClient.time_delay_take*1000):
                    self.is_send_take_command = False
                    self.catch_status = CatchStatus.COk

        if self.catch_status == CatchStatus.CDrop:
            if not self.is_send_command:
                # 本身IO
                # self.robotClient.sendIOControl(self.robotClient.con_ios[0], 0)
                # 网络继电器
                close(1, 0, 0)
                for _ in range(self.drop_count):
                    # self.robotClient.sendIOControl(self.robotClient.con_ios[1], 1, delay=self.robotClient.time_delay_put)
                    open(0, 1, 0)
                    time.sleep(self.robotClient.time_delay_put) # 会造成这个时间点 其他命令插入不进去 需要另开线程
                    close(0, 1, 0)
                    # self.robotClient.sendIOControl(self.robotClient.con_ios[1], 0)

                # self.robotClient.sendIOControl(self.robotClient.con_ios[1], 1)
                open(0, 1, 0)
                self.is_send_command = True
            if self.drop_continue.Q(True,self.robotClient.time_delay_put*1000*self.drop_count):
                # if Constant.Debug or self.robotClient.check_outputQ(self.robotClient.con_ios[1]) and not self.robotClient.check_outputQ(self.robotClient.con_ios[0]):
                self.is_send_command = False
                self.catch_status = CatchStatus.COk
                self.drop_continue.SetReset()

        if self.catch_status == CatchStatus.CShake:     # 1500
            self.shake_Q = not self.shake_Q   #   10
            if not self.shake_continue.Q(True, 600):
                if self.shake_Q:
                    open(0, 0, 1)
                else:
                    close(0, 0, 1)
            else:
                self.shake_continue.SetReset()
                self.catch_status = CatchStatus.COk
                #if Constant.Debug or self.robotClient.check_outputQ(self.robotClient.con_ios[2]):
                # self.robotClient.sendIOControl(self.robotClient.con_ios[2], 0)
                close(0, 0, 1)
                print("震动结束")

        if self.catch_status == CatchStatus.COk :
            # self.robotClient.sendIOControl(self.robotClient.con_ios[1], 0,emptyList='1')
            close(0, 1, 0)
            close(0, 0, 1)
            pass


    def take_bag(self):
        return True

    def press_bag(self):
        return True

    def shake_bag(self):
        return True