openvis.m

%% File to open visualization for Cosmos Beta in MATLAB Simulink

%% Constants
% Earth rotation velocity around Z-axis.
EARTH_ROT = (2*pi/86164); % [rad/s]

%% Set paths
% The parameters below normally should never change.
warning on verbose;

% Inform the name of this file without the extension "m".
THIS_FILE_NAME = 'openvis';

% Inform the name of the project file for the Cosmos simulation.
PROJECT_FILE_NAME = 'cosmosVisualization'; % Without extension 'prj'.

if(~isdeployed)
  % Get directory path of the active file in MATLAB's Editor.
  [pathActiveFile,~,~] = fileparts(matlab.desktop.editor.getActiveFilename);
  addpath(genpath(pathActiveFile)); % Append path to the current MATLAB path.
  
  % Get path for directory of the file name set in THIS_FILE_NAME.
  [pathCosmos,~,~] = fileparts(which(THIS_FILE_NAME));
  addpath(genpath(pathCosmos)); % Append path to the current MATLAB path.
  
  % Add path to ancillary folders.
  addpath(strcat(pathCosmos,filesep,'config'));
  addpath(strcat(pathCosmos,filesep,'utils'));
  
  % Get path for directory of the simulation project.
  [pathVisualization,~,~] = fileparts(which([PROJECT_FILE_NAME,'.prj']));
  
  % Change working directory to the directory of this m-file.
  cd(pathCosmos);
end

% Check if there is any Matlab project already open.
if(isempty(matlab.project.rootProject))
  % If there is no project open, launch project from path.
  proj = openProject(pathVisualization);
  fprintf('\nTo prevent issues, check if project path is correct:\n');
  fprintf('%s\n\n',proj.RootFolder);
else
  % If there is a project already open, get the Project object.
  proj = currentProject();
  % Check if the name of the project is correct.
  if(~strcmp(proj.Name,PROJECT_FILE_NAME))
    % If name is different, show error.
    error(['\nProject ''%s'' is already open. ',...
      'Close it before running ''%s''.'], ...
      proj.Name, PROJECT_FILE_NAME);
  else
    % If name is correct, show message.
    fprintf(2,'\nAttention: Project with name ''%s'' already open.\n',proj.Name);
    fprintf('To prevent issues, check if project path is correct:\n');
    fprintf('%s\n\n',proj.RootFolder);
  end
end

% Set timer start time.
timeScriptStart = posixtime(datetime('now')); % Posixtime [seconds].


%% Read parameters from JSON files
vis = readjson('configVisualization.json');

% Set parameter to automatically run Simulink visualization.
AUTORUN = vis.AutoRun; % [true: 1 | false: 0]

% Set path for coordinates folder.
% IMPORTANT: The FIRST file in the coordinates folder MUST BE the reference for
% the other satellites.
COORD_FOLDER = strcat(vis.ParentCoordFolder,filesep,vis.CoordFolder);
COORD_FOLDER = fullfile(pathCosmos,COORD_FOLDER);

% Set struct for selected camera mode.
CAM_MODE = struct('time',0,'signals',struct('dimensions',0,'values',0));

% Set parameter to automatically smooth changes in satellite orientations.
SMOOTH_ENABLE = vis.SmoothSatOrientationChanges; % [true: 1 | false: 0]

% If smoothing is enabled, set num of data points for computing smoothed value.
SMOOTH_SPAN = vis.SmoothingDataSpan;
SMOOTH_METHOD_LIST = vis.SmoothingMethodList;
SMOOTH_METHOD = SMOOTH_METHOD_LIST{vis.SmoothingMethodChosen};

% Option to override lat-long-radius values for testing purposes.
OVERRIDE_LLR = vis.OverrideLLR;

% Option to override roll-pitch-yaw values for testing purposes.
OVERRIDE_RPY = vis.OverrideRPY;


