import time

import cv2

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


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

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

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
        self.detection = detection
        self.detection_image = None
        self.init_detection_image()
        pass

    def init_detection_image(self):
        self.detection_image = cv2.imread("./Image/wait.png")
    def run(self):
        # 获取事件坐标
        real_position = Real_Position()
        real_position.init_position(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)
        # real_position.init_position(0,
        #                             0,
        #                             0,
        #                             0,
        #                             0,
        #                             0);

        if self.feedConfig == None:
            self.feedStatus = FeedStatus.FNone
        elif self.feedConfig.num == 0:
            self.feedStatus = FeedStatus.FNone

        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.feedLine.safe_position.compare(real_position):
                self.feedStatus = FeedStatus.FPhoto
                self.sendTargPosition(self.feedConfig.feedLine.photo_position)

        elif self.feedStatus == FeedStatus.FPhoto:
            if self.feedConfig.feedLine.photo_position.compare(real_position):
                code, img, xyz, uvw = self.detection.get_position() #检测结果
                self.detection_image = img
                if xyz!=None:

                    #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-10,
                                        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.feedLine.take_position = Real_Position().init_position(*target_position[:3],*noraml_base)
                    self.feedStatus = FeedStatus.FTake
                    self.sendTargPosition(self.feedConfig.feedLine.take_position)
                else:
                    print("打印日志，保存失败图像")


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

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

        elif self.feedStatus==FeedStatus.FFeedP:
            if self.feedConfig.feedLine.feed_position.compare(real_position):
                self.feedStatus = FeedStatus.FBroken
                self.sendTargPosition(self.feedConfig.feedLine.broken_position)
                pass #破袋

        elif self.feedStatus==FeedStatus.FBroken:
            if self.feedConfig.feedLine.broken_position.compare(real_position):
                self.feedConfig.num = self.feedConfig.num-1
                if self.feedConfig.num == 0:
                    self.feedStatus=FeedStatus.FNone
                    self.sendTargPosition(self.feedConfig.feedLine.safe_position)
                    self.init_detection_image()
                else:
                    self.feedStatus = FeedStatus.FSafeP
                    self.sendTargPosition(self.feedConfig.feedLine.safe_position)
        pass

    def sendTargPosition(self,real_position):
        from Model.RobotModel import Instruction
        position_instruction = Instruction()
        position_instruction.speed = Constant.speed
        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
        position_instruction.action = MoveType.WORLD.value
        instruction_command = CMDInstructRequest()
        instruction_command.instructions.append(position_instruction)
        request_command = instruction_command.toString()
        print(request_command)
        self.robotClient.add_sendQuene(request_command)
        pass