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

import rospy
import tf
from naoqi import ALProxy

from controller import inform_controller_factory


_inform_controller = inform_controller_factory('imitator')
_FRAME_TORSO = 0


if __name__ == '__main__':
    rospy.init_node('imitator')
    rospy.wait_for_service('inform_controller')
    ll = tf.TransformListener()
    mp = ALProxy('ALMotion', os.environ['NAO_IP'], 9559)
    mp.wakeUp()
    mp.setAngles(['LElbowRoll', 'RElbowRoll'],
                 [radians(-50), radians(50)], 0.5)

    while not rospy.is_shutdown():
        sleep(0.1)
        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:
                print e
                continue
            permission = _inform_controller('imitate')
            if not permission:
                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]))

            # sign = 1 if roll > 0 else -1
            # roll -= sign * radians(10)
            # print degrees(roll)
            mp.setAngles([
                '{}ShoulderRoll'.format(eff),
                '{}ShoulderPitch'.format(eff)
            ], [
                roll,
                pitch
            ], 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()