import logging; logger = logging.getLogger("morse." + __name__)
from morse.core.services import async_service
from morse.core import status, mathutils
from morse.sensors.camera import Camera
from morse.sensors.video_camera import VideoCamera
from morse.helpers.components import add_data, add_property

class AbstractDepthCamera(VideoCamera):

    add_property('near_clipping', 1.0, 'cam_near')
    add_property('far_clipping', 20.0, 'cam_far')
    add_property('retrieve_depth', True, 'retrieve_depth')

    def __init__(self, obj, parent=None):
        """ Constructor method.

        Receives the reference to the Blender object.
        The second parameter should be the name of the object's parent.
        """
        # Call the constructor of the VideoCamera class
        VideoCamera.__init__(self, obj, parent)
        # Component specific initialize (converters)
        self.pts = None
        self.initialize()

    # abstractmethod
    def process_image(self, image):
        pass
    # abstractmethod
    def initialize(self):
        pass

    def default_action(self):
        """ Update the texture image. """
        # Grab an image from the texture
        if self.bge_object['capturing'] and (self._n != 0) :

            # Call the action of the Camera class
            Camera.default_action(self)

            self.process_image(self.image_data)

            self.capturing = True

            if self._n > 0:
                self._n -= 1
                if self._n == 0:
                    self.completed(status.SUCCESS)
        else:
            self.capturing = False



class DepthCamera(AbstractDepthCamera):
    """
    This sensor generates a 3D point cloud from the camera perspective.

    See also :doc:`../sensors/camera` for generic informations about Morse cameras.
    """

    _name = "Depth (XYZ) camera"
    _short_desc = "A camera capturing 3D points cloud"

    add_data('points', 'none', 'memoryview', "List of 3D points from the depth "
             "camera. memoryview of a set of float(x,y,z). The data is of size "
             "``(nb_points * 12)`` bytes (12=3*sizeof(float).")
    add_data('nb_points', 0, 'int', "the number of points found in the "
             "points list. It must be inferior to cam_width * cam_height")

    add_property('_keep_list', None, 'keep_list', 'list(int)',
"a list of lines number we preserve. Other lines are discared. It allows to "
"better simulate sparse sensor such as velodyne. If not specified, keep the "
"image dense")

    add_property('_keep_resolution', False, 'keep_resolution', 'boolean',
    "By default the clipping of the camera removes unreals points"
    "This option allows adding dummy points (0,0,0) for keeping the sensor resolution")

    def initialize(self):
        from morse.sensors.zbufferto3d import ZBufferTo3D
        if self._keep_list is None:
            keep_list = list(range(self.image_height))
        else:
            keep_list = eval(self._keep_list)
        # Store the camera parameters necessary for image processing
        self.converter = ZBufferTo3D(self.local_data['intrinsic_matrix'][0][0],
                                     self.local_data['intrinsic_matrix'][1][1],
                                     self.near_clipping, self.far_clipping,
                                     self.image_width, self.image_height, self._keep_resolution,
                                     keep_list)

    def process_image(self, image):
        self.pts = self.converter.recover(image)
        self.local_data['points'] = self.pts
        self.local_data['nb_points'] = int(len(self.pts) / 12)


class DepthVideoCamera(AbstractDepthCamera):
    """
    This sensor generates a Depth 'image' from the camera perspective.

    "Depth images are published as sensor_msgs/Image encoded as 32-bit float.
    Each pixel is a depth (along the camera Z axis) in meters."
    [ROS Enhancement Proposal 118](http://ros.org/reps/rep-0118.html) on Depth
    Images.

    If you are looking for PointCloud data, you can use external tools like
    [depth_image_proc](http://ros.org/wiki/depth_image_proc) which will use the
    ``intrinsic_matrix`` and the ``image`` to generate it, or eventually the
    ``XYZCameraClass`` in this module.
    """

    _name = "Depth camera"

    add_data('image', 'none', 'buffer', "Z-Buffer captured by the camera, "
             "converted in meters. memoryview of float of size "
             "``(cam_width * cam_height * sizeof(float))`` bytes.")

    def initialize(self):
        from morse.sensors.zbuffertodepth import ZBufferToDepth
        # Store the camera parameters necessary for image processing
        self.converter = ZBufferToDepth(self.near_clipping, self.far_clipping,
                                        self.image_width, self.image_height)

    def process_image(self, image):
        # Convert the Z-Buffer
        self.local_data['image'] = self.converter.recover(image)


class RawImage(AbstractDepthCamera):
    """
    This sensor gets raw Z-Buffer from the camera perspective.

    See also :doc:`../sensors/camera` for generic informations about Morse cameras.
    """

    _name = "Depth camera (raw Z-Buffer)"

    add_data('image', 'none', 'buffer', "Raw Z-Buffer captured by the camera, "
             "not converted. bgl.Buffer of float of size "
             "``(cam_width * cam_height * sizeof(float))`` bytes.")

    def process_image(self, image):
        """Same behaviour as VideoCamera

        Let the middleware deal with image processing. For example convert and
        publish in C++, without having to re-serialize a Python object.
        """
        self.local_data['image'] = image


class DepthCameraRotationZ(DepthCamera):
    """Used for Velodyne sensor"""

    add_property('rotation', 0.01745, 'rotation')

    def default_action(self):
        DepthCamera.default_action(self)
        self.applyRotationZ(self.rotation)
    def applyRotationZ(self, rotation):
        # The second parameter specifies a "local" movement
        self.bge_object.applyRotation([0, rotation, 0], True)
