#!/usr/bin/python3
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#这里到时候可以改成True，然后加一个获取继电器状态变成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:
            # 这里可以添加继电器状态获取，把is_send_command = False
            if not self.is_send_take_command:
                # 本身IO
                #self.robotClient.sendIOControl(self.robotClient.con_ios[0],1)
                # 网络继电器
                close(1, 0, 0) #逻辑关闭电磁阀1
                self.is_send_take_command = True

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

                # self.robotClient.sendIOControl(self.robotClient.con_ios[1], 1)
                #close(0, 0, 1)
                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, 6000):
            #if self.shake_Q:
                    #open(0, 1, 0)
                #else:
                    #close(0, 1, 0)
            #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, 1, 0)
                print("震动结束")

        if self.catch_status == CatchStatus.COk:
            self.shake_continue.SetReset()
            # self.robotClient.sendIOControl(self.robotClient.con_ios[1], 0,emptyList='1')
            open(1,0,0)#逻辑
            #close(0, 1, 0)
            #close(0, 0, 1)
            self.is_send_take_command = False
            pass


    def take_bag(self):
        return True

    def press_bag(self):
        return True

    def shake_bag(self):
        return True