from Model.Position import Real_Position
from enum import Enum

class FeedStatus(Enum):
    FNone = 0
    FStart = 1
    FSafeP = 2
    FPhoto = 3
    FTake = 4
    FSafeF = 5
    FFeedP = 6
    FFinished = 7

class FeedLine:
    def __init__(self,safe_position:Real_Position,photo_position:Real_Position,feed_position:Real_Position):
        self.safe_position = safe_position
        self.photo_position = photo_position
        self.feed_position = feed_position
        self.take_position = None


class FeedingConfig:
    def __init__(self, num:int, feedLine:FeedLine):
        self.num = num
        self.feedLine = feedLine


    def get_line_info(self):
        pass


class Feeding():
    def __init__(self,robotClient:RobotClient):
        self.feedConfig = None
        self.feedStatus = FeedStatus.FNone
        self.robotClient = robotClient
        pass

    def run(self):
        # 获取事件坐标
        real_position = Real_Position()
        real_position.init_position(self.robotClient.status_model.world_n[0],
                                    self.robotClient.status_model.world_n[1],
                                    self.robotClient.status_model.world_n[2],
                                    self.robotClient.status_model.world_n[3],
                                    self.robotClient.status_model.world_n[4],
                                    self.robotClient.status_model.world_n[5],
                                    self.robotClient.status_model.world_n[6]);

        if self.feedStatus==FeedStatus.FNone:

            pass
        elif self.feedStatus==FeedStatus.FStart:
            if self.feedConfig.num != 0:
                self.feedStatus = FeedStatus.FSafeP

                sendTargPosition(self.feedConfig.safe_position)
                #print(request_command)

            pass
        elif self.feedStatus==FeedStatus.FSafeP:

             if self.feedConfig.safe_position.compare(real_position):
                self.feedStatus = FeedStatus.FPhoto
                sendTargPosition(self.feedConfig.photo_position)

        elif self.feedStatus == FeedStatus.FPhoto:
            if self.feedConfig.photo_position.compare(real_position):
                self.feedStatus = FeedStatus.FTake
                pass # 发送拍照获取坐标 并 开始移动

        elif self.feedStatus == FeedStatus.FTake:
            if self.feedConfig.take_position != None and self.feedConfig.take_position.compare(real_position):
                self.feedStatus = FeedStatus.FSafeF
                pass #打开吸嘴并返回

        elif self.feedStatus == FeedStatus.FSafeF:
            if self.feedConfig.safe_position.compare(real_position):
                self.feedStatus = FeedStatus.FFeedP
                sendTargPosition(self.feedConfig.feed_position)
                pass  #吸嘴开始


        elif self.feedStatus==FeedStatus.FFeedP:
            if self.feedConfig.feed_position.compare(real_position):
                self.feedConfig.num = self.feedConfig.num-1
                if self.feedConfig.num == 0:
                    self.feedStatus=FeedStatus.FNone
                else:
                    self.feedStatus = FeedStatus.FSafeP
                    sendTargPosition(self.feedConfig.safe_position)
                pass

    def sendTargPosition(self,real_position):
        position_instruction = Instruction()
        position_instruction.m0 = self.feedConfig.safe_position.X
        position_instruction.m1 = self.feedConfig.safe_position.Y
        position_instruction.m2 = self.feedConfig.safe_position.Z
        position_instruction.m3 = self.feedConfig.safe_position.U
        position_instruction.m4 = self.feedConfig.safe_position.V
        position_instruction.m5 = self.feedConfig.safe_position.W
        instruction_command = CMDInstructRequest()
        instruction_command.instructions.append(vars(position_instruction))
        request_command = vars(instruction_command)
        self.robotClient.add_sendQuene(request_command)
        pass