#! /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 controller import inform_controller_factory


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