#!/usr/bin/env python
# KAREL InputOutput CONTROL
# Used for TCP/IP communication with FANUC Controller
# Currently supported boolean IO only up to 999 addresses
#
# v3.0 (22y5m)
#
# Send data to this node using ROS Topic /robocheckers/io_control ONLY!
# USAGE: ros_publisher.publish(get_msg(type, number, state)), number is any number in range 1-999
# EXAMPLE: ros_publisher.publish(get_msg(io.DOUT, 119, io.ON))

import socket
import rospy
from std_msgs.msg import String

# States
OFF         = b'0'
ON          = b'1'
BLINK       = b'2'

# IO types
ALL         = b'00'  # Any I/O type
DIN         = b'01'  # Digital input
DOUT        = b'02'  # Digital output
ANIN        = b'03'  # Analog input
ANOUT       = b'04'  # Analog output
TOOL        = b'05'  # Tool output
PLCIN       = b'06'  # PLC input
PLCOUT      = b'07'  # PLC output
RDI         = b'08'  # Robot digital input
RDO         = b'09'  # Robot digital output
BRAKE_OUT   = b'10'  # Brake output
OPIN        = b'11'  # operator panels input
OPOUT       = b'12'  # operator panels output
SOPIN       = b'11'  # Same as io_opin
SOPOUT      = b'12'  # Same as io_sopout
ESTOP       = b'13'  # Emergency stop
TPIN        = b'14'  # Teach pendant digital input
TPOUT       = b'15'  # Teach pendant digital output
WDI         = b'16'  # weld inputs
WDO         = b'17'  # weld outputs
GPIN        = b'18'  # Grouped inputs
GPOUT       = b'19'  # Grouped outputs
UOPIN       = b'20'  # User operator's panel input
UOPOUT      = b'21'  # User operator's panel output
LDIN        = b'22'  # laser DIN
LDOUT       = b'23'  # laser DOUT
LAIN        = b'24'  # laser AIN
LAOUT       = b'25'  # laser AOUT
WSTK_IN     = b'26'  # weld stick input
WSTK_OUT    = b'27'  # weld stick output
MEM_BOO     = b'28'  # memory image boolean's
MEM_NUM     = b'29'  # memory image din's
DUMMY_BOO   = b'30'  # dummy boolean port type
DUMMY_NUM   = b'31'  # dummy numeric port type
PROC_AXES   = b'32'
IOPIN       = b'33'  # Internal operator's panel input
IOPOUT      = b'34'  # Internal operator's panel output
FLAG        = b'35'  # Flag (F[ ])
MARKER      = b'36'  # Marker (M[ ])
GPIN32      = b'37'  # Grouped inputs 32 bits
GPOUT32     = b'38'  # Grouped outputs 32 bits

class io_control(object):
    def __init__(self):
        # Host IP and port definition
        host = rospy.get_param("/robocheckers/robot_ip") # Get server IP address from ROS param server
        port = 11004                                # The port used by the server
        # Socket initialization
        self.sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        self.sock.connect((host, port))

        # Node initialization
        rospy.init_node('io_control')
        self.sub = rospy.Subscriber('/robocheckers/io_message', String, self.callback)

        # Declaring blinking list
        self.blink_list = []
        self.blink_enable = False

    def callback(self, data):
        msg = data.data

        # Checking correct length of message
        if len(msg) != 6:
            return

        self.blink_enable = False
        rospy.sleep(0.1)

        if msg[5] == BLINK:
            # Adding addres to blinking list
            self.blink_list.append(msg[:-1])
            
        elif msg[5] in [OFF, ON]:
            # If output in blink list, remove it
            if msg[:-1] in self.blink_list:
                self.blink_list.remove(msg[:-1])

            # Printing and sending 6 Byte message
            print "IO Control:", msg
            self.sock.send(msg)

        else:
            print "IO ERROR: Callback data invalid!"

        rospy.sleep(0.1)
        self.blink_enable = True

    def send(self, io_type, address, state):
        address = get_addr(address)
        if not address:
            print "IO ERROR: Address is invalid!"
            return False
        msg = io_type + address + state

        # Checking correct length of message
        if len(msg) != 6:
            return False

        # Printing and sending 6 Byte message
        print "IO Control:", msg
        self.sock.send(msg)

        return True


# Returns 3 bytes from int. Must be 1-999.
def get_addr(val):
    '''
        Generate valid io address (3 bytes message) from 1-999 integer value
    '''
    # Range check
    if val > 999 | val < 1:
        return False

    # Append the value to two zeros and transfer last 3 elements to bytes
    val = ["0", "0"] + list(str(val))
    val = bytes("".join(val[-3:]))

    return val


def get_msg(io_type, address, state):
    '''
        Generate valid message from io_type, io address and wanted state
    '''
    address = get_addr(address)
    if not address:
        print "IO ERROR: Address is invalid!"
        return None
    msg = io_type + address + state

    # Checking correct length of message
    if len(msg) != 6:
        return None

    # Returning 6 Byte message
    return msg
