import math

from Constant import position_accuracy
class Position:
    def __init__(self):
        self.X = 0.0
        self.Y = 0.0
        self.Z = 0.0
        self.U = 0.0
        self.V = 0.0
        self.W = 0.0

    def compare(self,position):
        distance = math.sqrt((self.X-position.X)**2+
                             (self.Y-position.Y)**2+
                             (self.Z - position.Z)**2+
                             (self.U - position.U)**2+
                             (self.V - position.V)**2+
                             (self.W - position.W) ** 2)
        if distance<=position_accuracy:
            return True
        else:
            return False

    # def compare(self,position):
    #     if self.X-position.X<position_accuracy and \
    #         self.Y-position.Y<position_accuracy and \
    #         self.Z - position.Z < position_accuracy and \
    #         self.U - position.U < position_accuracy and \
    #         self.V - position.V < position_accuracy and \
    #         self.W - position.W < position_accuracy:
    #         return True
    #     else:
    #         return False
    #     pass

    def is_error_angel_move(self,position,interval):
        if self.X - position.X >= interval or \
                self.Y - position.Y >= interval or \
                self.Z - position.Z >= interval or \
                self.U - position.U >= interval or \
                self.V - position.V >= interval or \
                self.W - position.W >= interval:
            return True
        else:
            return False
        pass
"""
摄像头识别位置和角度
"""
class Detection_Position(Position):
    def __init__(self):
        super().__init__()

    def init_position(self,X,Y,Z,U,V,W):
        self.X = X
        self.Y = Y
        self.Z = Z
        self.U = U
        self.V = V
        self.W = W


class Real_Position(Position):
    def __init__(self):
        super().__init__()

    def init_position(self, X, Y, Z, U, V, W):
        self.X = X
        self.Y = Y
        self.Z = Z
        self.U = U
        self.V = V
        self.W = W
        return self

    def to_string(self):
        return "X:{:.3f},Y:{:.3f},Z:{:.3f},U:{:.3f},V:{:.3f},W:{:.3f}".format(self.X,self.Y,self.Z,self.U,self.V,self.W)