# Simple example program for connecting to Nao and testing some services.
# E.g. sets an arm angle and writes a camera image to file. Play with it to do more ...
# To run this program, set the PYTHONPATH to the directory where the Aldebaran software is installed,
# i.e. for the bash shell in .bashrc add a line (without "#"):
# export PYTHONPATH=/informatik/isr/wtm/public/installations/naoqi-sdk-1.10.52-linux/lib/
# This should work, but if it doesn't, then try another version of the Aldebaran software via:
# export PYTHONPATH=/opt/aldebaran-sdk:/opt/aldebaran-sdk/lib
# Further, make sure the IP address of your Nao is set correctly, below, and Nao switched on.
# Then run like:  python 01_start_Nao.py
# If using Webots, open within Webots a world file with Nao in it before starting this program.

import naoqi
import numpy
import time
import vision_definitions

#IP = "134.100.10.101" # Cable
#IP = "134.100.10.102" # WLAN
IP = "127.0.0.1" # Webots
PORT = 9559

# move motors
motProxy = naoqi.ALProxy("ALMotion", IP, PORT)
fractionMaxSpeed  = 0.2

# let head face to the position as when collecting data
motProxy.setAngles(["HeadPitch", "HeadYaw"], [0.03, -0.261799388], fractionMaxSpeed)

# set the names of the 5 arm angles
jointNames = ["RElbowRoll","RElbowYaw","RShoulderPitch","RShoulderRoll","RWristYaw"]

# !!! write your 5 arm angles in here: !!!
jointAngles = [1.242582, 0.504644, 0.495524, -0.579894, 1.119778] # these values are as from the last image - overwrite them !!!

print "moving to specified angle"

# move the limbs to the specified angles
motProxy.setAngles(jointNames, jointAngles, fractionMaxSpeed)


print "ending movement"
time.sleep(3)
print "now taking picture"



# now take the camera image and export it to a file "testP6.pnm"
vidProxy = naoqi.ALProxy("ALVideoDevice", IP, PORT)
vidProxy.setParam(vision_definitions.kCameraSelectID, 1) # 0:top camera; 1:bottom camera  -  does not work with the unwrapped Webots Nao as in  KSERALabGrasp_tmp1/2/3.wbt
                                                         #                                   hence use: KSERALabGrasp.wbt
time.sleep(1) # necessary so that the camera selection (line above) has an effect in Webots (!)

# resolution:  0 = (k)QQVGA = 160x120   1 = (k)QVGA = Quarter Video Graphics Array = 320x240   2 = (k)VGA = 640x480
# colorSpace:  0 = kYuv, 9 = kYUV422, 10 = kYUV, 11 = kRGB, 12 = kHSY, 13 = kBGR
# fps:         5, 10, 15, 30
eyes     = vidProxy.subscribe("eyes", 2, 11, 5) # resolution,colorSpace,fps
img1     = vidProxy.getImageRemote(eyes)

# image is an array containing image information:
# [0] : width;
# [1] : height;
# [2] : number of layers;
# [3] : ColorSpace;
# [4] : time stamp (highest 32 bits);
# [5] : time stamp (lowest 32 bits);
# [6] : array of size height * width * nblayers containing image data;
# [7] : camera ID

vidProxy.unsubscribe(eyes)

img2      = tuple(map(lambda c: ord(c),img1[6]))
# print "shape = ", numpy.shape(img2)
width, height = 640, 480
# print "h= ", height, " w=", width, " h*w*3=", height * width *3

# extract RGB pixels
imgR = numpy.zeros(height*width)
for i in range(height*width):
    imgR[i] = img2[3*i]
imgR = imgR * 255 / numpy.max(img2)

imgG = numpy.zeros(height*width)
for i in range(height*width):
    imgG[i] = img2[3*i+1]
imgG = imgG * 255 / numpy.max(img2)

imgB = numpy.zeros(height*width)
for i in range(height*width):
    imgB[i] = img2[3*i+2]
imgB = imgB * 255 / numpy.max(img2)

# write color image with characters (8 bit)
print "writing picture testP6.pnm"
f = open("testP6.pnm", 'wb')
f.write("P6\n")
line = str(width) + " " + str(height) + "\n"
f.write(line)
f.write("255\n") # values range from 0 to 255
c = ''
for i in range(height*width):
    c += chr(int(imgR[i])) + chr(int(imgG[i])) + chr(int(imgB[i]))
f.write (c)
f.close()

exit(0)


#for the real robot, thiese lines might have to be added earlier before invoking setAngles:
#motProxy.stiffnessInterpolation(["HeadPitch", "HeadYaw"], 0.2, 0.2)
#motProxy.stiffnessInterpolation(jointNames, 0.5, 0.2)