#! /usr/bin/env python
import os
import json
from time import sleep
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():
        sleep(0.1)
        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()