#! /usr/bin/env python
import os
import json
from math import atan, asin, radians, sqrt

import rospy
import tf
import numpy as np
from naoqi import ALProxy

from masterloop import inform_masterloop_factory


_inform_masterloop = inform_masterloop_factory('imitator')


_FRAME_TORSO = 0
arm = None


def _elbow(arm_):
    quot = min(1.0, arm_ / arm)
    return radians(180 * (1 - quot))


if __name__ == '__main__':
    rospy.init_node('imitator')
    rospy.wait_for_service('inform_masterloop')
    ll = tf.TransformListener()
    am = ALProxy('ALAutonomousMoves', os.environ['NAO_IP'], 9559)
    am.setExpressiveListeningEnabled(False)
    mp = ALProxy('ALMotion', os.environ['NAO_IP'], 9559)
    mp.wakeUp()
    mp.setAngles(['LElbowRoll', 'RElbowRoll', 'LElbowYaw', 'RElbowYaw'],
                 [radians(-70), radians(70), -radians(40), radians(40)], 0.5)

    here = os.path.dirname(os.path.realpath(__file__))
    with open('{}/../config/default.yaml'.format(here)) as f:
        x = json.load(f)

    arm = x['arm']

    while not rospy.is_shutdown():
        rospy.Rate(10).sleep()
        if not _inform_masterloop('imitate'):
            continue
        rospy.logdebug('IMITATOR: ACTIVE')

        # torso movement

        # try:
            # _, q = ll.lookupTransform('odom',
                                      # 'Aruco_0_frame',
                                      # rospy.Time(0))
            # rot = tf.transformations.euler_from_quaternion(q)
            # mp.setAngles(['LHipYawPitch', 'RHipYawPitch'],
                         # [-rot[1], -rot[1]], 0.3)
        # except Exception as e:
            # rospy.logwarn(e)

        # continue

        for i, eff in enumerate(['L',
                                 'R'], 1):
            try:
                trans, _ = ll.lookupTransform(
                    'Aruco_0_frame',
                    'Aruco_{}_frame'.format(i),
                    rospy.Time(0)
                )
            except Exception as e:
                rospy.logwarn(e)
                continue

            sign = 1 if eff == 'L' else -1
            roll = asin(trans[1] /
                        sqrt(trans[0]**2 + trans[1]**2 + trans[2]**2))
            roll -= sign * radians(25)
            pitch = atan(-trans[2] / abs(trans[0]))

            eroll = -sign * _elbow(np.linalg.norm(np.array(trans)))

            mp.setAngles([
                '{}ShoulderRoll'.format(eff),
                '{}ShoulderPitch'.format(eff),
                '{}ElbowRoll'.format(eff)
            ], [
                roll,
                pitch,
                eroll
            ], 0.3)

            # targ = np.array(trans)
            # targ = targ / np.linalg.norm(targ) * 0.3
            # mp.setPositions(eff, _FRAME_TORSO,
                            # targ.tolist() + [0, 0, 0], 0.5, 7)
            # print eff, targ

    mp.rest()