classdef Ros2ModelHelper
    %Ros2ModelHelper Helper class which contains implementations specific to ROS2
    
    %   Copyright 2023 The MathWorks, Inc.
    
    methods
        function fieldMap = getFieldMap(~, dataType)
            fieldMap = [];
            if(strcmp(dataType,"odometry"))
                fieldMap.position.x = "pose.pose.position.x";
                fieldMap.position.y = "pose.pose.position.y";
                fieldMap.position.z = "pose.pose.position.z";
                fieldMap.orientation.x = "pose.pose.orientation.x";
                fieldMap.orientation.y = "pose.pose.orientation.y";
                fieldMap.orientation.z = "pose.pose.orientation.z";
                fieldMap.orientation.w = "pose.pose.orientation.w";
            end
        end

        function [msg, type] = getMsgFieldType(~, msgType, fieldName)
            % Get a default message and type of a field from a message type.
            
            msgInfo = ros.internal.ros2.getMessageInfo(msgType);
            [~, info] = eval(msgInfo.msgStructGen);
            val = info.(fieldName);

            type = val.MessageType;
            msgInfo = ros.internal.ros2.getMessageInfo(type);
            [msg, ~] = eval(msgInfo.msgStructGen);
        end
        
        function msgStruct = getMsgStructOfType(~, type)
            %Returns a default msg struct

            msgStruct = ros2message(type);
        end

        function [topicNames, topicMsgType] = getTopicNameAndType(~,rosDomainID)
        %getTopicNamesType Get topic names and messages for topics
        
            h = ros.ros2.internal.Introspection;
            ret = h.topiclisttypes([], rosDomainID);
            topicNames = ret(:,1);
            topicMsgType = cellfun(@(x)x{1}, ret(:,2), 'UniformOutput', false);
        end

        function hNode = createNode(~, name, rosDomainID)
            %create a node with given name

            hNode = ros2node(name,rosDomainID);
        end

        function subscriber = createSubscriber(~, hNode, topic, readOption, fieldPath)
            %Creates a subscriber for the topic using the given node

            subscriber = ros2subscriber(hNode,topic);

            if readOption == "message" && isempty(fieldPath)
                return;
            end

            subscriber.setMsgPreprocessing(readOption,fieldPath);
        end

        function [topicNames , topicTypes] = filterTopics(~,topicNames,topicTypes)
            %filterTopics validates and filters the topic list
            
            supportedMessageTypes = ros2type.getMessageList;
            topicsToRemove = false(numel(topicTypes), 1);
            for topicIdx = 1:numel(topicTypes)
                if ~any(ismember(topicTypes{topicIdx}, supportedMessageTypes))
                    topicsToRemove(topicIdx) = 1;
                end
            end

            topicNames(topicsToRemove) = [];
            topicTypes(topicsToRemove) = [];
        end
    end
end

