import rospy
# ROS Image message
from sensor_msgs.msg import Image, CameraInfo
# ROS Image message -> OpenCV2 image converter
from cv_bridge import CvBridge, CvBridgeError
from gazebo_msgs.msg import LinkStates

# !/usr/bin/env python
import roslib

# roslib.load_manifest('learning_tf')
import rospy

import tf
# import turtlesim.msg

# OpenCV2 for saving an image
import cv2
import numpy as np
from utils.projection import projectToCamera
from utils.transforms import getTransform, get_transform_tree_dict
from utils.draw import drawSquare2D

# Instantiate CvBridge
# bridge = CvBridge()
# link_states = None
# camera_info = None
flip = 0
br = tf.TransformBroadcaster()


# def image_callback(msg):
#     # print("Received an image!")
#
#     # Convert your ROS Image message to OpenCV2
#     cv2_img = bridge.imgmsg_to_cv2(msg, "bgr8")
#
#     camera_info_K = np.array(camera_info.K).reshape([3, 3])
#     camera_info_D = np.array(camera_info.D)
#     width = camera_info.width
#     height = camera_info.height
#     global link_states
#     global br
#     global flip
#     # global camera_info
#
#     file_xacro = "xacros/" + "novo.urdf.xacro"
#     world_frame = "world"
#     transform_dict = get_transform_tree_dict(file_xacro)
#     extrinsics = getTransform("camera_2_rgb_optical_frame", world_frame, transform_dict)
#     # print(extrinsics)
#     # exit(0)


def link_states_callback(msg):
    i = 0
    global flip

    for name in msg.name:
        if "actor::" in name:
            joint = name[len("actor::"):]
            if joint != "actor_pose":
                x = msg.pose[i].position.x
                y = msg.pose[i].position.y
                z = msg.pose[i].position.z

                q_x = msg.pose[i].orientation.x
                q_y = msg.pose[i].orientation.y
                q_z = msg.pose[i].orientation.z
                q_w = msg.pose[i].orientation.w

                br.sendTransform((x, y, z),
                                 (q_x, q_y, q_z, q_w),
                                 rospy.Time.now(),
                                 joint,
                                 "world")

                if joint == "LeftArm":
                    x= 0.17416358369963778
                    y= 0.33534451843549273
                    z= -0.01573965993604387

                    q_x= 0.0025657313359878225
                    q_y= 0.10198773275211998
                    q_z= -0.99476186147194889
                    q_w= 0.0063842267861563828

                        # x = -0.21508988841275012
                    # y = 0.30628006130950125
                    # z = 0.046526558900167853
                    #
                    # q_x = -0.178452732593838
                    # q_y = -0.017973485572601974
                    # q_z = -0.98368664411604345
                    # q_w = 0.013862259335335355

                    #
                    # x = -0.016618864660828205
                    # y = 0.26117820144569048
                    # z = 0.047082693145915247
                    #
                    # q_x = -0.024417754149575747
                    # q_y = 0.028400752121466692
                    # q_z = 0.6233825158909817
                    # q_w = 0.7810194680305067

                    # flip += 1

                    br.sendTransform((x, y, z),
                                     (q_x, q_y, q_z, q_w),
                                     rospy.Time.now(),
                                     joint + '_gz',
                                     "Hips")
                    print(msg.pose[i])

        i = i + 1


def main():
    rospy.init_node('image_listener')
    # Define your image topic
    # image_topic = "/camera_2/rgb/image_raw"
    topic_link_states = '/gazebo/link_states'
    # Set up your subscriber and define its callback

    rospy.Subscriber(topic_link_states, LinkStates, link_states_callback)
    # rospy.Subscriber(image_topic, Image, image_callback)
    #
    # global camera_info
    # camera_info = rospy.wait_for_message('/camera_2/rgb/camera_info', CameraInfo)

    # Spin until ctrl + c
    rospy.spin()


if __name__ == '__main__':
    main()