classdef mavlinkio < handle & uav.internal.mavlink.Unsaveable
%MAVLINKIO Interface with MAVLink clients using MAVLink message definitions
%   The MAVLINKIO object interfaces with MAVLink clients through UDP
%   ports to exchange messages using MAVLink communication protocols.
%
%   MAVLINK = MAVLINKIO(MSGDEFINITION) creates an interface based on an
%   input mavlinkdialect object, MSGDEFINITION, that defines the
%   message definitions.
%
%   MAVLINK = MAVLINKIO(MSGXML) directly specifies the MAVLink
%   message definition as an .xml file path with protocol version set to 2
%
%   MAVLINK = MAVLINKIO(MSGXML, VERSION) directly specifies the MAVLink
%   message definition as an .xml file path and protocol version
%
%   MAVLINK = MAVLINKIO(__, NAME, VALUE) specifies additional
%   arguments using the following name-value pairs. These name-value
%   pairs directly set the MAVLink client information in the
%   LocalClient property. Available argument names:
%
%      'SystemID':          - Local client system ID.
%      'ComponentID':       - Local client component ID.
%      'ComponentType':     - Local client component type.
%      'AutopilotType':     - Local client autopilot type.
%
%   MAVLINKIO properties:
%       Dialect                   - Message definition dialect
%       LocalClient               - Local MAVLink client information
%
%   MAVLINKIO methods:
%       connect              - Connect to MAVLink clients via an UDP port
%       listConnections      - List all active connections
%       disconnect           - Disconnect active connections
%       listClients          - List all MAVLink clients communicating with the local client
%       listTopics           - List all topics received on local client
%       sendmsg              - Send MAVLink message to all connected MAVLink clients
%       sendudpmsg           - Send MAVLink message to specified UDP endpoint
%       serializemsg         - Serialize MAVLink message to binary buffer
%
%   Example:
%
%       % Create a MAVLink interface based on 'common' dialect
%       mavlink = mavlinkio('common.xml', 2);
%       connect(mavlink, 'udp', 'LocalPort', 14550);
%       pause(5);
%       listTopics(mavlink)
%       disconnect(mavlink);
%
%   See also mavlinkdialect, mavlinkclient, mavlinksub.

