#!/usr/bin/env python3
# coding=utf-8

""" WiBotic CAN Sample"""

__copyright__ = "Copyright 2018 WiBotic Inc."
__version__ = "0.1"
__email__ = "info@wibotic.com"
__status__ = "CAN Preview"

import uavcan

class WiboticCANDemo:
    def __init__(self, device_name, name, software_version, uid):
        # Creates a Node Info object
        node_info = uavcan.protocol.GetNodeInfo.Response()
        
        # Set the name of the object
        node_info.name = name
        
        # Set the version of the node
        node_info.software_version.major = software_version
        
        # Gives the node a unique ID
        node_info.hardware_version.unique_id = uid
        
        # Stores node as an instance field to be used throughout the object
        try:
            self.node = uavcan.make_node(device_name, node_id=123, node_info=node_info)
        except OSError:
            print('ERROR: Device not found. Please confirm the device name is correctly set!')
            
        # Defines a function that will be run when a message is received (callback function)
        def node_read_callback(event):
            print('Incoming message from node', event.transfer.source_node_id)
            print('Data:', uavcan.to_yaml(event))
        
        # Creates a handler for receiving a WiBoticCommand (the type that is replied with when
        # asking to read a certain parameter.
        self.node.add_handler(uavcan.wibotic.WiBoticCommand, node_read_callback)
        
    def send_command(self, command, payload):
        # Creates the WiBoticCommand object
        message = uavcan.wibotic.WiBoticCommand()
        
        # Sets the command equal to user entered command
        message.command = command
        
        # Sets the payload to user entered payload
        message.payload = payload
        
        # Broadcasts the message through the CAN bus
        self.node.broadcast(message)
        
    def read_parameter(self, parameter_id):        
        # Creates the WiBoticRead object
        message = uavcan.wibotic.WiBoticRead()
        
        # Sets the parameter name equal to the user set id
        message.parameter_id = parameter_id
        
        # Sends the read request across the CAN bus to the receiver
        self.node.broadcast(message)
        
    def spin(self):
        try:
            self.node.spin()
        except KeyboardInterrupt:
            pass

    def kill(self):
        self.node.close()
if __name__ == "__main__":
    # Defines the can device name (e.g. can0). This should be changed to the
    # device name on your system.
    can_device = 'can0'
    
    # Defines the intilization parameters for the demo object
    node_name = 'co.wibotic.uavcan_demo'
    software_version = 1
    uid = b'12345'
    
    # Creates the demo object
    canNode = WiboticCANDemo(can_device, node_name, software_version, uid)

    # Runs a command to change the RxBatteryNumCells value to 3
    # 62 -- the id for RxBatteryNumCells (can be found in param lookup)
    # 3 -- the number of cells we are setting our receiver to think it has
    canNode.send_command(62, 3)
    
    # Reads the value of RxBatteryNumCells
    # Data should print once a response is found
    canNode.read_parameter(62)
    
    # Spins the node so that we can continue to receive callbacks
    canNode.spin()
    
    # Close the can node
    canNode.kill()