"""
Fnord
"""
import binascii
import time
import datetime
import canopen

def data_to_int(datain):
    """
    SDO Data to Integer
    """
    return int(binascii.hexlify(datain), 16)

### SETTINGS NEDAN ###########################################################

# COPID of node
POLLING_NODEID = 0x0f

# (Index, SubIndex, Size)
POLLING_SDO = (0x4000, 0x03, 4)

# Time between poll, in seconds
POLLING_FREQ = 1.0

# Index of byte
POLLING_BYTE_INDEX = 3

# Threshold value
POLLING_THRESHOLD = 0x55

##############################################################################

# Setup CANOpen Network
NETWORK = canopen.Network()
NETWORK.connect(channel='can0', bustype='socketcan')

#
# Create "Object Dictionary" to reduce error messages
#
OD_SDO = canopen.objectdictionary.Variable("fnord", POLLING_SDO[0],
        POLLING_SDO[1])
OD_SDO.access_type = "ro"
OD_SDO.add_bit_definition("fnord", range(1, POLLING_SDO[2]*8))
OD = canopen.ObjectDictionary()
OD.add_object(OD_SDO)

TESTOBJ = NETWORK.add_node(POLLING_NODEID, OD)

print("STARTING POLLING!")
while 1:
    time.sleep(POLLING_FREQ)
    data = TESTOBJ.sdo.upload(POLLING_SDO[0], POLLING_SDO[1])

    value = data_to_int(data[POLLING_BYTE_INDEX])
    if value < POLLING_THRESHOLD:
        print("%s: %02X\n", str(datetime.datetime.now()), value)