#! /usr/bin/env python
"""Module for walking in the environment (Human Joystick).

ROS Node: `walker`.
Uses `inform_masterloop` service.

"""
from __future__ import division

import os
import json
from time import sleep

import rospy
import tf

from masterloop import inform_masterloop_factory, mp


FW = None
BK = None
LT = None
RT = None
LR = None
RR = None

VMIN = 0.3
VMAX = 1.0


def n_way(a, b, n=3):
    """A point which is (b - a)/n away from a."""
    return a + (b - a) / n


def global_init():
    global FW, BK, LT, RT, LR, RR
    here = os.path.dirname(os.path.realpath(__file__))
    with open('{}/../config/default.yaml'.format(here)) as f:
        x = json.load(f)

    cx, cy, cz = x['cr']
    FW = n_way(cx, x['fw']), x['fw']
    BK = n_way(cx, x['bk']), x['bk']
    LT = n_way(cy, x['lt']), x['lt']
    RT = n_way(cy, x['rt']), x['rt']
    LR = n_way(cz, x['lr'], 2), x['lr']
    RR = n_way(cz, x['rr'], 2), x['rr']


_inform_masterloop = inform_masterloop_factory('walker')


def _speed(pos, interval):
    """Calculate speed based on the operators position."""
    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')
    global_init()
    rospy.wait_for_service('inform_masterloop')
    ll = tf.TransformListener()
    mp.setMoveArmsEnabled(False, False)
    mp.setAngles(('HeadYaw', 'HeadPitch'), (0.0, 0.35), 0.3)

    while not rospy.is_shutdown():
        sleep(0.3)
        try:
            trans, q = ll.lookupTransform('odom',
                                          'Aruco_0_frame',
                                          rospy.Time(0))
            rot = tf.transformations.euler_from_quaternion(q)

        except Exception as e:
            mp.stopMove()
            _inform_masterloop('stop')
            continue

        movement = [0, 0, 0]
        ref_vec = trans[:2] + rot[2:]
                               #-1   1  -1   1  -1   1
        for i, dr in enumerate((BK, FW, RT, LT, RR, LR)):
            # figure out in which direction to move
            idx = i // 2
            sign = 1 if (i % 2) else -1
            speed = _speed(ref_vec[idx], dr)
            if speed:
                movement[idx] = sign * speed
                break

        rospy.loginfo('WALKER: TRANS: {}'.format(trans))
        rospy.loginfo('WALKER: ROTTN: {}'.format(rot))
        rospy.loginfo('WALKER: MOVMT: {}\n'.format(movement))

        if not any(movement):
            rospy.logdebug('WALKER: STOP')
            _inform_masterloop('stop')  # release the control
            # mp.move(0, 0, 0)
            mp.stopMove()
            continue

        permission = _inform_masterloop('move')  # try to seize the control

        if not permission:
            mp.stopMove()
        else:
            # set the head so that the scene is clearly visible on cameras
            mp.setAngles(('HeadYaw', 'HeadPitch'), (0.0, 0.35), 0.3)
            mp.moveToward(*movement)  # DON'T DELETE THIS ONE
            pass

    mp.rest()
