classdef mavlinkdialect < handle
%MAVLINKDIALECT Parse and store MAVLink dialect defined by xml file
%   MAVLINKDIALECT object parses and stores message and enum
%   definitions extracted from MAVLink message definition XML file. The
%   XML file defines the messages supported for this specific dialect.
%   The structure is defined by the MAVLink message protocol.
%
%   m = MAVLINKDIALECT(XMLFILEPATH) creates a MAVLink dialect based on
%   message definition file XMLFILEPATH with mavlink protocol version 2.
%
%   m = MAVLINKDIALECT(XMLFILEPATH, VERSION) additionally specifies mavlink
%   version
%
%   m = MAVLINKDIALECT(XMLFILEPATH, VERSION, SigningChannel=Value)
%   uses signing channel generated by addChannel from mavlinksigning
%   to authenticate MAVLink packets.
%
%   MAVLINKDIALECT properties:
%       DialectXML              - MAVLink dialect name
%       Version                 - MAVLink protocol version
%
%   MAVLINKDIALECT methods:
%       msginfo        - Query message definition based on given message ID
%       enuminfo       - Query enum definition based on given enum name
%       enum2num       - Return enum value that matches given enum entry name
%       num2enum       - Return enum entry name that matches given enum value
%       createmsg      - Create blank MAVLink message
%       createcmd      - Create blank MAVLink command message
%       deserializemsg - Deserialize MAVLink message from binary buffer
%
%   Example:
%
%       % Create a dialect based on common.xml
%       dialect = mavlinkdialect('common.xml');
%
%       % Query about message
%       msginfo(dialect, 'HEARTBEAT')
%
%   See also mavlinkio, mavlinkclient, mavlinksub

