#!/usr/bin/env python3

import rospy

from fanuc_msgs.srv import SetPayloadNum
from industrial_msgs.msg import RobotStatus
from fanuc_msg_translator_msgs.msg import FanucPayloadNum

# Namespace of the node
ns = rospy.get_namespace()

robot_status = RobotStatus()
payload_status = -1

payload_status_publisher = rospy.Publisher(ns + 'bridge/set_payload_num/state', FanucPayloadNum, queue_size=10)


def set_payload_callback(msg):
    """
    Callback function for the /set_payload topic. Calls the /set_payload_num service with the received value if the
    robot is in the correct state.

    :param msg: The received message from the /set_payload topic
    :type msg: FanucPayloadNum
    """
    global payload_status

    # Create a service client for /set_payload_num
    rospy.wait_for_service(ns + 'set_payload_num')
    try:
        # Create a proxy to call the service
        set_payload_num = rospy.ServiceProxy(ns + 'set_payload_num', SetPayloadNum)

        if robot_status.mode.val == 2 \
                and robot_status.e_stopped.val == 0 \
                and robot_status.drives_powered.val == 1 \
                and robot_status.motion_possible.val == 1 \
                and robot_status.in_motion.val == 0 \
                and robot_status.in_error.val == 0 \
                and robot_status.error_code == 0:
            # Call the service with the received value from /set_payload topic
            set_payload_num(msg.payload_num)
            set_payload_num.wait_for_service()
            payload_status = msg.payload_num
            rospy.loginfo("[Payload Converter] SetPayloadNum service called with payload: " + str(payload_status))

        else:
            rospy.loginfo("[Payload Converter] Robot is not in the correct state to set payload")

    except rospy.ServiceException as e:
        # Log the error if the service call failed
        rospy.logerr("[Payload Converter] Service call failed: %s", str(e))


def payload_status_publisher_callback(event):
    """
    Callback function for the timer that publishes the payload status every 0.2 seconds.
    """
    global payload_status

    # Create a new FanucRobotStatus message
    payload_status_msg = FanucPayloadNum()

    # Set the values of the new message
    payload_status_msg.header.stamp = rospy.Time.now()
    payload_status_msg.payload_num = payload_status

    # Publish the new message
    payload_status_publisher.publish(payload_status_msg)


def robot_status_callback(msg):
    """
    Callback function for the /robot_status topic. Updates the robot_status variable with the received message.
    """
    global robot_status

    # Update the robot_status variable
    robot_status = msg


def main():
    # Initialize the node
    rospy.init_node('fanuc_payload_converter_node')

    # Subscribe to the /set_payload topic
    rospy.Subscriber(ns + 'bridge/set_payload_num/goal', FanucPayloadNum, set_payload_callback)
    rospy.Subscriber(ns + 'robot_status', RobotStatus, robot_status_callback)

    # Create a timer to publish the payload status every 0.2 seconds
    interval = rospy.Duration(0.2)
    rospy.Timer(interval, payload_status_publisher_callback)

    rospy.loginfo("[Payload Converter] Node initialized!")

    # Spin the node
    rospy.spin()


if __name__ == '__main__':
    main()
