#!/usr/bin/env python3

import rospy

from fanuc_msgs.srv import ReadSingleIO
from industrial_msgs.msg import RobotStatus
from std_msgs.msg import Int16
from fanuc_msg_translator_msgs.msg import FanucIOState, FanucReadSingleIO

io_publisher = rospy.Publisher('/msg_translator/io_status', FanucIOState, queue_size=10)


def read_single_io_callback(msg):
    # Create a service client for /set_payload_num
    rospy.wait_for_service('/read_single_io')
    try:
        # Create a proxy to call the service
        read_single_io = rospy.ServiceProxy('/read_single_io', ReadSingleIO)

        value = read_single_io(0, 101)

        rospy.loginfo("[Read Single IO Converter] Value:" + str(value))

        fanuc_io_state = FanucIOState()
        fanuc_io_state.header.stamp = rospy.Time.now()
        fanuc_io_state.type = msg.type
        fanuc_io_state.address = msg.address
        fanuc_io_state.value = value

        io_publisher.publish(value)

        rospy.loginfo("[Read Single IO Converter] IO Value: " + str(value))

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


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

    # Subscribe to the /set_payload topic
    rospy.Subscriber('/msg_translator/read_single_io', FanucReadSingleIO, read_single_io_callback)

    # Spin the node
    rospy.spin()


if __name__ == '__main__':
    main()