%   Copyright 2018-2022 The MathWorks, Inc.


    properties (SetAccess = private)
        %DialectXML MAVLink dialect xml name
        DialectXML

        %Version MAVLink protocol version
        Version
    end

    properties (Access = {?mavlinkdialect, ?matlab.unittest.TestCase, ?uav.internal.mavlink.TabCompletion, ?mavlinktlog})
        %PayloadParser Internal MAVLink message payload parser
        PayloadParser

        %Serializer
        Serializer

        %SigningChannel
        SigningChannel
    end

    methods
        function obj = mavlinkdialect(xmlfilepath, version, varargin)
        %MAVLINKDIALECT

            obj.SigningChannel = [];
            if ~uav.internal.mavlink.isSigningEnabled()
                narginchk(1,2);
            else
                p = inputParser;
                p.addParameter("SigningChannel", [], @(x)validateattributes(x, "struct", ["scalar", "nonempty"], "mavlinkdialect", "SigningChannel"));
                p.parse(varargin{:});
                obj.SigningChannel = p.Results.SigningChannel;
            end

            validateattributes(xmlfilepath, {'string', 'char'}, ...
                               {'scalartext', 'nonempty'}, 'mavlinkdialect', 'xmlfilepath');
            robotics.internal.validation.findFilePath(convertStringsToChars(xmlfilepath), 'mavlinkdialect', 'xmlfilepath');
            [~, obj.DialectXML, extension] = fileparts(convertCharsToStrings(xmlfilepath));
            if ~strcmpi(extension, '.xml')
                error(message('uav:robotuav:mavlink:WrongMessageDefinitionFileExt'));
            end

            if nargin == 1
                % set default protocol version to 2
                version = 2;
            else
                validateattributes(version, {'numeric'}, {'integer', 'scalar', '>=', 1, '<=', 2},  'mavlinkdialect', 'version');
                if ~isempty(obj.SigningChannel) && version == 1
                    error(message('uav:robotuav:mavlink:SigningNotSupportedInV1'));
                end
            end
            obj.Version = double(version);

            msgdef = uav.internal.mavlink.parser.Import(xmlfilepath, obj.Version);
            obj.PayloadParser = uav.internal.mavlink.parser.PayloadParser(msgdef, obj.Version);

            % initialize MAVLink channel parser with imported message
            % definitions
            updateSerializer(obj);
        end

        function msg = createmsg(obj, msgid)
        %CREATEMSG Create blank message based on message ID or message name.
        %   MSG = CREATEMSG(obj, MSGID) returns a blank message
        %   structure based on the message definition and the provided
        %   MSGID. If MSGID is a non-negative integer, the message
        %   definition with matching message ID is returned. If MSGID
        %   is a string, the message definition with matching message
        %   name is returned. The returned message structure has
        %   following fields: {Payload, MsgID}

            narginchk(2,2);
            idx = findmsg(obj, msgid, 'createmsg');

            if any(idx)
                msg = uav.internal.mavlink.Structures.createMsg(1);
                msg(1).MsgID = uint32(obj.PayloadParser.MessageDefinitions.Messages.ID(idx));
                msg(1).Payload = obj.PayloadParser.MessagePrototypes.find(msg.MsgID(1)).Prototype;
            else
                error(message('uav:robotuav:mavlink:UndefinedMessageIDToParse', msgid, obj.DialectXML));
            end
        end

        function msg = createcmd(obj, cmdsetting, cmdtype)
        %CREATECMD Create blank MAVLink command message
        %   MSG = CREATECMD(obj, CMDSETTING, CMDTYPE) returns a blank
        %   COMMAND_INT or COMMAND_LONG message structure based on
        %   CMDSETTING, which must be either "int" or "long". The
        %   CMDTYPE is either a non-negative integer that matches
        %   "MAV_CMD" enum values, or a string that matches "MAV_CMD"
        %   enum field names. The returned command message is
        %   constructed with the matching "MAV_CMD" enum value.

            narginchk(3,3);
            cmdsetting = validatestring(cmdsetting, ["long", "int"], "createcmd", "cmdsetting");

            if strcmp(cmdsetting, "long")
                msg = createmsg(obj, "COMMAND_LONG");
            else
                msg = createmsg(obj, "COMMAND_INT");
            end

            if isnumeric(cmdtype)
                validateattributes(cmdtype, {'numeric'}, {'integer', 'nonnegative', 'scalar'}, 'createcmd', 'cmdtype');
                % check that the MAV_CMD:cmdtype is a valid enum
                obj.num2enum('MAV_CMD', cmdtype);
                msg.Payload.command(:) = cmdtype;
            else
                validateattributes(cmdtype, {'char', 'string'}, {'scalartext','nonempty'}, 'createcmd', 'cmdtype');
                msg.Payload.command(:) = enum2num(obj, 'MAV_CMD', cmdtype);
            end

        end

        function tbl = msginfo(obj, msgid)
        %MSGINFO Query message definition based on given message ID
        %   TBL = MSGINFO(dialect, messageID) returns a table detailing the
        %   message definition based on messageID. messageID can be either
        %   an integer value or a string. TBL is an Nx4 table with column
        %   headers MESSAGEID, MESSAGENAME, DESCRIPTION, and FIELDS. N is
        %   the number of messages that match the messageID. The table
        %   lists all messages in the MAVLink dialect that have this
        %   matching message ID or name, ignoring case.

            narginchk(2,2);
            idx = findmsg(obj, msgid, 'msginfo');

            if isempty(idx) || ~any(idx)
                tbl = cell2table(cell(0,4), 'VariableNames', {'ID', 'Name', 'Description', 'Fields'});
            else
                id = obj.PayloadParser.MessageDefinitions.Messages.ID(idx);
                names = obj.PayloadParser.MessageDefinitions.Messages.Names(idx);
                descriptions = obj.PayloadParser.MessageDefinitions.Messages.Descriptions(idx);

                msgfields = obj.PayloadParser.MessageDefinitions.Messages.Fields(idx);
                if obj.Version == 1 && obj.PayloadParser.MessageDefinitions.Messages.IsExtended(idx)
                    numFields = obj.PayloadParser.MessageDefinitions.Messages.ExtendedStartIdx(idx)-1;
                else
                    numFields = cellfun(@numel, {msgfields.Sizes});
                end
                fieldtables = cell(numel(msgfields), 1);
                for i = 1:numel(msgfields)
                    msgfield = msgfields(i);
                    numField = numFields(i);
                    fieldtables{i} = table(msgfield.Names(1:numField)', msgfield.Types(1:numField)', msgfield.Sizes(1:numField)', ...
                                           msgfield.EnumTypes(1:numField)', msgfield.Units(1:numField)', msgfield.Descriptions(1:numField)', ...
                                           'VariableNames', {'Name', 'Type', 'Size', 'EnumType', 'Units', 'Description'});
                end

                tbl = table(id, names, descriptions, fieldtables, ...
                            'VariableNames', {'MessageID', 'MessageName', 'Description', 'Fields'});
            end

        end

        function tbl = enuminfo(obj, enum)
        %ENUMINFO Query enum definition based on given enum name
        %   TBL = ENUMINFO(dialect, enumID) returns a table detailing
        %   the enum definition based on a string enumID. TBL is an Nx3
        %   table with column headers NAME, DESCRIPTION, and ENTRIES. N
        %   is the number of enums whose name matches enumID, ignoring
        %   case.

            narginchk(2,2);
            idx = findenum(obj, enum, 'enuminfo');

            if ~any(idx)
                tbl = cell2table(cell(0,3), 'VariableNames', {'Name', 'Description', 'Entries'});
            else
                name = obj.PayloadParser.MessageDefinitions.Enums.Names(idx);
                descriptions = obj.PayloadParser.MessageDefinitions.Enums.Descriptions(idx);

                entries = obj.PayloadParser.MessageDefinitions.Enums.Entries(idx);
                entrytables = cell(numel(entries), 1);
                for i = 1:numel(entries)
                    entry = entries(i);
                    entrytables{i} = table(entry.Names', entry.Values', entry.Descriptions', ...
                                           'VariableNames', {'Name', 'Value', 'Description'});
                end

                tbl = table(name, descriptions, entrytables, ...
                            'VariableNames', {'Name', 'Description', 'Entries'});
            end
        end

        function val = enum2num(obj, enum, entry)
        %ENUM2NUM Return enum value that matches given enum entry
        %   VALUE = ENUM2NUM(DIALECT, ENUM, ENTRY) returns the value
        %   for the given ENTRY defined in the given ENUM. The ENUM and
        %   ENTRY are strings that match MAVLink Enum and Enum Fields
        %   definitions.

            narginchk(3,3);
            validateattributes(entry, {'char', 'string'}, {'scalartext','nonempty'}, 'enum2num', 'entry');
            idx = findenum(obj, enum, 'enum2num');
            enumdef = obj.PayloadParser.MessageDefinitions.Enums.Entries(idx);

            if isempty(enumdef)
                error(message('uav:robotuav:mavlink:UndefinedEnumEntryToParse', enum, entry, obj.DialectXML));
            else
                entryidx = strcmpi(enumdef.Names, entry);

                val = enumdef.Values(entryidx);

                if isempty(val)
                    error(message('uav:robotuav:mavlink:UndefinedEnumEntryToParse', enum, entry, obj.DialectXML));
                end
            end
        end

        function entry = num2enum(obj, enum, val)
        %NUM2ENUM Return enum entry name that matches given enum value
        %   ENTRY = NUM2ENUM(DIALECT, ENUM, ENTRY, VAL) returns the
        %   entry name for the given VAL defined in the given ENUM. The
        %   ENUM is a string that match MAVLink Enum and VAL is a
        %   non-negative integer

            narginchk(3,3);
            validateattributes(val, {'numeric'}, {'nonnegative', 'integer', 'scalar'}, 'num2enum', 'val');
            idx = findenum(obj, enum, 'num2enum');
            enumdef = obj.PayloadParser.MessageDefinitions.Enums.Entries(idx);

            if isempty(enumdef)
                error(message('uav:robotuav:mavlink:UndefinedEnumValueToParse', enum, val, obj.DialectXML));
            else
                entryidx = enumdef.Values == val;

                entry = enumdef.Names(entryidx);

                if isempty(entry)
                    error(message('uav:robotuav:mavlink:UndefinedEnumValueToParse', enum, val, obj.DialectXML));
                end
            end
        end

        function varargout = deserializemsg(obj, buffer, varargin)
        %DESERIALIZEMSG Deserialize MAVLink message from binary buffer
        %   MSGEXT = DESERIALIZEMSG(obj, BUFFER) de-serializes binary
        %   buffer received through communication channel to MAVLink
        %   messages. BUFFER must be an array of uint8 integers.
        %   Messages obtained from the Buffer would be returned as a
        %   struct array with Payload, MsgID, SystemID, ComponentID,
        %   and Seq fields. The payload follows MAVLink message
        %   structures. When a message was divided into multiple
        %   buffers, if the buffers are passed into this method in the
        %   same order they are received, the message would be
        %   eventually de-serialized and returned.
        %
        %   [MSGEXT, STATUS] = DESERIALIZEMSG(obj, BUFFER,
        %   OutputAllMessages=true) returns all messages MSGEXT that can be
        %   deserialized from buffer, and the status STATUS of each
        %   message, even if there is error in signature check or checksum
        %   check. The STATUS is an N-element row vector of integers
        %   between 0 and 2, where N is the total number of messages in
        %   MSGEXT. The i-th element in STATUS corresponds to the parse
        %   state of the i-th message in MSGEXT. These are the possible
        %   STATUS values and their meanings for their corresponding
        %   message:
        %      - 0 - Correctly serialized
        %      - 1 - Checksum mismatch
        %      - 2 - Signature mismatch
        
            outputAllMessages = false;

            if uav.internal.mavlink.isSigningEnabled()
                p = inputParser;
                p.addParameter("OutputAllMessages", false, @(x)validateattributes(x, "logical", ["scalar", "nonempty"], "deserializemsg", "OutputAllMessages"));
                p.parse(varargin{:});
                outputAllMessages = p.Results.OutputAllMessages;
                nargoutchk(0,2);
            else
                narginchk(2,2);
                nargoutchk(0,1);
            end
            
            validateattributes(buffer, {'uint8'}, {'vector'}, 'deserializemsg', 'buffer');

            out = obj.Serializer.deserialize(buffer);
            packets = out{1};
            status = out{2};
            % Only return good message frames
            if outputAllMessages
                msgCount = numel(status);
            else
                msgCount = sum(status == 1);
            end
            msgExt = uav.internal.mavlink.Structures.createMsgExt(msgCount);

            outputIdx = 1;
            for idx = 1:numel(packets)
                % Add message if the OutputAllMessages flag is true, or the
                % message parse status is MAVLINK_FRAME_OK
                if outputAllMessages || status(idx) == 1
                    msgExt(outputIdx).MsgID = packets(idx).MsgID;
                    msgExt(outputIdx).SystemID = packets(idx).SystemID;
                    msgExt(outputIdx).ComponentID = packets(idx).ComponentID;
                    msgExt(outputIdx).Seq = packets(idx).Seq;
                    msgExt(outputIdx).Payload = obj.PayloadParser.parse(packets(idx).MsgID, packets(idx).Payload);
                    outputIdx = outputIdx + 1;
                end
            end

            varargout{1} = msgExt;
            if uav.internal.mavlink.isSigningEnabled() && nargout == 2
                % Return status from MATLAB is shifted by 1
                % 0: MAVLINK_FRAME_OK
                % 1: MAVLINK_BAD_CRC
                % 2: MAVLINK_BAD_SIGNATURE
                varargout{2} = double(status)-1;
            end

        end

        function sobj = saveobj(obj)
        %SAVEOBJ Specify save behavior
            sobj.DialectXML = obj.DialectXML;
            sobj.Version = obj.Version;
            sobj.PayloadParser = obj.PayloadParser;
            sobj.SigningChannel = obj.SigningChannel;
        end

        function cobj = copy(obj)
        %COPY Create a deep copy of current dialect
        %   Creates a dummy dialect first then overwrite its content
            cobj = mavlinkdialect('test.xml');
            cobj.DialectXML = obj.DialectXML;
            cobj.Version = obj.Version;
            cobj.PayloadParser = obj.PayloadParser;
            cobj.SigningChannel = obj.SigningChannel;
            updateSerializer(cobj);
        end
    end

    methods (Static)

        function obj = loadobj(sobj)
        %LOADOBJ Specify load behavior
        %   Creates a dummy dialect first then overwrite its content
            obj = mavlinkdialect('test.xml');
            obj.DialectXML = sobj.DialectXML;
            obj.Version = sobj.Version;
            obj.PayloadParser = sobj.PayloadParser;
            if isfield(sobj, "SigningChannel")
                obj.SigningChannel = sobj.SigningChannel;
            else
                % handle mat-file loading from before 22a
                obj.SigningChannel = [];
            end
            updateSerializer(obj);
        end

    end

    methods (Access = private)

        function idx = findmsg(obj, msgid, fcnname)
        %findmsg returns the logical index for the MAVLink message that matches msgid.

            if isnumeric(msgid)
                validateattributes(msgid, {'numeric'}, {'integer', 'nonnegative', 'scalar'}, fcnname, 'msgid');
                idx = obj.PayloadParser.MessageDefinitions.Messages.ID == msgid;
            else
                validateattributes(msgid, {'char', 'string'}, {'scalartext', 'nonempty'}, fcnname, 'msgid');
                idx = strcmpi(obj.PayloadParser.MessageDefinitions.Messages.Names, convertCharsToStrings(msgid));
            end

        end

        function idx = findenum(obj, enum, fcnname)
        %findenum returns the logical index for the MAVLink Enum that
        %matches enum by name

            validateattributes(enum, {'char', 'string'}, {'scalartext', 'nonempty'}, fcnname, 'enum');

            idx = strcmpi(obj.PayloadParser.MessageDefinitions.Enums.Names, enum);
        end

        function updateSerializer(obj)
        %updateSerializer creates MCOS MAVLinkChannel and update it
        %with new dialect information

            msgdef = obj.PayloadParser.MessageDefinitions;
            obj.Serializer = uav.internal.MAVLinkSerializer;
            messageInfo = zeros(1,7*msgdef.Messages.Count);
            [sortedID, sortedIdx] = sort(msgdef.Messages.ID);
            messageInfo(1:7:end) = sortedID;
            messageInfo(2:7:end) = msgdef.Messages.CRCExtra(sortedIdx);
            messageInfo(3:7:end) = msgdef.Messages.MinLength(sortedIdx);
            messageInfo(4:7:end) = msgdef.Messages.MaxLength(sortedIdx);
            obj.Serializer.updateMessageEntries(messageInfo);
            obj.Serializer.setVersion(obj.Version);

            if ~isempty(obj.SigningChannel)
                obj.Serializer.setSigningChannel(obj.SigningChannel.Stream.SigningStream, ...
                    obj.SigningChannel.SystemID, obj.SigningChannel.ComponentID, obj.SigningChannel.LinkID);
            end
        end

    end

    methods (Access = {?mavlinkdialect, ?mavlinkio})
        function io = createIO(obj, varargin)
        %createIO creates a mavlink IO implementation
            if ~isempty(obj.SigningChannel)
                if isempty(varargin{1})
                    % user only supplied signing channel
                    varargin{1} = obj.SigningChannel.SystemID;
                elseif obj.SigningChannel.SystemID ~= varargin{1}
                    % user supplied signing channel and system ID
                    error(message('uav:robotuav:mavlink:IOSystemIDMismatch',varargin{1},obj.SigningChannel.SystemID));
                end
                if isempty(varargin{2})
                    % user only supplied signing channel
                    varargin{2} = obj.SigningChannel.ComponentID;
                elseif obj.SigningChannel.ComponentID ~= varargin{2}
                    % user supplied signing channel and component ID
                    error(message('uav:robotuav:mavlink:IOComponentIDMismatch',varargin{1},obj.SigningChannel.ComponentID));
                end
            else
                if isempty(varargin{1})
                    % user didn't supply system ID, use default
                    varargin{1} = 255;
                end
                if isempty(varargin{2})
                    % user didn't supply component ID, use default
                    varargin{2} = 1;
                end
            end
            io = uav.internal.mavlink.IO(obj.PayloadParser, obj.Serializer, varargin{:});
        end
    end
end
