#! /usr/bin/env python

import rospy
import tf

from masterloop import inform_masterloop_factory
from controller import 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(10).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:
                my_arm_xyz, _ = ll.lookupTransform(
                    'Aruco_0_frame',
                    'Aruco_{}_frame'.format(i),
                    rospy.Time(0)
                )
            except Exception as e:
                rospy.logwarn(e)
                continue

            movement(my_arm_xyz, side, our_cartesian)