#!/usr/bin/env python
"""Fake comm node dropper for simulation."""

import subprocess

import rospy
import tf2_ros
from tf2_geometry_msgs import *
from hw_msgs.msg import CommDropperStatus
from std_msgs.msg import Empty, Int16
from geometry_msgs.msg import PointStamped


status_msg = CommDropperStatus()
status_msg.status = [CommDropperStatus.UNKNOWN
                     for i in range(len(status_msg.status))]

result_pub = None
tf_buf = None
robot_name = None


def drop_cb(msg):
    global status_msg

    # Get the first available node index
    try:
        servo_id = status_msg.status.index(CommDropperStatus.LOADED)
    except ValueError:
        rospy.logwarn("No more radio mounted")
        result_pub.publish(-1)
        return
    rospy.loginfo("Received drop command for servo %d", servo_id)

    # Spawn gazebo model
    try:
        point = get_drop_position()
    except Exception:
        rospy.logerr("Robot pose is not available")
        point = None

    if point:
        rospy.loginfo("Spawning radio model in gazebo")
        model_name = 'radio_{}_{}'.format(robot_name, servo_id)
        cmd_template = 'rosrun gazebo_ros spawn_model ' \
                       '-model "{model_name}" ' \
                       '-sdf -database Radio ' \
                       '-x "{x}" -y "{y}" -z "{z}" -R 1.57'
        cmd = cmd_template.format(
                    model_name=model_name,
                    x=point.point.x,
                    y=point.point.y,
                    z=point.point.z,
                  )
        rospy.loginfo("Executing: %s", cmd)
        subprocess.check_call(cmd, shell=True)

    # Update status
    status_msg.header.stamp = rospy.Time.now()
    status_msg.status[servo_id] = CommDropperStatus.EMPTY

    # Report result
    result_pub.publish(servo_id)

    servo_id += 1


def get_drop_position():
    point = PointStamped()
    point.header.frame_id = robot_name + '/base_link'
    point.point.x = -0.8
    point.point.y = 0
    point.point.z = -0.1
    
    return tf_buf.transform(point, 'world', timeout=rospy.Duration(1))


def main():
    global tf_buf, robot_name, result_pub

    rospy.init_node('fake_comm_node_dropper')

    robot_name = rospy.get_namespace().split('/')[1]

    # Get loaded radios from config file
    droppable_list = rospy.get_param('comm_node_manager/{}'.format(robot_name))
    for droppable in droppable_list:
        status_msg.status[droppable['servo_id']] = CommDropperStatus.LOADED

    status_pub = rospy.Publisher('comm_dropper/status', CommDropperStatus)
    result_pub = rospy.Publisher('comm_dropper/drop_result', Int16)
    drop_pub = rospy.Subscriber('comm_dropper/drop', Empty, drop_cb)
    tf_buf = tf2_ros.Buffer()
    tf_listener = tf2_ros.TransformListener(tf_buf)

    r = rospy.Rate(1)
    while not rospy.is_shutdown():
        status_pub.publish(status_msg)
        r.sleep()


if __name__ == '__main__':
    main()