#! /usr/bin/env python
import os
from time import sleep

from naoqi import ALProxy

import rospy
import tf

FW = -0.30
BK = -0.55
LT = -0.55
RT = 0.0


if __name__ == '__main__':
    rospy.init_node('walker')
    ll = tf.TransformListener()
    mp = ALProxy('ALMotion', os.environ['NAO_IP'], 9559)
    mp.wakeUp()
    mp.moveInit()

    while not rospy.is_shutdown():
        sleep(0.3)
        try:
            trans, rot = ll.lookupTransform('Aruco_0_frame',
                                            'CameraTop_optical_frame',
                                            rospy.Time(0))
        except Exception as e:
            mp.stopMove()
            # inform_controller('walker', 'stop')
            continue
        print trans
        # continue

        if BK < trans[2] < FW and LT < trans[0] < RT:
            mp.move(0, 0, 0)
            # inform_controller('walker', 'stop')
            continue

        permission = True
        # permission = inform_controller('walker', 'move')
        if not permission:
            mp.stopMove()
            continue
        if trans[2] < BK:  # backwards
            mp.move(-1, 0, 0)
        elif FW < trans[2]:  # forwards
            mp.move(1, 0, 0)
        elif RT < trans[0]:  # right
            mp.move(0, -1, 0)
        elif trans[0] < LT:  # left
            mp.move(0, 1, 0)

    mp.rest()