function emBody = emissionsInBody(emSceneIn, bodyFrame)
% emissionsInBody  Transform emissions to the body frame of a platform
%   EMBODY = emissionsInBody(EMSCENE, BODYFRAME) returns emissions,
%   EMSCENE, converted into the body frame of the platform specified by
%   BODYFRAME. EMSCENE is an array or cell array of emission objects.
%   EMBODY has the same format as EMSCENE.
%
%   BODYFRAME is either a scalar struct or a platform object provided by
%   the Platforms property on trackingScenario.
%
%   When BODYFRAME is a scalar struct, it is specified with the following
%   fields:
%       Position        A 3-element vector specifying the position of the
%                       local reference frame's origin relative to its
%                       global frame in meters.
%
%                       Default: [0 0 0]
%
%       Velocity        A 3-element vector specifying the velocity of the
%                       local reference frame's origin relative to its
%                       global frame in meters per second.
%
%                       Default: [0 0 0]
%
%       Orientation     A scalar quaternion or a 3-by-3 real-valued
%                       orthonormal frame rotation matrix specifying the
%                       orientation of the local reference frame relative
%                       to its global frame.
%
%                       Default: eye(3)
%
%   Any structure defining the fields above can be used to define a
%   platform's body frame. For example, the structures returned by
%   the platformPoses method on trackingScenario can be used.
%
%   EXAMPLE 1: Convert a radar emission from scenario to body frame.
%
%   % Define a radar emission in the scenario frame.
%   emScene = radarEmission('PlatformID', 1, 'EmitterIndex', 1, ...
%                   'OriginPosition', [0 0 0])
%   
%   % Define the body frame for a platform relative to the scenario frame.
%   bodyFrame = struct( ...
%       'Position', [10 0 0], ...
%       'Velocity', [5 5 0], ...
%       'Orientation', quaternion([45 0 0], 'eulerd', 'zyx', 'frame'));
%
%   % Convert the emission's frame to the scenario frame.
%   emBody = emissionsInBody(emScene, bodyFrame)
%
%   EXAMPLE 2: Convert a reflected radar emission back to radar body frame.
%
%   % Create a radar emitter
%   emitter = radarEmitter(1);
%
%   % Assume the radar is mounted on a platform located at [100 0 -10].
%   platTxRx = struct('PlatformID', 1, 'Position', [100 0 -10], ...
%     'Orientation', quaternion([0 0 0], 'eulerd', 'zyx', 'frame'));
%
%   % Create a target
%   platTgt = struct('PlatformID', 2, 'Position', [20e3 0 -500], ...
%       'Orientation', quaternion([45 0 0], 'eulerd', 'zyx', 'frame'), ...
%       'Signatures', {rcsSignature});
%
%   % Emit the signal. The emitted signal is in scenario frame.
%   simulationTime = 0;
%   emTx = step(emitter, platTxRx, simulationTime);
%
%   % Reflect the emission off the target.
%   emProp = radarChannel(emTx, platTgt)
%
%   % Convert the emission back to the body frame of the radar.
%   emRx = emissionsInBody(emProp, platTxRx)
%   disp(emRx(1))
% 
%   See also: radarEmission, radarEmitter, radarChannel.

%#codegen
%   Copyright 2018-2020 The MathWorks, Inc.

% Validate inputs
narginchk(2,2);

% Parse inputs
emScene = parseEmissions(emSceneIn);
[refPos, refVel, refOrient] = parseBodyFrame(bodyFrame);

% Output follows format of input
emBody = emScene;

% Are there any emissions to transform?
numEM = numel(emScene);
if numEM<1
    return % no
end

