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.shakePulse = CClockPulse()
        self.shake_count = 5
    def run(self):
        if self.catch_status == CatchStatus.CNone:
            return

        if self.catch_status == CatchStatus.CTake:
            self.robotClient.sendIOControl(self.robotClient.con_ios[0],1)
            if self.robotClient.check_outputQ(self.robotClient.con_ios[0]) or Constant.Debug:
                self.catch_status = CatchStatus.COk

        if self.catch_status == CatchStatus.CDrop:
            self.robotClient.sendIOControl(self.robotClient.con_ios[0],0)
            self.robotClient.sendIOControl(self.robotClient.con_ios[1],1)
            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.catch_status = CatchStatus.COk
        if self.catch_status == CatchStatus.CShake:
            if not self.shake_continue.Q(True, 1500):
                shakeQ = self.shakePulse.Q(True, 50, 50)
                self.robotClient.sendIOControl(self.robotClient.con_ios[2], 1 if shakeQ else 0)
                print("正在震动" if shakeQ else "震动结束")
            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)
            pass


    def take_bag(self):
        return True

    def press_bag(self):
        return True

    def shake_bag(self):
        return True