%% Prepare coordinate files
disp('Satellite coordinate files:');
disp(strcat(pathCosmos,filesep));
fprintf('  %s\n',strcat(vis.ParentCoordFolder,filesep,vis.CoordFolder));
% Read names of coordinate files.
coordfiles = {dir(strcat(COORD_FOLDER,filesep,'*.csv')).name};
% Rearrange files so that the local coordinate is the last one in the list.
% This step is necessary for correct visualization in the 3D simulink world.
coordfiles = circshift(coordfiles,-1);
disp(coordfiles');

% Clear satellite struct.
clear simsat;

% Define satellite struct.
simsat(1).time  = -1;

simsat(1).x      = struct('time',0,'signals',struct('dimensions',0,'values',0));
simsat(1).y      = struct('time',0,'signals',struct('dimensions',0,'values',0));
simsat(1).z      = struct('time',0,'signals',struct('dimensions',0,'values',0));
simsat(1).xyz    = struct('time',0,'signals',struct('dimensions',0,'values',0));

simsat(1).lat    = struct('time',0,'signals',struct('dimensions',0,'values',0));
simsat(1).long   = struct('time',0,'signals',struct('dimensions',0,'values',0));
simsat(1).sma    = struct('time',0,'signals',struct('dimensions',0,'values',0));

simsat(1).pos    = struct('time',0,'signals',struct('dimensions',0,'values',0));
simsat(1).posU1  = struct('time',0,'signals',struct('dimensions',0,'values',0));
simsat(1).posU2  = struct('time',0,'signals',struct('dimensions',0,'values',0));

simsat(1).pitch  = struct('time',0,'signals',struct('dimensions',0,'values',0));
simsat(1).yaw    = struct('time',0,'signals',struct('dimensions',0,'values',0));
simsat(1).roll   = struct('time',0,'signals',struct('dimensions',0,'values',0));

simsat(1).rot    = struct('time',0,'signals',struct('dimensions',0,'values',0));
simsat(1).inc    = -1;
simsat(1).lanDeg = -1;
simsat(1).lanRad = -1;

% Allocate memory for array of satellites.
numsats = length(coordfiles);
simsat = repmat(simsat(1),numsats,1);


%% Set parallel pool
createparpool(numsats);

% Create data queue for parallel pool.
dq = parallel.pool.DataQueue;

% Define function to call when new data is received on the DataQueue.
afterEach(dq, @disp);


%% Read data from coordinate files
fprintf('%s','Reading data from coordinate files ... ');
% Set timer start time.
timeReadStart = posixtime(datetime('now')); % Posixtime [seconds].
for n = 1:numsats
  coord = readmatrix(strcat(COORD_FOLDER,filesep,coordfiles{n}));
  timestamps = coord(:,1); % [seconds]
  x = coord(:,2); % [m]
  y = coord(:,3); % [m]
  z = coord(:,4); % [m]
  pitch = coord(:,6); % [degrees]
  yaw   = coord(:,7); % [degrees]
  roll  = coord(:,5); % [degrees]
  inclinationDeg = coord(1,8); % [degrees]
  orbitLANDeg = coord(1,9); % [degrees]
  
  % Get length of data samples.
  dataLength = length(timestamps);
  % Set column array of zeros with length of data samples.
  zeroArray = zeros(dataLength,1); % [TIMEx1 vector]
  % Set column array of ones with length of data samples.
  onesArray = ones(dataLength,1); % [TIMEx1 vector]
  
  % Update camera mode struct.
  CAM_MODE.time = timestamps;
  CAM_MODE.signals.dimensions = 1;
  CAM_MODE.signals.values = vis.SelectedCameraMode*onesArray; % [TIMEx1 vector]
  
  if(OVERRIDE_LLR)
    % Values to override for each of the satellites.
    smaKm = onesArray * 6871;
    if n == 1
      latDeg  = onesArray * -20;
      longDeg = onesArray * 0;
    elseif n == 2
      latDeg  = onesArray * 0;
      longDeg = onesArray * 0;
    elseif n == 3
      latDeg  = onesArray * 20;
      longDeg = onesArray * 0;
    else
      % Reference (n == 4).
      latDeg  = onesArray * 0;
      longDeg = onesArray * 0;
    end
  end
  
  if(OVERRIDE_RPY)
    % Values to override for each of the satellites.
    if n == 1
      roll  = onesArray * 45;
      pitch = onesArray * 45;
      yaw   = onesArray * 45;
    elseif n == 2
      roll  = onesArray * 0;
      pitch = onesArray * 45;
      yaw   = onesArray * 0;
    elseif n == 3
      roll  = onesArray * 0;
      pitch = onesArray * 0;
      yaw   = onesArray * 45;
    else
      % Reference (n == 4).
    end
  end
  
  if(SMOOTH_ENABLE)
    pitch = smooth(pitch,SMOOTH_SPAN,SMOOTH_METHOD);
    yaw   = smooth(yaw  ,SMOOTH_SPAN,SMOOTH_METHOD);
    roll  = smooth(roll ,SMOOTH_SPAN,SMOOTH_METHOD);
  end
  
  % Convert data to SI units.
  pitch = pitch * (pi/180); % [rad]
  yaw   = yaw   * (pi/180); % [rad]
  roll  = roll  * (pi/180); % [rad]
  inclinationRad = inclinationDeg * (pi/180); % [rad]
  orbitLANRad = orbitLANDeg * (pi/180); % [rad]
  
  % Place data into satellite struct.
  simsat(n).x.time = timestamps;
  simsat(n).x.signals.dimensions = 1;
  simsat(n).x.signals.values = x; % [m] [TIMEx1 vector]
  
  simsat(n).y.time = timestamps;
  simsat(n).y.signals.dimensions = 1;
  simsat(n).y.signals.values = y; % [m] [TIMEx1 vector]
  
  simsat(n).z.time = timestamps;
  simsat(n).z.signals.dimensions = 1;
  simsat(n).z.signals.values = z; % [m] [TIMEx1 vector]
  
  simsat(n).xyz.time = timestamps;
  simsat(n).xyz.signals.dimensions = 3;
  simsat(n).xyz.signals.values = [x y z]; % [TIMEx3 vector]
  
  simsat(n).pos.time = timestamps;
  simsat(n).pos.signals.dimensions = 3;
  simsat(n).pos.signals.values = [zeroArray zeroArray zeroArray];
  
  simsat(n).posU1.time = timestamps;
  simsat(n).posU1.signals.dimensions = 3;
  simsat(n).posU1.signals.values = [zeroArray zeroArray zeroArray];
  
  simsat(n).posU2.time = timestamps;
  simsat(n).posU2.signals.dimensions = 3;
  simsat(n).posU2.signals.values = [zeroArray zeroArray zeroArray];
  
  simsat(n).rot.time = timestamps;
  simsat(n).rot.signals.dimensions = 4;
  simsat(n).rot.signals.values = zeros(dataLength,4); % [TIMEx4 vector]
  
  simsat(n).pitch.time = timestamps;
  simsat(n).pitch.signals.dimensions = 1;
  simsat(n).pitch.signals.values = pitch; % [rad]
  
  simsat(n).yaw.time = timestamps;
  simsat(n).yaw.signals.dimensions = 1;
  simsat(n).yaw.signals.values = yaw; % [rad]
  
  simsat(n).roll.time = timestamps;
  simsat(n).roll.signals.dimensions = 1;
  simsat(n).roll.signals.values = roll; % [rad]
  
  simsat(n).inc = inclinationRad;
  simsat(n).lanDeg = orbitLANDeg;
  simsat(n).lanRad = orbitLANRad;
  simsat(n).time = timestamps; % [seconds]
end
fprintf('%s','done. ');
% Set timer stop time.
timeReadStop = posixtime(datetime('now')); % Posixtime [seconds].
timeReadDuration = timeReadStop - timeReadStart;
fprintf('(%.2f seconds)\n',timeReadDuration);


%% Make transformations for 3D visualization
% Get last timestamp of the time vector, set it as simulation time.
stopTime = simsat(1).time(end);

% Get data length.
dataLength = length(simsat(1).time);
fprintf('%s%d.\n','Data length of each coordinate file: ',dataLength);

parsat = simsat;

% Get each time-coordinate-orientation triple for each satellite.
fprintf('%s\n','Processing data from coordinate files ... ');
spmd(numsats)
  
  n = labindex;
  
  % Set timer start time.
  timeProcStart = posixtime(datetime('now')); % Posixtime [seconds].
  msg = sprintf('%3.d/%d ... processing ...',n,numsats);
  send(dq, msg);
  for i = 1:dataLength
    
    % Get time, in seconds, since beginning of simulation.
    time = parsat(n).time(i);
    
    % Get coordinate values for the current time.
    x = parsat(n).x.signals.values(i);
    y = parsat(n).y.signals.values(i);
    z = parsat(n).z.signals.values(i);
    
    parsat(n).pos.signals.values(i,1) = x;
    parsat(n).pos.signals.values(i,2) = y;
    parsat(n).pos.signals.values(i,3) = z;
    
    % Get roll, pitch, yaw angles.
    rollAngle  = parsat(n).roll.signals.values(i);  % [rad]
    pitchAngle = parsat(n).pitch.signals.values(i); % [rad]
    yawAngle   = parsat(n).yaw.signals.values(i);   % [rad]
    
    % Compute unit vector from x, y, z.
    xyzUnit = [x;y;z] / norm([x;y;z]); % [3x1 unit vector]
    
    
    %% Correction of initial orientation
    % Rotate to set initial attitude:
    %   - payload pointing to Nadir, and
    %   - wings perpendicular to Equator line.
    % To correct orientation of the 3D satellite to initial attitude,
    % Rotate -90deg (-pi/2) on ECEF's Y-axis.
    a = -pi/2;
    rot = [cos(a/2), 0, sin(a/2), 0]; % [quaternion]
    
    
    %% Initial body axes
    % Define local satellite axes according to ECEF's XYZ axes.
    localYaw   = [1 0 0]'; % [3x1 U-vector]
    localRoll  = [0 1 0]'; % [3x1 U-vector]
    localPitch = [0 0 1]'; % [3x1 U-vector]
    
    
    %% Satellite rotation for orbital inclination
    % Positive rotation on ECEF's X-axis.
    a = parsat(n).inc; % [rad]
    rotInc = [cos(a/2), sin(a/2), 0, 0]; % [quaternion]
    rot = quatmultiply(rotInc, rot);
    % Correct local body axes.
    adeg = a * 180/pi; % [deg]
    rotInc = rotx(adeg);
    localRoll  = rotInc*localRoll;  % [3x1 U-vector]
    localPitch = rotInc*localPitch; % [3x1 U-vector]
    localYaw   = rotInc*localYaw;   % [3x1 U-vector]
    
    
    %% Total Earth rotation since beginning of simulation
    aEarthRad = EARTH_ROT * time; % [rad]
    aEarthDeg = aEarthRad * 180/pi; % [deg]
    % Correct for orbit longitude of ascending node (LAN).
    aEarthRad = aEarthRad - parsat(n).lanRad; % [rad]
    aEarthDeg = aEarthDeg - parsat(n).lanDeg; % [deg]
    
    
    %% Satellite correction for Earth rotation
    % Negative rotation on ECEF's Z-axis.
    a = -aEarthRad; % [rad]
    rotEarthFix = [cos(a/2), 0, 0, sin(a/2)]; % [quaternion]
    rot = quatmultiply(rotEarthFix, rot);
    % Correct local body axes.
    localRoll  = rotz(-aEarthDeg)*localRoll;  % [3x1 U-vector]
    localPitch = rotz(-aEarthDeg)*localPitch; % [3x1 U-vector]
    localYaw   = rotz(-aEarthDeg)*localYaw;   % [3x1 U-vector]
    
    
    %% Normal vector of the orbital plane
    % Set unit vector pointing towards ECEF's +Z-axis.
    %   unitVector = [0; 0; 1];
    % Rotate unit vector on ECEF's X-axis, with angle equal to the orbital
    % inclination. Rotation angle in degrees:
    angX = parsat(n).inc * 180/pi; % [deg]
    % Apply rotation on unit vector. The resulting vector represents the normal
    % vector of the orbital plane:
    normalOrbitVector = rotx(angX)*[0;0;1]; % [3x1 U-vector]
    % Consider retrograde motion of the orbit.
    % Rotate normal orbit vector around Z-axis.
    normalOrbitVector = rotz(-aEarthDeg)*normalOrbitVector; % [3x1 U-vector]
    
    
    %% Angle from Equator line to current orbital position
    % Equator point considering orbit retrograde motion.
    v1 = rotz(-aEarthDeg)*[1;0;0]; % [3x1 U-vector]
    % Current orbit position.
    v2 = xyzUnit; % [3x1 U-vector]
    % Normal vector of plane between Equator point and current position.
    normalVector = normalOrbitVector; % [3x1 U-vector]
    % From function a = vecangle360(v1,v2,n).
    x = cross(v1,v2);
    c = sign(dot(x,normalVector)) * norm(x);
    angPos = atan2(c,dot(v1,v2)); % [rad]
    
    
    %% Pitch correction for orbital position
    % Rotate with an angle equal to the angle of the current orbital position.
    % Use corrected pitch axis as axis for pitch rotation.
    rodriguesRotMatrix = rotrodrigues(angPos,localPitch);
    % Convert Rodrigues rotation matrix to quaternions.
    pitchCorrection = rotm2quat(rodriguesRotMatrix);
    rot = quatmultiply(pitchCorrection, rot);
    % Correct local body axes.
    localRoll  = rodriguesRotMatrix*localRoll;  % [3x1 U-vector]
    localPitch = rodriguesRotMatrix*localPitch; % [3x1 U-vector]
    localYaw   = rodriguesRotMatrix*localYaw;   % [3x1 U-vector]
    
    
    %% Roll
    rodriguesRotMatrix = rotrodrigues(rollAngle,localYaw);
    rodriguesQuat = rotm2quat(rodriguesRotMatrix);
    rot = quatmultiply(rodriguesQuat, rot);
    
    
    %% Pitch
    rodriguesRotMatrix = rotrodrigues(pitchAngle,localPitch);
    rodriguesQuat = rotm2quat(rodriguesRotMatrix);
    rot = quatmultiply(rodriguesQuat, rot);
    
    
    %% Yaw
    rodriguesRotMatrix = rotrodrigues(yawAngle,localYaw);
    rodriguesQuat = rotm2quat(rodriguesRotMatrix);
    rot = quatmultiply(rodriguesQuat, rot);
    
    
    %% Total rotation
    % Multiplied all rotation quaternions to get final rotation.
    parsat(n).rot.signals.values(i,:) = rot;
    
  end
  % Set timer stop time.
  timeProcStop = posixtime(datetime('now')); % Posixtime [seconds].
  timeProcDuration = timeProcStop - timeProcStart;
  msg = sprintf('%3.d/%d ... done. (%.2f seconds)',n,numsats,timeProcDuration);
  send(dq, msg);
  
  % Globally concatenate all output variables on lab index 1.
  % Must be the last lines of code of the parallel pool.
  % satellites = gcat(parsat,1,1);
end % Parallel code.

for i = 1:numsats
  allsats = parsat{i};
  simsat(i) = allsats(i);
end


%% Simulink
% Model names.
fprintf('\n%s','Preparing Simulink models ... ');
% Set timer start time.
timeProcStart = posixtime(datetime('now')); % Posixtime [seconds].
modelMain = 'asbCubeSat';
model3DWorld = 'asbCubeSat/Visualization/Virtual Reality World/';

% Load and open main Simulink model.
open_system(modelMain);


%% Delete all code-generated lines and blocks
blockPaths = find_system(model3DWorld,'Type','Block');

% Find '/_Sat' in blockPaths.
blocksToDelete = find(contains(blockPaths,'/_Sat'));

% Delete lines and blocks.
for n = 1:length(blocksToDelete)
  % Check if block is a main- or sub-block in the Simulink model.
  % First get block name.
  blockName = blockPaths{blocksToDelete(n)};
  % Then check number of '/' occurrences.
  pathSeparators = count(blockName,'/');
  if pathSeparators == 3 % it is a main-block.
    % Delete line(s).
    h = get_param(blockName,'LineHandles');
    for p = 1:length(h.Outport) % for each output port
      if h.Outport(p) > 0 % if line exists
        delete_line(h.Outport(p)); % delete line
      end
    end
    % Delete block.
    delete_block(blockName);
  end
end


%% Update X3D File and VR Block
% Name must match the name of the block in Simulink.
b3DWorld = 'VR Sink1/';

% Open x3D file for writing and discard all contents ('w' option).
file3DWorld = fopen([pathVisualization,filesep,'simulation',filesep,...
  'cosmosSimulation.x3d'],'w');

% Write initial structure before satellites section.
% Open file with initial structure.
file3DWorld_part1 = fopen([pathVisualization,filesep,'simulation',filesep,...
  'cosmosSimulation_struct1.x3d'],'r');
while ~feof(file3DWorld_part1)
  sline = fgetl(file3DWorld_part1);
  fprintf(file3DWorld,'%s\n',sline);
end
% Close file.
fclose(file3DWorld_part1);

% Write N satellite structures.
for n=1:(numsats-1)
  % Define satellite structure.
  structSat = {
    ['  <Transform DEF="Satellite',num2str(n),'"  scale="0.05 0.05 0.05" rotation="0.50838 0.84409 -0.08794 2">']
    ['    <Shape DEF="SatelliteBus',num2str(n),'" >']
    '      <Appearance>'
    '        <ImageTexture url=''"texture/cubesat.jpg" ''>'
    '        </ImageTexture>'
    '        <Material specularColor="1 1 1" shininess="1" emissiveColor="0.2 0.2 0.2" diffuseColor="0.2 0.2 0.2" ambientIntensity="0.6">'
    '        </Material>'
    '      </Appearance>'
    '      <Box size="0.3 0.3 0.3">'
    '      </Box>'
    '    </Shape>'
    ['    <Transform DEF="SolarPanelTwo',num2str(n),'"  translation="0 0.16 -0.3">']
    '      <Shape>'
    '        <Appearance>'
    '          <ImageTexture url=''"texture/solarpanel.jpg" ''>'
    '          </ImageTexture>'
    '          <Material shininess="1" diffuseColor="0.9 0.76733 0.61963" ambientIntensity="0">'
    '          </Material>'
    '        </Appearance>'
    '        <Box size="0.3 0.02 0.3">'
    '        </Box>'
    '      </Shape>'
    '    </Transform>'
    ['    <Transform DEF="SolarPanelOne',num2str(n),'"  translation="0 0.16 0.3">']
    '      <Shape>'
    '        <Appearance>'
    '          <ImageTexture url=''"texture/solarpanel.jpg" ''>'
    '          </ImageTexture>'
    '          <Material shininess="1" diffuseColor="0.9 0.76733 0.61963" ambientIntensity="0">'
    '          </Material>'
    '        </Appearance>'
    '        <Box size="0.3 0.02 0.3">'
    '        </Box>'
    '      </Shape>'
    '    </Transform>'
    ['    <Transform DEF="Antenna',num2str(n),'" >']
    ['      <Transform DEF="AntennaDish',num2str(n),'"  translation="0 -0.15 0">']
    '        <Shape>'
    '          <Appearance>'
    '            <Material specularColor="1 1 1" shininess="1" diffuseColor="0.9 0.65504 0.31917">'
    '            </Material>'
    '          </Appearance>'
    '          <Cone height="0.07" bottomRadius="0.06">'
    '          </Cone>'
    '        </Shape>'
    '      </Transform>'
    '    </Transform>'
    ['    <Transform DEF="FollowSat',num2str(n),'"  translation="-5.5 2 0" rotation="-0.0923 -0.7011 -0.0923 1.5878">']
    ['      <Viewpoint DEF="FollowSatView',num2str(n),'"  fieldOfView="0.7854" description="Lock Satellite-',num2str(n),'" position="0 0 0">']
    '      </Viewpoint>'
    '    </Transform>'
    '  </Transform>'
  };
  
  % Write structure to X3D file.
  fprintf(file3DWorld,'%s\n',structSat{:});
end

% Define final structure after satellites section.
structFinal = {
  '</Transform>'
  '</Scene>'
  '</X3D>'
};

% Write structure to X3D file.
fprintf(file3DWorld,'%s\n',structFinal{:});

% Close x3D file.
fclose(file3DWorld);

% Help!
% VR block input ports - To see structure of the input ports:
% get_param([model3DWorld,b3DWorld],'FieldsWritten')





%%TO DO
% Define length of variable holding the description of the input ports.





% Define input ports order.
b3DWorldInputPorts = [...
  'Earth.rotation.4.1.1.double',...
  '#Sun.translation.3.1.1.double',...
  '#Umbra.rotation.4.1.1.double',...
  '#Umbra.translation.3.1.1.double',...
  '#FollowReference.rotation.4.1.1.double',...
  '#FollowReference.translation.3.1.1.double',...
  '#SatelliteReference.rotation.4.1.1.double',...
  '#SatelliteReference.translation.3.1.1.double'];
for n=1:(numsats-1)
  b3DWorldInputPorts = [b3DWorldInputPorts,...
    '#Satellite',num2str(n),'.rotation.4.1.1.double',...
    '#Satellite',num2str(n),'.translation.3.1.1.double'];
end

% Update input ports to VR block.
set_param([model3DWorld,b3DWorld],'FieldsWritten',b3DWorldInputPorts);


%% Add Blocks and Lines
% Update input variables for reference satellite.
refSatVar = ['simsat(',num2str(numsats),')'];
set_param([model3DWorld,'Local_Ref_Rot'],'VariableName',[refSatVar,'.rot']);
set_param([model3DWorld,'Local_Ref_Pos'],'VariableName',[refSatVar,'.pos']);

% Add blocks and lines for each satellite in the formation.
for n=1:(numsats-1)
  % Help!
  % To check the dialog parameters of a block:
  % get_param(blockPath,'DialogParameters')
  bSatPos = ['_Sat',num2str(n),'_Pos'];
  add_block('simulink/Sources/From Workspace',[model3DWorld,bSatPos],...
    'VariableName',['simsat(',num2str(n),').pos']);
  
  bSatPosConv = ['_Sat',num2str(n),'_ECEFtoVRML'];
  add_block([model3DWorld,'ECEF to VRML'],[model3DWorld,bSatPosConv]);
  
  bSatRot = ['_Sat',num2str(n),'_Rot'];
  add_block('simulink/Sources/From Workspace',[model3DWorld,bSatRot],...
    'VariableName',['simsat(',num2str(n),').rot']);
  
  bSatRotConv = ['_Sat',num2str(n),'_QUATtoVRML'];
  add_block([model3DWorld,'Quaternion to VRML'],[model3DWorld,bSatRotConv]);
  
  % Help!
  % To get the port connectivity of the 3D World block:
  % p = get_param([model3DWorld,b3DWorld],'PortConnectivity')
  % for n=1:length(p), disp([num2str(n),' : ',num2str(p(n).Position)]), end
  
  % Compute 3D World's input port numbers for current satellite.
  pSatN = 9 + (n-1)*2;
  pRotation    = num2str(pSatN);
  pTranslation = num2str(pSatN + 1);
  
  
  
  
  
  
  
  %PUT scale directly into xml code generation for satellites
  satSizeInitial = [2e5 2e5 2e5]; % defined by Simulink model.
  satSizeConverted = satSizeInitial .* (1/.3e6); % Model to VRML conversion.
  satUserScale = 0.1; % defined by user (configurable).
  satSizeFinal = satSizeConverted .* satUserScale; % [1x3 VRML]
  % 0.066666666666667 -> 0.067
    
  
  
  
  
  
  
  
  % Add lines to connect blocks.
  add_line(model3DWorld,[bSatPos,'/1'],[bSatPosConv,'/1']);
  add_line(model3DWorld,[bSatPosConv,'/1'],[b3DWorld,pTranslation]);
  
  add_line(model3DWorld,[bSatRot,'/1'],[bSatRotConv,'/1']);
  add_line(model3DWorld,[bSatRotConv,'/1'],[b3DWorld,pRotation]);
end

% Auto-arrange model3DWorld.
% Not necessary yet.

% Save model.
save_system(modelMain);
% Set timer stop time.
timeProcStop = posixtime(datetime('now')); % Posixtime [seconds].
timeProcDuration = timeProcStop - timeProcStart;
fprintf('done. (%.2f seconds)\n',timeProcDuration);


%% Check and Run
% Help!
% To see all object handles open in MATLAB, enter in Command Window:
% object_handles = findall(groot)
% To find handle of the figure for 3D visualization, enter in Command Window:
% findall(groot,'Name','COSMOS Visualization')

% Create a virtual world associated with the virtual world 3D file filename and
% return its handle. If the VR world already exists, a handle to the existing
% virtual world is returned. Specify the file name as a string.
vrWorldObj = vrworld('cosmosSimulation.x3d');
reload(vrWorldObj);

% Get handle of the current virtual reality figure. The current virtual reality
% figure is the currently active virtual reality figure window in which you can
% get and set the viewer properties. If no virtual reality figure exists, the
% MATLAB software returns an empty vrfigure object.
% vrFigHandle = vrgcf;

% Try to get handle of the figure window for 3D visualization.
vrFigHandle = findall(groot,'Name','COSMOS Visualization');
if isempty(vrFigHandle)
  % If handle is empty, means that the virtual world figure is closed.
  % Then, open VR figure.
  vrFigHandle = vrfigure(vrWorldObj);
  % cosmosVisHandle = findall(groot,'Name','COSMOS Visualization');
  % Find VRSimMenu in cosmosVisHandle.Children(5)
  % Find submenus in cosmosVisHandle.Children(5).Children(1) to (3)
else
  % If handle is found, bring figure to front.
  figure(vrFigHandle);
end

% Set timer stop time.
timeScriptStop = posixtime(datetime('now')); % Posixtime [seconds].
timeScriptDuration = timeScriptStop - timeScriptStart;
fprintf('\nTotal visualization processing time: %.2f seconds\n',timeScriptDuration);

fprintf('%s','Ready to play visualization: ')
if AUTORUN
  fprintf('%s\n','Autoplay [ON]')
  % Start Simulink visualization.
  sim(modelMain, stopTime);
else
  fprintf('%s\n','Autoplay [OFF]')
  % Confirmation here:
  msgfig = msgbox('Visualization Processing Completed','MATLAB Info','help','modal');
  uiwait(msgfig);
end

% Clear composites.
% Needed to close active spmd.
% See: https://nl.mathworks.com/matlabcentral/answers/223939-how-to-specify-the-number-of-labs-in-smpd
clear a adeg aEarthDeg aEarthRad angPos angX c I lat localPitch localRoll localYaw long msg ...
  normalOrbitVector normalVector pitchAngle pitchCorrection rodriguesQuat ...
  rodriguesRotMatrix rollAngle rot rotEarthFix rotInc ...
  sma time timeProcDuration timeProcStart timeProcStop u v1 v2 W x xyzUnit ...
  y yawAngle z satellites parsat;