% Transform the signals to the platform's body frame
usesQuat = false(numEM,1);
orientRot = repmat(refOrient,1,1,numEM);
for iEM = 1:numEM
    thisEM = fusion.internal.remotesensors.CommonUtilities.getIndexed(emScene,iEM);
    
    emPos = thisEM.OriginPosition(:);
    emVel = thisEM.OriginVelocity(:);
    hasFaces = isa(thisEM,'radarEmission') || (isstruct(thisEM) && isfield(thisEM,'IlluminatedSurface'));
    if hasFaces
        faces = thisEM.IlluminatedSurface;
    end
    [emOrient, usesQuat(iEM)] = fusion.internal.remotesensors.CommonUtilities.getRotmat(thisEM.Orientation);
    
    % Rotate into receiving platform's body frame
    emPos = refOrient*(emPos-refPos);
    emVel = refOrient*(emVel-refVel);
    if hasFaces
        faces = bsxfun(@minus,faces,refPos(:));
        faces(:,:) = refOrient*faces(:,:);
    end
    orientRot(:,:,iEM) = emOrient*refOrient';
    
    thisEM.OriginPosition(:) = emPos;
    thisEM.OriginVelocity(:) = emVel;
    if hasFaces
        thisEM.IlluminatedSurface(:) = faces;
    end
    
    emBody = fusion.internal.remotesensors.CommonUtilities.setIndexed(emBody,iEM,thisEM);
end

% Set orientations
emBody = setQuats(emBody, usesQuat, orientRot);
emBody = setRotmats(emBody, ~usesQuat, orientRot);
end

function emBody = setRotmats(emBody, usesRotmat, orientRot)
% Set orientation rotation matrices
iFnd = find(usesRotmat);
for m = 1:numel(iFnd)
    iEM = iFnd(m);
    
    thisEM = fusion.internal.remotesensors.CommonUtilities.getIndexed(emBody,iEM);
    thisOrient = orientRot(:,:,iEM);
    
    if coder.target('MATLAB')
        thisEM.Orientation = thisOrient;
        emBody = fusion.internal.remotesensors.CommonUtilities.setIndexed(emBody,iEM,thisEM);
    else
        if ~isa(thisEM.Orientation, 'quaternion')
            thisEM.Orientation = thisOrient;
            emBody = fusion.internal.remotesensors.CommonUtilities.setIndexed(emBody,iEM,thisEM);
        end
    end
end
end

function emBody = setQuats(emBody, usesQuat, orientRot)
% Set orientation quaternions
iFnd = find(usesQuat);
if ~isempty(iFnd)
    quatOut = quaternion(orientRot(:,:,iFnd),'rotmat','frame');
    for m = 1:numel(iFnd)
        iEM = iFnd(m);

        thisEM = fusion.internal.remotesensors.CommonUtilities.getIndexed(emBody,iEM);
        thisOrient = quatOut(m);
        
        if coder.target('MATLAB')
            thisEM.Orientation = thisOrient;
            emBody = fusion.internal.remotesensors.CommonUtilities.setIndexed(emBody,iEM,thisEM);
        else
            if isa(thisEM.Orientation, 'quaternion')
                thisEM.Orientation = thisOrient;
                emBody = fusion.internal.remotesensors.CommonUtilities.setIndexed(emBody,iEM,thisEM);
            end
        end
    end
end
end

function em = parseEmissions(emIn)
% Returns validated and parsed emissions input

if isnumeric(emIn) && isempty(emIn)
    % Allow [] to be passed in for empty arrays of emissions
    em = emIn;
else
    validEmissionTypes = supportedEmissionTypes();
    
    if isstruct(emIn)
        % Allow users to pass in structs
        structType = fusion.internal.interfaces.DataStructures.structType(emIn);
        cond = ~(strcmp(structType, 'radarEmissionStruct') || strcmp(structType, 'sonarEmissionStruct'));
        if cond
            coder.internal.errorIf(cond,'shared_radarfusion:RemoteSensors:expectStructTypeCG','''radarEmission, sonarEmission''','EMSCENE');
        end
        
        em = fusion.internal.interfaces.DataStructures.assembleStructs(emIn);
    else
        validateattributes(emIn, {validEmissionTypes{:} 'cell'}, {}, mfilename, 'EMSCENE');
        
        cond = iscell(emIn) && ~fusion.internal.remotesensors.CommonUtilities.allValidTypes(emIn, validEmissionTypes);
        coder.internal.errorIf(cond,'shared_radarfusion:RemoteSensors:IncorrectInputClass','EMSCENE',strjoin(validEmissionTypes, ', '));
        
        em = emIn;
    end
