from Model.Position import Real_Position, Detection_Position
from enum import Enum
from COM.COM_Robot import RobotClient
from Model.RobotModel import CMDInstructRequest
from Vision.camera_coordinate_dete import Detection


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,detection: Detection):
        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]);
        real_position.init_position(0,
                                    0,
                                    0,
                                    0,
                                    0,
                                    0);


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

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

            pass
        elif self.feedStatus==FeedStatus.FSafeP:

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

        elif self.feedStatus == FeedStatus.FPhoto:
            if self.feedConfig.photo_position.compare(real_position):
                _, _, xyz, uvw = self.detection.get_position() #检测结果
                if xyz!=None:
                    self.feedStatus = FeedStatus.FTake
                    #dp = Detection_Position().init_position(*xyz, *uvw)
                    from Trace.handeye_calibration import R_matrix,getPosition
                    rotation = R_matrix(self.robotClient.status_model.world_0,
                                        self.robotClient.status_model.world_1,
                                        self.robotClient.status_model.world_2,
                                        self.robotClient.status_model.world_3,
                                        self.robotClient.status_model.world_4,
                                        self.robotClient.status_model.world_5)

                    # 黄老师给我的xyz和法向量
                    target_position, noraml_base = getPosition(*xyz, *uvw,rotation)
                    self.feedConfig.take_position = Real_Position().init_position(*target_position,*noraml_base)
                else:
                    print("打印日志，保存失败图像")
                    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
                self.sendTargPosition(self.feedConfig.feedLine.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
                    self.sendTargPosition(self.feedConfig.safe_position)
        pass

    def sendTargPosition(self,real_position):
        from Model.RobotModel import Instruction
        position_instruction = Instruction()
        position_instruction.m0 = real_position.X
        position_instruction.m1 = real_position.Y
        position_instruction.m2 = real_position.Z
        position_instruction.m3 = real_position.U
        position_instruction.m4 = real_position.V
        position_instruction.m5 = real_position.W
        instruction_command = CMDInstructRequest()


        instruction_command.instructions.append(position_instruction)
        request_command = instruction_command.toString()
        self.robotClient.add_sendQuene(request_command)
        pass