#! /usr/bin/env python
# -*- encoding: UTF-8 -*-

"""Example: Shows how to show live images from Nao using PyQt"""

import qi
import argparse
import sys
from PyQt4.QtGui import QWidget, QImage, QApplication, QPainter
from PyQt4 import QtGui, QtCore
from PIL import Image
from PIL import ImageFont
from PIL import ImageDraw
import vision_definitions
import numpy as np


def main(session, robot_ip, port):
    """
    This is a tiny example that shows how to show live images from Nao using PyQt.
    You must have python-qt4 installed on your system.
    """
    CameraID = 2

    # Get the service ALVideoDevice.


    video_service = session.service("ALVideoDevice")
    app = QApplication([robot_ip, port])
    myWidget = ImageWidget(video_service, CameraID)
    myWidget.show()
    sys.exit(app.exec_())



class ImageWidget(QWidget):
    """
    Tiny widget to display camera images from Naoqi.
    """
    def __init__(self, video_service, CameraID, parent=None):
        """
        Initialization.
        """
        QWidget.__init__(self, parent)
        self.video_service = video_service
        self._image = QImage()
        self.setWindowTitle('Robot')

        self._imgWidth = 320
        self._imgHeight = 240
        self._cameraID = CameraID
        self.setFixedSize(self._imgWidth, self._imgHeight)
        # Our video module name.
        self._imgClient = ""

        # This will contain this alImage we get from Nao.
        self._alImage = None

        self._registerImageClient()

        # Trigget 'timerEvent' every 100 ms.
        self.startTimer(100)

    def _registerImageClient(self):
        """
        Register our video module to the robot.
        """
        resolution = vision_definitions.kQQQVGA  # 80*60 px
        colorSpace = vision_definitions.kDepthColorSpace
        self._imgClient = self.video_service.subscribe("_client", resolution, colorSpace, 5)


        # Select camera.
        self.video_service.setParam(vision_definitions.kCameraSelectID, self._cameraID)


    def _unregisterImageClient(self):
        """
        Unregister our naoqi video module.
        """
        if self._imgClient != "":
            self.video_service.unsubscribe(self._imgClient)


    def paintEvent(self, event):
        """
        Draw the QImage on screen.
        """
        painter = QPainter(self)
        painter.drawImage(painter.viewport(), self._image)


    def _updateImage(self):
        """
        Retrieve a new image from Nao.
        """

        self._alImage = self.video_service.getImageRemote(self._imgClient)

        #lets measure depth of this rectangle on image: (30,25) upper left corner
        # and (40,35) lower right corner
        #assume depth is the same on rectangle
        print self.getDepht(self.get3DImageFromAlValue(self._alImage), [(30,25),(40,35)])

        self._image = QImage(self._alImage[6],           # Pixel array.
                             self._alImage[0],           # Width.
                             self._alImage[1],           # Height.
                             QImage.Format_RGB888)

    def get3DImageFromAlValue(self, alImage):
        width = alImage[0]
        height = alImage[1]
        pixels = alImage[6]
        im = np.zeros((height, width, 1), dtype=np.uint8)
        index = 0
        for y in range(height):
            for x in range(width):
                depth = np.zeros(1, dtype=np.uint8)
                depth[0] = pixels[index]
                im[y][x] = depth
                index += 1
        return im

    def getDepht(self, im3d, bbox):
        x1 = int(bbox[0][0])
        y1 = int(bbox[0][1])
        x2 = int(bbox[1][0])
        y2 = int(bbox[1][1])
        average = 0.0
        strIm = ""
        num = 0
        for y in range(y1,y2+1):
            strIm += "|"
            for x in range(x1,x2+1):
                d = im3d[y][x][0]
                strIm += "{:>3}|".format(d)
                average += 1.0*d
                num += 1
            strIm += "\n"
        print strIm
        return average/num

    def timerEvent(self, event):
        """
        Called periodically. Retrieve a nao image, and update the widget.
        """
        self._updateImage()
        self.update()


    def __del__(self):
        """
        When the widget is deleted, we unregister our naoqi video module.
        """
        self._unregisterImageClient()


if __name__ == "__main__":
    parser = argparse.ArgumentParser()
    parser.add_argument("--ip", type=str, default="127.0.0.1",
                        help="Robot IP address. On robot or Local Naoqi: use '127.0.0.1'.")
    parser.add_argument("--port", type=int, default=9559,
                        help="Naoqi port number")

    args = parser.parse_args()
    session = qi.Session()
    try:
        session.connect("tcp://" + args.ip + ":" + str(args.port))
    except RuntimeError:
        print ("Can't connect to Naoqi at ip \"" + args.ip + "\" on port " + str(args.port) +".\n"
               "Please check your script arguments. Run with -h option for help.")
        sys.exit(1)
    main(session, args.ip, args.port)