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

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


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'], [0, 0], 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
            roll = atan(trans[1] / abs(trans[0]))
            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()