import time
from enum import Enum

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


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:
                self.robotClient.sendIOControl(self.robotClient.con_ios[0],1)
                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:
                self.robotClient.sendIOControl(self.robotClient.con_ios[0], 0)
                for _ in range(self.drop_count):
                    self.robotClient.sendIOControl(self.robotClient.con_ios[1], 1, delay=self.robotClient.time_delay_put)
                    self.robotClient.sendIOControl(self.robotClient.con_ios[1], 0)

                self.robotClient.sendIOControl(self.robotClient.con_ios[1], 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:
            if not self.shake_continue.Q(True, 600):       # 1500
                self.shake_Q = not self.shake_Q   #   10
                self.robotClient.sendIOControl(self.robotClient.con_ios[2], 1 if self.shake_Q else 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)
                print("震动结束")

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


    def take_bag(self):
        return True

    def press_bag(self):
        return True

    def shake_bag(self):
        return True