#! /usr/bin/env python
from argparse import ArgumentParser

import rospy
import tf
import numpy as np

from masterloop import inform_masterloop_factory
from controller import nao_cart_movement, movement, dumb, our_cartesian


_inform_masterloop = inform_masterloop_factory('imitator')


TORSO = False


if __name__ == '__main__':
    rospy.init_node('imitator')
    rospy.wait_for_service('inform_masterloop')

    ll = tf.TransformListener()

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

        # if TORSO:
            # 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)

        for i, side in enumerate(['L', 'R'], 1):
            try:
                a0, _ = ll.lookupTransform(
                    'odom',
                    'Aruco_0_frame',
                    rospy.Time(0)
                )
                arm, _ = ll.lookupTransform(
                    'odom',
                    'Aruco_{}_frame'.format(i),
                    rospy.Time(0)
                )
            except Exception as e:
                rospy.logwarn(e)
                continue
            my_arm_xyz = np.array(arm) - np.array(a0)
            # rospy.loginfo('{}'.format(my_arm_xyz))
            # rospy.loginfo('{}'.format(dumb(my_arm_xyz, side)))
            movement(my_arm_xyz, side, dumb)