classdef uavSensor < handle
%uavSensor Create a UAV sensor for use in scenario
%   uavSensor creates a sensor that is rigidly attached to uavPlatform. You
%   can specify different mounting location and angles. You can configure
%   this object to automatically generate readings from insSensor,
%   gpsSensor, gnssMeasurementGenerator, and uavLidarPointCloudGenerator
%   sensor at fixed rate.
%
%   SENSOR = uavSensor(NAME, PLATFORM, SENSORMODEL) creates a SENSOR
%   that generates readings based on input SENSORMODEL and motion data
%   from PLATFORM. SENSOR is uniquely identified as PLATFORM.NAME/NAME
%
%   s = uavSensor(___, Name, Value) additionally take in the following
%   name value pairs:
%      MountingLocation       - Sensor's (x,y,z) mounted location on platform (m)
%      MountingAngles         - Mounting rotation angles of sensor mounted on platform
%      UpdateRate             - Sensor's update rate (Hz)
%
%   uavSensor properties:
%      Name                   - Sensor's name
%      MountingLocation       - Sensor's (x,y,z) mounted location on platform (m)
%      MountingAngles         - Mounting rotation angles of sensor mounted on platform
%      UpdateRate             - Sensor's update rate (Hz)
%      SensorModel            - Sensor's simulation model
%
%   uavSensor methods:
%      read                    - Gather latest sensor readings
%
%   Example:
%
%      % Create scenario and platform
%      scenario = uavScenario();
%      platform = uavPlatform("UAV", scenario);
%
%      % Create a GPS sensor and attach to platform
%      gps = uavSensor("GPS", platform, gpsSensor(), "UpdateRate", scenario.UpdateRate);
%
%      % Gather initial readings at time 0
%      setup(scenario);
%      [isUpdated, t, lla, vel, groundspeed, course] = read(gps)
%
%      % Simulate the scenario and gather readings
%      advance(scenario);
%      updateSensors(scenario);
%      [isUpdated, t, lla, vel, groundspeed, course] = read(gps)
%
%   See also uavScenario, uavPlatform, insSensor, gpsSensor,
%   gnssMeasurementGenerator, uavLidarPointCloudGenerator