end
end

function [refPos, refVel, refRot] = parseBodyFrame(bodyFrame)
% Returns validated and parsed bodyFrame input

validBodyFrameTypes = supportedBodyFrameTypes();

cond = ~fusion.internal.remotesensors.CommonUtilities.allValidTypes(bodyFrame, validBodyFrameTypes);
coder.internal.errorIf(cond,'shared_radarfusion:RemoteSensors:IncorrectBodyFrame','BODYFRAME',class(bodyFrame));

validateattributes(bodyFrame, validBodyFrameTypes, {'scalar'}, mfilename, 'BODYFRAME');

if isstruct(bodyFrame)
    [refPos, refVel, refRot] = parseFrameStruct(bodyFrame);
else
    plat = truePose(bodyFrame);
    refPos = plat.Position(:);
    refVel = plat.Velocity(:);
    refRot = fusion.internal.remotesensors.CommonUtilities.getRotmat(plat.Orientation);
end
end

function [refPos, refVel, refRot] = parseFrameStruct(bodyFrame)
% Returns the parsed states defining the body frame. When fields are
% unspecified, they are populated with their default values.

% Validate fields

% If struct has fields defined, but none of them are defining the body's
% frame, then through an error. This is probably not what the user
% intended. They are probably passing the wrong struct by accident.
flds = fieldnames(bodyFrame);
cond = ~isempty(flds) && ...
    ~(isfield(bodyFrame, 'Position') || isfield(bodyFrame, 'Velocity') || isfield(bodyFrame, 'Orientation'));
coder.internal.errorIf(cond,'shared_radarfusion:RemoteSensors:IncorrectBodyFrameStruct','Position, Velocity, Orientation','BODYFRAME',strjoin(flds,', '));

if isfield(bodyFrame, 'Position')
    val = bodyFrame.Position;
    validateattributes(val, {'double','single'}, ...
        {'real','finite','vector','numel',3}, ...
        '', 'BODYFRAME.Position');
end

if isfield(bodyFrame, 'Velocity')
    val = bodyFrame.Velocity;
    validateattributes(val, {'double','single'}, ...
        {'real','finite','vector','numel',3}, ...
        '', 'BODYFRAME.Velocity');
end

if isfield(bodyFrame, 'Orientation')
    val = bodyFrame.Orientation;
    validateattributes(val, {'quaternion','double','single'}, ...
        {}, ...
        '', 'BODYFRAME.Orientation');
    if ~isa(val, 'quaternion')
        validateattributes(val, {'double','single'}, ...
            {'real', 'finite', '2d', 'size', [3 3]}, ...
            '', 'BODYFRAME.Orientation');
    end
end

% Figure out what class to use for unspecified fields.
classToUse = 'double';
if isfield(bodyFrame, 'Position')
    val = bodyFrame.Position;
    classToUse = class(val);
elseif isfield(bodyFrame, 'Velocity')
    val = bodyFrame.Velocity;
    classToUse = class(val);
elseif isfield(bodyFrame, 'Orientation')
    val = fusion.internal.remotesensors.CommonUtilities.getRotmat(bodyFrame.Orientation);
    classToUse = class(val);
end

% Extract the frame's state. Assign default values where unspecified
if isfield(bodyFrame, 'Position')
    refPos = bodyFrame.Position(:);
else
    refPos = zeros(3,1,classToUse);
end

if isfield(bodyFrame, 'Velocity')
    refVel = bodyFrame.Velocity(:);
else
    refVel = zeros(3,1,classToUse);
end

if isfield(bodyFrame, 'Orientation')
    % Convert reference orientation to a rotation matrix
    refRot = fusion.internal.remotesensors.CommonUtilities.getRotmat(bodyFrame.Orientation);
else
    refRot = eye(3,3,classToUse);
end
end

function types = supportedEmissionTypes()
% Returns the emission types supported for conversion
types = {'radarEmission', 'sonarEmission'};
end

function types = supportedBodyFrameTypes()
% Returns the body frame types supported for conversion
types = {'struct', 'radarfusion.internal.scenario.Platform'};
end
