from enum import Enum

import numpy as np
from PySide6.QtCore import Signal
import Constant
from Model.Position import Real_Position
from Trace.handeye_calibration import getPosition
from Vision.camera_coordinate_dete import Detection


class DetectStatus(Enum):
    DNone = 0
    DDetect = 1
    DOk = 2

class Detect:
    update_detect_image = Signal(np.ndarray)
    def __init__(self):
        self.detection = Detection(alignmentType='depth2color')
        self.detect_status = DetectStatus.DNone
        self.detect_position = None
    def run(self):
        if self.detect_status == DetectStatus.DNone:
            return
        if self.detect_status == DetectStatus.DDetect:
            if Constant.Debug:
                self.detect_status = DetectStatus.DOk
                return
            _, img, xyz, uvw, points = self.detection.get_position(Point_isVision=False, Box_isPoint=True,
                                                                   First_Depth=True, Iter_Max_Pixel=30,
                                                                   save_img_point=1, Height_reduce=30, width_reduce=30)

            self.detection_image = img.copy()
            if xyz == None or uvw == None or points == None:
                self.detect_position = None
                self.detect_status = DetectStatus.DOk
                return
            target_position, noraml_base = getPosition(*xyz, *uvw, None, points)

            position = Real_Position().init_position(*target_position[:3], *noraml_base[:3])
            position.Z = position.Z + 200
            self.detect_position = position
            self.detect_status = DetectStatus.DOk

        if self.detect_status == DetectStatus.DOk:
            return