%   Copyright 2018-2021 The MathWorks, Inc.

    properties (SetAccess = private)
        %Dialect Message definition dialect
        %   Dialect is a mavlinkdialect object that stores all message
        %   definitions. It provides query and deserialization functions
        Dialect
    end

    properties (Dependent, SetAccess = private)
        %LocalClient Local MAVLink client information
        LocalClient
    end

    properties (Access = private)
        %IO mavlink i/o implementation
        IO

        %Clients container of all MAVLink clients associated with
        %this IO
        Clients

        %HeartbeatSubscriber monitors all heartbeat messages and update the
        %client's component type and autopilot type accordingly
        HeartbeatSubscriber
    end

    methods
        function obj = mavlinkio(dialect, varargin)
        %MAVLINKIO Construct an instance of this class

            argIn = 1;
            if ~isa(dialect, 'mavlinkdialect')
                validateattributes(dialect, {'string', 'char'}, ...
                                   {'scalartext'}, 'mavlinkio', 'dialect');

                if nargin > 1 && isnumeric(varargin{argIn})
                    validateattributes(varargin{argIn}, {'numeric'}, ...
                                       {'integer', 'scalar', '>=', 1, '<=', 2},  'mavlinkio', 'version');
                    dialect = mavlinkdialect(dialect, varargin{argIn});
                    argIn = argIn + 1;
                else
                    dialect = mavlinkdialect(dialect);
                end

            end

            p = inputParser;
            p.addParameter('SystemID', [], ...
                           @(x)validateattributes(x, {'numeric'}, ...
                                                  {'integer', 'positive', '<=', 255}, 'mavlinkio', 'SystemID'));
            p.addParameter('ComponentID', [], ...
                           @(x)validateattributes(x, {'numeric'}, ...
                                                  {'integer', 'nonnegative', '<=', 255}, 'mavlinkio', 'ComponentID'));
            p.addParameter('ComponentType', "MAV_TYPE_GCS", ...
                           @(x)validateattributes(x, {'string', 'char'}, ...
                                                  {'scalartext'}, 'mavlinkio', 'ComponentType'));
            p.addParameter('AutopilotType', "MAV_AUTOPILOT_INVALID", ...
                           @(x)validateattributes(x, {'string', 'char'}, ...
                                                  {'scalartext'}, 'mavlinkio', 'AutopilotType'));
            parse(p, varargin{argIn:end});

            obj.IO = dialect.createIO(p.Results.SystemID, p.Results.ComponentID, ...
                                      p.Results.ComponentType, p.Results.AutopilotType);
            obj.Clients = uav.internal.mavlink.container.Map;
            obj.Dialect = dialect;

            key = obj.hashClientIDs(obj.IO.LocalClient.SystemID, obj.IO.LocalClient.ComponentID);
            obj.Clients.insert(key, obj.IO.LocalClient);


            % subscribe to heartbeat message if possible to get client
            % component type and autopilot type
            info = dialect.msginfo("heartbeat");
            if ~isempty(info)
                clients = obj.Clients;
                % for the heartbeat subscriber used here, the message source is not used in 
                % mavlinkio.updateClients callback. passing []
                obj.HeartbeatSubscriber = obj.createSubscriber([], -1, -1, info.MessageID(1), ...
                                                               10, @(~,msgExt)mavlinkio.updateClients(clients,dialect,msgExt));
            else
                obj.HeartbeatSubscriber = [];
            end
        end

        function delete(obj)
        %DELETE Remove internal implementation
            delete(obj.HeartbeatSubscriber);
            delete(obj.IO);
        end

        function client = get.LocalClient(obj)
        %getter for client
            client = obj.IO.LocalClient.toStruct();
        end

        function connectName =connect(obj, type, varargin)
        %CONNECT Connect to MAVLink clients via an UDP port
        %   connectionName = CONNECT(OBJ, TYPE) establish
        %   connections according to the TYPE and corresponding
        %   name-value pairs. The TYPE argument supports "udp".
        %
        %   connectionName = CONNECT(__, NAME, VALUE) specifics
        %   additional argument using name-value pairs. Available
        %   argument names:
        %
        %       "ConnectionName",      - Name used to identify each
        %                                open connection.Default names
        %                                would be automatically
        %                                generated as "Connection#"
        %
        %       "LocalPort":           - LocalPort for UDP connection.
        %                                When Local Port is set to 0,
        %                                the UDP connection is bind to
        %                                random open port
        %
        %   Example:
        %       mavlink = mavlinkio('common.xml');
        %       connect(mavlink, 'udp')

            validatestring(type, "UDP", 'connect', 'type');
            parser = inputParser();
            parser.addParameter('ConnectionName', '', ...
                                @(x)validateattributes(x, {'string', 'char'}, ...
                                                       {'scalartext'}, 'connect', 'ConnectionName'));

            %Currently only supported type is UDP
            parser.addParameter('LocalPort', 0, ...
                                @(x)validateattributes(x, {'numeric'}, ...
                                                       {'integer', 'nonnegative', '<=', intmax('uint16')}, ...
                                                       'connect', 'LocalPort'));
            parse(parser, varargin{:});

            connectName = obj.IO.connectUDP(parser.Results.LocalPort, parser.Results.ConnectionName);
        end

        function tbl = listConnections(obj)
        %listConnections List all active connections
        %   Tbl = listConnections(OBJ) returns a Nx2 table with headers
        %   ConnectionName and ConnectionInfo, where N is number of
        %   active connections.
        %
        %   Example:
        %       mavlink = mavlinkio('common.xml');
        %       connect(mavlink, 'udp')
        %       listConnections(mavlink)

            tbl = obj.IO.listConnections();
        end

        function disconnect(obj, varargin)
        %DISCONNECT Disconnect active connections
        %    DISCONNECT(OBJ) disconnects all active connections
        %
        %    DISCONNECT(OBJ, CONNECTION) disconnects connection with
        %    name that matches CONNECTION

            narginchk(1,2);
            if nargin == 2
                validateattributes(varargin{1}, {'char', 'string'}, {'scalartext'}, 'disconnect', 'connectName');
            end

            obj.IO.disconnect(varargin{:});

        end

        function tbl = listTopics(obj)
        %listTopics List all topics received on local client
        %    Tbl = listTopics(OBJ) returns a NX5 table with headers
        %    {"SystemID", "ComponentID", "MessageID", "MessageName",
        %    "MessageFrequency"}. N is the number of topics received.
        %    NumMessages is the number of messages received for the
        %    specific message.

            tbl = obj.IO.listTopics();
        end

        function tbl = listClients(obj)
        %listClients List all MAVLink clients communicating with the local client
        %    Tbl = listClients(OBJ) returns a Nx4 table with headers
        %    {"SystemID", "ComponentID", "ComponentType",
        %    "AutopilotType"}. N is the number of MAVLink clients
        %    communicating with MATLAB, including the MAVLink client
        %    created by mavlinkio itself.

            tbl = obj.IO.listClients();

            componentType = strings(size(tbl,1), 1);
            autopilotType = strings(size(tbl,1), 1);

            isLocalClientListed = false;
            for idx = 1:size(tbl, 1)
                sysid = tbl.SystemID(idx);
                compid = tbl.ComponentID(idx);
                client = obj.createClient(sysid, compid);

                componentType(idx) = client.ComponentType;
                autopilotType(idx) = client.AutopilotType;

                isLocalClientListed = isLocalClientListed ||...
                    (sysid == obj.LocalClient.SystemID && ...
                     compid == obj.LocalClient.ComponentID);
            end

            tbl = addvars(tbl, componentType, autopilotType, 'NewVariableNames', {'ComponentType', 'AutopilotType'});

            % add local client to the list if necessary
            if ~isLocalClientListed
                clientInfo = struct2cell(obj.LocalClient);
                tbl = [tbl; reshape(clientInfo(1:4),1,[])];
            end
        end

        function sendmsg(obj, msg, client)
        %SENDMSG Send MAVLink message to connected MAVLink clients
        %    SENDMSG(OBJ, MSG) sends the MSG to all connected MAVLink
        %    clients. MSG is a struct with fields {Payload, MsgID}.
        %    MSG.Payload must be a structure that follows MAVLink message
        %    definitions, with matching field names and field value types
        %
        %    SENDMSG(OBJ, MSG, CLIENT) sends the MSG to the
        %    MAVLink client with match sysid and compid. If no such
        %    client is connected, nothing would be sent

            narginchk(2,3);

            validateattributes(msg, {'struct'}, {'scalar', 'nonempty'}, 'sendmsg', 'msg');

            if nargin == 2
                obj.IO.sendmsg(msg);
            else
                validateattributes(client, {'mavlinkclient'}, {'scalar', 'nonempty'}, 'sendmsg', 'client');
                obj.IO.sendmsg(msg, client.SystemID, client.ComponentID);
            end
        end

        function sendudpmsg(obj, msg, remotehost, remoteport)
        %SENDUDPMSG Send MAVLink message to specified UDP endpoint
        %    SENDUDPMSG(OBJ, MSG, REMOTEHOST, REMOTEPORT) sends the MSG
        %    to the remote udp port specified by host name REMOTEHOST
        %    and port number REMOTEPORT

            narginchk(4,4);
            % basic check for input types and ranges
            validateattributes(msg, {'struct'}, {'scalar', 'nonempty'}, 'sendudpmsg', 'msg');
            validateattributes(remotehost, {'char','string'}, {'scalartext'}, 'sendudpmsg', 'remotehost');
            validateattributes(remoteport, {'numeric'}, {'integer', 'positive', '<=', intmax('uint16')}, 'sendudpmsg', 'remoteport')

            % check for udp connections
            info = listConnections(obj);
            if ~any(contains(info.ConnectionInfo, "UDP", 'IgnoreCase', true))
                error(message('uav:robotuav:mavlink:NotConnectedWithUDP'));
            end

            % check that remote host is in the format of ipv4 'x.x.x.x'
            ipsegments = regexp(convertCharsToStrings(remotehost),'^(\d+).(\d+).(\d+).(\d+)$','tokens');

            % the regular expression should match to the whole remotehost
            % address
            validip = ~isempty(ipsegments);
            % for each ip segment, it must be an integer between 0 and 255
            remoteHostSegments = zeros(1,4);
            if validip
                ipsegments = ipsegments{1};
                for idx = 1:numel(ipsegments)
                    % the regular expression already ensures the match
                    % would be non-negative integer value, only need to
                    % check upper bound
                    remoteHostSegments(idx) = str2double(ipsegments(idx));
                    validip = remoteHostSegments(idx) <= 255;
                    if ~validip
                        break;
                    end
                end
            end
            if validip
                % remove leading zeros to any of the remote host segments
                remotehost = sprintf("%d.%d.%d.%d", remoteHostSegments(1), ...
                remoteHostSegments(2), ...
                    remoteHostSegments(3), ...
                    remoteHostSegments(4));
            else
                error(message('uav:robotuav:mavlink:WrongRemoteHostFormat'));
            end

            obj.IO.sendudpmsg(msg, remotehost, remoteport);
        end

        function buffer = serializemsg(obj, msg)
        %SERIALIZEMSG Serialize MAVLink message to binary buffer
        %    BUFFER = SERIALIZEMSG(OBJ, MSG) serializes MAVLink message
        %    to binary buffer for transmission. MSG must be a
        %    structure that follows MAVLink message definitions, with
        %    matching field names and field value types. This function
        %    should be used if user wants to utilize their own
        %    communication channel.

            narginchk(2,2);
            validateattributes(msg, {'struct'}, {'scalar', 'nonempty'}, 'serializemsg', 'msg');
            buffer = obj.IO.serializemsg(msg);
        end
    end

    methods (Access = {?mavlinkio, ?mavlinkclient})
        function client = createClient(obj, sysid, compid)
        %createClient creates a MAVLink client implementation

            key = obj.hashClientIDs(sysid, compid);
            client = obj.Clients.find(key);
            if isempty(client)
                client = uav.internal.mavlink.Client(sysid, compid);
                obj.Clients.insert(key, client);
            end
        end
    end

    methods (Access = {?mavlinkio, ?mavlinksub})
        function sub = createSubscriber(obj, varargin)
        %createSubscriber constructs a subscriber instance using this
        %IO channel
            sub = uav.internal.mavlink.Subscriber(obj.IO.IOChannel, obj.IO.PayloadParser, varargin{:});
        end
    end

    methods (Static, Access = private)
        function key = hashClientIDs(sysid, compid)
        %create unique key from system id and component id, since
        %they are most maxed out at 255
            key = double(sysid)*256+double(compid);
        end

        function updateClients(clients, dialect, msgExt)
        %updateClients Heartbeat subscriber callback
            key = mavlinkio.hashClientIDs(msgExt.SystemID, msgExt.ComponentID);
            type = convertCharsToStrings(dialect.num2enum('MAV_TYPE', msgExt.Payload.type));
            autopilot = convertCharsToStrings(dialect.num2enum('MAV_AUTOPILOT', msgExt.Payload.autopilot));
            client = clients.find(key);
            if isempty(client)
                client = uav.internal.mavlink.Client(...
                    msgExt.SystemID, msgExt.ComponentID, type, autopilot);
                clients.insert(key, client);
            elseif isvalid(client)
                client.ComponentType = type;
                client.AutopilotType = autopilot;
            end
        end
    end

end
