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

import rospy
import tf
from naoqi import ALProxy

from teleoperation.srv import InformController

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


def _inform_controller(what):
    inform_controller = rospy.ServiceProxy('inform_controller',
                                           InformController)
    return inform_controller('walker', what).permission


if __name__ == '__main__':
    rospy.init_node('walker')
    rospy.wait_for_service('inform_controller')
    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('stop')
            continue

        print trans, rot
        # continue

        if (
            BK < trans[2] < FW and
            LT < trans[0] < RT
            # CW < trans[1] < CC
        ):
            mp.move(0, 0, 0)
            _inform_controller('stop')
            continue

        permission = _inform_controller('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()