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

import rospy
import tf
from naoqi import ALProxy

from controller import inform_controller_factory


      #min   #max
FW =  1.65,  1.45
BK =  2.20,  2.40
LT = -0.35, -0.53
RT =  0.35,  0.53

VMIN = 0.3
VMAX = 1.0


_inform_controller = inform_controller_factory('walker')


def _speed(pos, interval):
    int_dir = 1 if interval[1] > interval[0] else -1
    if int_dir * (pos - interval[0]) < 0:
        return 0.0
    elif int_dir * (pos - interval[1]) > 0:
        return 1.0
    else:
        return (VMAX - VMIN) * abs(pos - interval[0]) / (
            abs(interval[1] - interval[0])
        ) + VMIN


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()
    mp.setMoveArmsEnabled(False, False)

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

        # print trans
        # continue

        movement = [0, 0, 0]
                               #-1   1  -1   1
        for i, dr in enumerate((BK, FW, RT, LT)):
            idx = i // 2
            sign = 1 if (i % 2) else -1
            speed = _speed(trans[idx], dr)
            if speed:
                movement[idx] = sign * speed
                break

        if not any(movement):
            rospy.logdebug('WALKER: STOP')
            _inform_controller('stop')
            mp.move(0, 0, 0)
            continue

        permission = _inform_controller('move')

        if not permission:
            mp.stopMove()
            continue

        rospy.loginfo('WALKER: TRANS: {}'.format(trans))
        rospy.loginfo('WALKER: MOVMT: {}'.format(movement))
        mp.moveToward(*movement)

    mp.rest()