classdef IO < handle
%This class is for internal use only. It may be removed in the future.

%IO internal implementation of mavlinkio

%   Copyright 2018-2020 The MathWorks, Inc.

    properties (SetAccess = private)
        %LocalClient Local MAVLink Client Information
        LocalClient

        %IOChannel MCOS MAVLink communication channel
        IOChannel

        %Serializer MCOS MAVLink serialzier
        Serializer

        %Dialect MAVLink dialect
        PayloadParser
    end

    methods
        function obj = IO(payloadParser, serializer, sysid, compid, compType, autopilotType)
        %IO Construct an instance of this class

            obj.PayloadParser = payloadParser;

            obj.LocalClient = uav.internal.mavlink.Client(sysid, compid);
            obj.LocalClient.ComponentType = convertCharsToStrings(compType);
            obj.LocalClient.AutopilotType = convertCharsToStrings(autopilotType);

            obj.IOChannel = uav.internal.MAVLinkChannel(serializer);
            obj.Serializer = obj.IOChannel.getSerializer();
            obj.IOChannel.run();
        end

        function delete(obj)
        %delete during destruction, disconnect IO channel
            obj.IOChannel.disconnect();
        end

        function connectName = connectUDP(obj, localport, connectName)
            connectName = convertCharsToStrings(obj.IOChannel.connectUDP(...
                uint16(localport), convertCharsToStrings(connectName)));
        end

        function tbl = listConnections(obj)
        %listConnections list all open connections
            connectInfo = convertCharsToStrings(obj.IOChannel.listConnections());
            names = reshape(connectInfo(1:2:end), [], 1);
            info = reshape(connectInfo(2:2:end), [], 1);
            tbl = table(names, info, 'VariableNames', {'ConnectionName', 'ConnectionInfo'});
        end

        function disconnect(obj, connectName)
        % disconnect Close connections.

            if nargin == 1
                obj.IOChannel.closeAllSockets();
            else
                obj.IOChannel.closeSocket(convertCharsToStrings(connectName));
            end

        end

        function tbl = listTopics(obj)
        % listTopics list all topics received over the MAVLink network.

            topics = obj.IOChannel.listMAVLinkTopics();
            msgids = reshape(topics(1:4:end),[],1);
            sysids = reshape(topics(2:4:end),[],1);
            compids = reshape(topics(3:4:end), [], 1);
            msgcounts = reshape(topics(4:4:end), [], 1);
            msgnames = strings(numel(msgids), 1);

            for idx = 1:numel(msgids)
                msgidx = obj.PayloadParser.MessageDefinitions.Messages.ID == msgids(idx);
                msgnames(idx) = obj.PayloadParser.MessageDefinitions.Messages.Names(msgidx);
            end

            tbl = table(msgids, msgnames, sysids, compids, msgcounts, ...
                        'VariableNames', {'MessageID', 'MessageName', 'SystemID', ...
                                'ComponentID', 'MessageFrequency'});
        end

        function tbl = listClients(obj)
        %listClients List MAVLink client communicating with MATLAB.

            clients = obj.IOChannel.listMAVLinkClients();
            sysids = reshape(clients(1:2:end),[],1);
            compids = reshape(clients(2:2:end), [], 1);
            tbl = table(sysids, compids, 'VariableNames', {'SystemID', 'ComponentID'});
        end

        function sendmsg(obj, msg, sysid, compid)
        %sendmsg Send a MAVLink message to clients

            packet = obj.msgToPacket(msg);

            if nargin == 2
                obj.IOChannel.broadcastMsg(packet);
            else
                obj.IOChannel.sendMsg(packet, uint8(sysid), uint8(compid));
            end

        end

        function sendudpmsg(obj, msg, remotehost, remoteport)
        %sendudpmsg Send a MAVLink message to remote UDP target
            packet = obj.msgToPacket(msg);
            obj.IOChannel.sendUDPMsg(packet, convertCharsToStrings(remotehost), uint16(remoteport));
        end

        function buffer = serializemsg(obj, msg)
        %serializemsg Serialize MAVLink message to binary buffer

            packet = obj.msgToPacket(msg);
            buffer = obj.Serializer.serialize(packet);

        end
    end

    methods (Access = private)
        function packet = msgToPacket(obj, msg)
        %msgToPacket convert struct with fields ['MsgID', 'Payload'] to
        %MAVLink packet
            packet = uav.internal.mavlink.Structures.createPacket();
            packet.MsgID(:) = msg.MsgID;
            packet.Payload(:) = obj.PayloadParser.serialize(msg.MsgID, msg.Payload);

            msgidx = obj.PayloadParser.MessageDefinitions.Messages.ID == msg.MsgID;
            minlen = obj.PayloadParser.MessageDefinitions.Messages.MinLength(msgidx);
            len = obj.PayloadParser.MessageDefinitions.Messages.MaxLength(msgidx);
            crc = obj.PayloadParser.MessageDefinitions.Messages.CRCExtra(msgidx);

            packet = obj.Serializer.finalizeMsg(packet, ...
                                                obj.LocalClient.SystemID, ...
                                                obj.LocalClient.ComponentID, ...
                                                uint8(minlen), ...
                                                uint8(len), ...
                                                uint8(crc));
        end
    end

end
