import numpy as np

from numpy import ndarray
from .projection_geometry import PyProjectionGeometry

from rq_interfaces.msg import Projection, ProjectionGeometry

class PyProjection(PyProjectionGeometry):
    def __init__(self, focal_spot_mm: ndarray, detector_postion_mm: ndarray, detector_orientation_quad: ndarray, image: np.ndarray,
                 detector_heigth_mm: float, detector_width_mm: float, frame_id: str = 'object') -> None:
        super().__init__(focal_spot_mm, detector_postion_mm, detector_orientation_quad, frame_id)
        self.image = image
        self.detector_heigth_mm = detector_heigth_mm
        self.detector_width_mm = detector_width_mm

    @classmethod
    def dummy(cls):
        return cls(np.array([1., 0., 0]), 
                   np.array([-1., 0., 0]),
                   np.array([1., 0., 0, 1.]),
                   np.zeros((10, 10)),
                   10., 10.)

    @classmethod
    def from_message(cls, msg: Projection):
        focal_spot_mm = np.array([msg.projection_geometry.focal_spot_postion_mm.x,
                                  msg.projection_geometry.focal_spot_postion_mm.y,
                                  msg.projection_geometry.focal_spot_postion_mm.z,])

        detector_center_mm = np.array([msg.projection_geometry.detector_postion_mm.x,
                                       msg.projection_geometry.detector_postion_mm.y,
                                       msg.projection_geometry.detector_postion_mm.z,])
        
        detector_orientation_quad = np.array([msg.projection_geometry.detector_orientation_quad.x,
                                              msg.projection_geometry.detector_orientation_quad.y,
                                              msg.projection_geometry.detector_orientation_quad.z,
                                              msg.projection_geometry.detector_orientation_quad.w])
        
        image = msg.image
        detector_heigth_mm = msg.detector_heigth_mm
        detector_width_mm = msg.detector_width_mm
        frame_id = msg.projection_geometry.header.frame_id

        return cls(focal_spot_mm, detector_center_mm, detector_orientation_quad, image, detector_heigth_mm, detector_width_mm, frame_id)
    
    def as_message(self) -> ProjectionGeometry:
        message = Projection()
        projection_geometry = ProjectionGeometry()

        projection_geometry.focal_spot_postion_mm.x = self.focal_spot_mm[0]
        projection_geometry.focal_spot_postion_mm.y = self.focal_spot_mm[1]
        projection_geometry.focal_spot_postion_mm.z = self.focal_spot_mm[2]

        projection_geometry.detector_postion_mm.x = self.detector_postion_mm[0]
        projection_geometry.detector_postion_mm.y = self.detector_postion_mm[1]
        projection_geometry.detector_postion_mm.z = self.detector_postion_mm[2]

        projection_geometry.detector_orientation_quad.x = self.detector_orientation_quad[0]
        projection_geometry.detector_orientation_quad.y = self.detector_orientation_quad[1]
        projection_geometry.detector_orientation_quad.z = self.detector_orientation_quad[2]
        projection_geometry.detector_orientation_quad.w = self.detector_orientation_quad[3]

        message.projection_geometry = projection_geometry
        message.image = self.image
        message.detector_heigth_mm = self.detector_heigth_mm
        message.detector_width_mm = self.detector_width_mm

        message.projection_geometry.header.frame_id = self.frame_id

        return message
    
    @property
    def detector_heigth_px(self) -> int:
        return self.image.shape[0]
    
    @property
    def detector_width_px(self) -> int:
        return self.image.shape[1]
    
    @property
    def pixel_pitch_x_mm(self) -> float:
        return self.detector_width_mm / self.detector_width_px
    
    @property
    def pixel_pitch_y_mm(self) -> float:
        return self.detector_heigth_mm / self.detector_heigth_px