import sys
import time

import cv2
import numpy as np
#from PIL import Image

import qi

class record_image:
    ipadress = "169.254.16.213" #moos wired
    port = "9559"
 
    def subscribeDevice(self):
        connection_url = "tcp://" + self.ipadress + ":" + self.port

        app = qi.Application(["BehaviorBasedRobotics", "--qi-url=" + connection_url])

        try:
            app.start()
        except RunTimeError:
            print("Can't connect to Naoqi at " + self.connection_url)
            sys.exit(1)

        self.session = app.session
        #video_cap = cv2.VideoCapture(0)
        self.video_proxy = self.session.service("ALVideoDevice")
        #self.video_proxy.setParam(18,0)
        self.video_proxy.setActiveCamera(0) # Top Camera
        self.resolution = 1
        self.colorSpace = 13
        self.fps =30
 #       self.video_client = self.video_proxy.subscribe("python_script", self.resolution, self.colorSpace, self.fps) # NaoQi 2.5
        self.video_client = self.video_proxy.subscribeCamera("python_script", 1, self.resolution, self.colorSpace, self.fps)

        self.motion_proxy = self.session.service("ALMotion")

    def setHead(self, yaw, pitch):
        self.motion_proxy.setStiffnesses("Head", 0.9)
        self.motion_proxy.setAngles(["HeadYaw", "HeadPitch"], [yaw,pitch], 0.1)
        time.sleep(2.0)
        self.motion_proxy.setStiffnesses("Head", 0.6)

    def getImage(self):

        shot = self.video_proxy.getImageRemote(self.video_client)

        if shot is None:
            self.unsubscribeDevice()
        assert shot is not None, "You need to do: nao restart and run again"

        w, h = shot[0], shot[1]
        img_data = shot[6] # raw image bytes

#        image_string = str(bytearray(array))
#        data_npyarr = np.fromstring(image_string, np.uint8)
        data_npyarr = np.frombuffer(img_data, dtype=np.uint8)
    
        frame = data_npyarr.reshape((h,w,3))
#        frame = Image.frombuffer("RGB", size, shot[6], "raw", 0, 1)
        print("saving image")
        cv2.imwrite('latest.png', frame)

    def unsubscribeDevice(self):
        self.video_proxy.unsubscribe(self.video_client)

if __name__ == '__main__':
    video_element = record_image()

    video_element.subscribeDevice()
    video_element.setHead(0.0,-0.45)
    video_element.getImage()
    video_element.unsubscribeDevice()