import numpy as np

from rq_interfaces.msg import RegionOfIntrest


class PyRegionOfIntrest():
    def __init__(self, start_point_mm: np.ndarray, end_point_mm: np.ndarray, frame_id: str = 'object'):
        self.start_point_mm = start_point_mm
        self.end_point_mm = end_point_mm
        self.frame_id = frame_id

    @classmethod
    def dummy(cls):
        return cls(np.ones(3,) * -1, np.ones(3,))

    @classmethod
    def from_message(cls, msg: RegionOfIntrest):
        start_point = np.array([msg.start_point_mm.x,
                                msg.start_point_mm.y,
                                msg.start_point_mm.z])
        
        end_point = np.array([msg.end_point_mm.x,
                              msg.end_point_mm.y,
                              msg.end_point_mm.z])
        frame = msg.header.frame_id
        
        return cls(start_point, end_point, frame)
    
    def as_message(self) -> RegionOfIntrest:
        message = RegionOfIntrest()

        message.start_point_mm.x = self.start_point_mm[0]
        message.start_point_mm.y = self.start_point_mm[1]
        message.start_point_mm.z = self.start_point_mm[2]

        message.start_point_mm.x = self.start_point_mm[0]
        message.start_point_mm.y = self.start_point_mm[1]
        message.start_point_mm.z = self.start_point_mm[2]

        message.header.frame_id = self.frame_id

        return message