%   Copyright 2020-2023 The MathWorks, Inc.

    properties (Dependent, SetAccess = private)
        %Name Sensor's name
        %   Name is a scalar string that identifies the senor. It is in the
        %   format of PlatformName/SensorName.
        Name

        %SensorModel Sensor's simulation model
        %   SensorModel is a insSensor, gpsSensor or
        %   uavLidarPointCloudGenerator object. This sensor model is used to
        %   generate sensor readings
        SensorModel

        %MountingLocation Sensor's (x,y,z) mounted location on platform (m)
        %   A 1-by-3 vector defining the offset of the sensor's origin from
        %   the origin of its platform. The default value leaves the
        %   system's origin at the same location as the origin of its
        %   platform.
        %
        %   Default: [0 0 0]
        MountingLocation

        %MountingAngles Mounting rotation angles of sensor mounted on platform
        %   Specify a 3-element vector corresponding to the intrinsic Euler
        %   angle rotations which align the body axes of the platform on
        %   which the system is mounted to the system's axes. The 3
        %   elements define the rotations around the z, y, and x axes
        %   respectively, in that order. The first rotation, rotates the
        %   body axes around the z-axis. Because these angles define
        %   intrinsic rotations, the second rotation is performed around
        %   the y-axis in its new position resulting from the previous
        %   rotation. The final rotation around the x-axis is performed
        %   around the x-axis as rotated by the first two rotations in the
        %   intrinsic system. These 3 Euler angles are sometimes referred
        %   to as yaw, pitch, and roll, however in this case, these
        %   rotations are relative to the platform's body frame and do not
        %   refer to the scenario frame. The angles are in degrees.
        %
        %   Default: [0 0 0]
        MountingAngles

        %UpdateRate Sensor's update rate (Hz)
        %   UpdateRate is a positive scalar that determines the update rate
        %   of the sensor. If not specified, this value is the same as the
        %   update rate of the sensor model used to construct this object
        %   if available. If the sensor model doesn't have update rate,
        %   this value is the same as the platform's update rate that is
        %   used to construct this object.
        UpdateRate
    end

    properties (Access = {?uavSensor, ?uav.internal.scenario.SceneInterface, ?matlab.unittest.TestCase})
        %SensorImpl Implementation of UAV sensor
        SensorImpl

        %PlatformImpl Implementation of the UAV platform
        PlatformImpl
    end

    methods
        function name = get.Name(obj)
            name = obj.SensorImpl.Name;
        end

        function smodel = get.SensorModel(obj)
            smodel = obj.SensorImpl.SensorImpl.getSensorModel;
        end

        function r = get.UpdateRate(obj)
            r = obj.SensorImpl.UpdateRate;
        end

        function location = get.MountingLocation(obj)
            tform = obj.SensorImpl.SensorImpl.TFTree.getTransform(obj.PlatformImpl.Name, obj.Name);
            location = tform2trvec(tform);
        end

        function angles = get.MountingAngles(obj)
            tform = obj.SensorImpl.SensorImpl.TFTree.getTransform(obj.PlatformImpl.Name, obj.Name);
            angles = rad2deg(tform2eul(tform));
        end
    end

    methods
        function obj = uavSensor(name, platform, sensorModel, varargin)
        %uavSensor

        % validate inputs
            validateattributes(platform, "uavPlatform", "scalar", "uavSensor", "platform");

            name = convertCharsToStrings(name);
            validateattributes(name, ["char", "string"], "scalartext", "uavSensor", "name");
            combinedName = platform.Name+"/"+name;
            if any(strcmp(combinedName, [platform.Sensors.Name]))
                error(message("uav:robotuav:scenario:SensorNameNotUnique", name, platform.Name));
            end

            % parse inputs
            p = inputParser;
            % Mounting default values needs to be updated to adapt to
            % detection generator sensors
            p.addParameter("MountingLocation", [],...
                           @(x)validateattributes(x, "numeric", {"nrows", 1, "ncols", 3, "2d"}, "uavSensor", "MountingLocation"));
            p.addParameter("MountingAngles", [],...
                           @(x)validateattributes(x, "numeric", {"nrows", 1, "ncols", 3, "2d"}, "uavSensor", "MountingAngles"));
            p.addParameter("UpdateRate", [],...
                           @(x)validateattributes(x, "numeric", ["scalar", "positive", "real", "finite"], "uavSensor", "UpdateRate"));
            p.parse(varargin{:});
            tform = struct("MountingLocation", double(p.Results.MountingLocation), "MountingAngles", double(p.Results.MountingAngles));

            if isempty(p.Results.UpdateRate)
                obj.SensorImpl = uav.internal.scenario.Sensor(platform.ScenarioImpl.Parent, ...
                                                              platform, combinedName, tform, sensorModel);
            else
                obj.SensorImpl = uav.internal.scenario.Sensor(platform.ScenarioImpl.Parent, ...
                                                              platform, combinedName, tform, sensorModel, ...
                                                              double(p.Results.UpdateRate));
            end

            sceneRate = platform.ScenarioImpl.UpdateRate;
            sensorRate = obj.SensorImpl.UpdateRate;
            if floor(sceneRate/sensorRate) ...
                    ~= sceneRate/sensorRate
                error(message('uav:robotuav:scenario:SensorUpdateRateMismatch',num2str(sceneRate),num2str(sensorRate)));
            end

            obj.PlatformImpl = platform.PlatformImpl;
            platform.PlatformImpl.addSensor(obj.SensorImpl);
            platform.Sensors(end+1) = obj;
        end

        function varargout = read(obj)
        %READ Gather latest sensor readings
        %   [ISUPDATED, T, VARARGOUT] = READ(S) reads the simulated
        %   sensor output from the latest update. It returns a Boolean
        %   ISUPDATED to indicate whether this value read here is newly
        %   generated in the last scenario simulation step. It returns
        %   a scalar T to indicate the time at which the readings are
        %   generated. VARARGOUT is the simulated sensor output, which
        %   depends on the sensor model specified in this object.
        %   varargout is the same as the step output from these
        %   sensors.

            [varargout{1:nargout}] = obj.SensorImpl.read();
        end
    end
end
