import naoqi
import time
from naoqi import ALProxy
import sys
import motion
import almath

def main(robotIP):
        try:
            motionProxy = ALProxy("ALMotion", robotIP, 9559)
        except Exception, e:
            print "Could not create proxy to ALMotion"
            print "Error was: ", e

        try:
            postureProxy = ALProxy("ALRobotPosture", robotIP, 9559)
        except Exception, e:
                print "Could not create proxy to ALRobotPosture"
                print "Error was: ", e

    ##############################
    #HERE THE INIT CODE
    ##############################
    #You don't need 'self' here because it's not a class (look at python class for more information)
    tracker = ALProxy( "ALTracker", robotIP, 9559) #DONT FORGET IP AND PORT because you're not using choregraphe

        x = 0.0
        y = 0.0
        z = 0.0

        maxSpeed = 0.5

        effector = "Arms"
    frame = 0 #FRAME TORSO

    ###################################
    # WHAT THE BOX DOES WHEN ACTIVATED (OnInput_Start)
    # In the class, self.getparameter() take the configuration of the box (when you click on the tool icon)
    # It's not in choregraphe so you have to hardcode these parameters or pass them as arguments
    #####################################
    #GET THE PARAMETERS (where to point, at what speed and using what)
        x = 1.5
        y = 2
        z = 3

        maxSpeed = 0.5 #50%
        effector = "Arms" #Look at the doc for effector list

        frameStr = "Robot" #Same, take a look at the doc
        if frameStr == "Torso":
            frame = 0
        elif frameStr == "World":
            frame = 1
        elif frameStr == "Robot":
            frame = 2

    #AND Now, the actual function. You just need to set the tracker and call this function to make the robot point something
    tracker.pointAt(effector, [x, y, z], frame, maxSpeed)

if __name__ == "__main__":
    robotIp = "1111111"

    if len(sys.argv) <= 1:
        print "Usage python motion_cartesianArm1.py robotIP (optional default: 127.0.0.1)"
    else:
        robotIp = sys.argv[1]

    main(robotIp)