miSim/test/test_miSim.m

classdef test_miSim < matlab.unittest.TestCase
    properties (Access = private)
        % System under test
        testClass = miSim;

        % Debug
        makeVideo = true; % disable video writing for big performance increase
        makePlots = true; % disable plotting for big performance increase (also disables video)
        plotCommsGeometry = false; % disable plotting communications geometries

        % Sim
        maxIter = 250;
        timestep = 0.1;

        % Domain
        domain = rectangularPrism; % domain geometry
        minDimension = 10;
        minAlt = 1; % minimum allowed agent altitude

        % Obstacles
        minNumObstacles = 1; % Minimum number of obstacles to be randomly generated
        maxNumObstacles = 3; % Maximum number of obstacles to be randomly generated
        minObstacleSize = 1; % Minimum size of a randomly generated obstacle
        maxObstacleSize = 6; % Maximum size of a randomly generated obstacle
        obstacles = cell(1, 0);
        
        % Objective
        discretizationStep = 0.01; % Step at which the objective function is solved in X and Y space
        protectedRange = 1; % Minimum distance between the sensing objective and the edge of the domain
        objective = sensingObjective;

        % Agents
        initialStepSize = 0.2; % gradient ascent step size at the first iteration. Decreases linearly to 0 based on maxIter.
        initialMaxAngleStepSize = 0.1; % angular step size (degrees) for tilt/azimuth gradient ascent per timestep.
        minAgents = 7; % Minimum number of agents to be randomly generated
        maxAgents = 7; % Maximum number of agents to be randomly generated
        useDoubleIntegrator = false;
        dampingCoeff = 2;
        agents = cell(0, 1);
        
        % Collision
        minCollisionRange = 0.1; % Minimum randomly generated collision geometry size
        maxCollisionRange = 0.5; % Maximum randomly generated collision geometry size
        collisionRanges = NaN;

        % Sensing
        sensor = sigmoidSensor;
        % sigmoidSensor
        betaDistMin = 3;
        betaDistMax = 15;
        betaTiltMin = 3;
        betaTiltMax = 15;
        alphaDistMin = 2.5;
        alphaDistMax = 3;
        alphaTiltMin = 15; % degrees
        alphaTiltMax = 30; % degrees
        opticalPartitioningMin = 1e-6;
        % rfSensor
        P_TX = 1e-3; % Transmit power (Watts)
        T_0 = 300; % Temperature (K)
        BW = 20e6; % Bandwidth (Hz)
        f_c = 3e9; % Center frequency (Hz)
        G_RX_dBi = 3; % Receiving Antenna Gain (dBi)
        beamwidthExponent = 16;
        lossExponent = 2;
        sinrPartitioningMin = 50;

        % Communications
        useFixedTopology = false;
        optimizeSensorPointing = false;
        minCommsRange = 3; % Minimum randomly generated collision geometry size
        maxCommsRange = 5; % Maximum randomly generated collision geometry size
        commsRanges = NaN;

        % Constraints
        barrierGain = 100;
        barrierExponent = 3;
    end

    % Setup for each test
    methods (TestMethodSetup)
        % Generate a random domain
        function tc = setDomain(tc)
            % random integer-dimensioned cubic domain
            tc.domain = tc.domain.initializeRandom(REGION_TYPE.DOMAIN, "Domain", tc.minDimension);
            % Random bivariate normal PDF objective
            tc.domain.objective = tc.domain.objective.initializeRandomMvnpdf(tc.domain, tc.discretizationStep, tc.protectedRange);
        end
        % Instantiate agents
        function tc = setAgents(tc)
            % Agents will be initialized under different parameters in individual test cases
            % Instantiate a random number of agents according to parameters
            for ii = 1:randi([tc.minAgents, tc.maxAgents])
                tc.agents{ii, 1} = agent;
            end

            % Random collision ranges for each agent
            tc.collisionRanges = tc.minCollisionRange + rand(size(tc.agents, 1), 1) * (tc.maxCollisionRange - tc.minCollisionRange);
        
            % Random commuunications ranges for each agent
            tc.commsRanges = tc.minCommsRange + rand(size(tc.agents, 1), 1) * (tc.maxCommsRange - tc.minCommsRange);
        end
    end

    methods (Test)
        % Test methods
        function miSim_initialization(tc)
            % Test flag: set true to exercise the SINR-aware routing
            % overlay on this random scenario. Routing requires SINR comms, so
            % the flag switches the whole SINR stack on (agents keep their
            % default 0.1 W txPower); the permissive -30 dB threshold keeps
            % random geometries fully connected.
            useRoutingTopology = true;

            % randomly create obstacles
            nGeom = tc.minNumObstacles + randi(tc.maxNumObstacles - tc.minNumObstacles);
            tc.obstacles = cell(nGeom, 1);

            % Iterate over obstacles to initialize
            for ii = 1:size(tc.obstacles, 1)
                badCandidate = true;
                while badCandidate
                    % Instantiate a rectangular prism obstacle inside the domain
                    tc.obstacles{ii} = rectangularPrism;
                    tc.obstacles{ii} = tc.obstacles{ii}.initializeRandom(REGION_TYPE.OBSTACLE, sprintf("Obstacle %d", ii), tc.minObstacleSize, tc.maxObstacleSize, tc.domain, tc.minAlt);
    
                    % Check if the obstacle collides with an existing obstacle
                    if ~tc.obstacleCollisionCheck(tc.obstacles(1:(ii - 1)), tc.obstacles{ii})
                        badCandidate = false;
                    end

                end
            end

            % Add agents individually, ensuring that each addition does not
            % invalidate the initialization setup
            for ii = 1:size(tc.agents, 1)
                initInvalid = true;
                while initInvalid
                    candidatePos = [tc.domain.objective.groundPos, 0];
                    % Generate a random position for the agent based on
                    % existing agent positions
                    if ii == 1
                        while agentsCrowdObjective(tc.domain.objective, candidatePos, mean(tc.domain.dimensions) / 2)
                            candidatePos = tc.domain.random();
                            candidatePos(3) = tc.minAlt  + rand * 3; % place agents at decent altitudes for sensing
                        end
                    else
                        candidatePos = tc.agents{randi(ii - 1)}.pos + sign(randn([1, 3])) .* (rand(1, 3) .* tc.commsRanges(ii)/sqrt(2));
                        candidatePos(3) = tc.minAlt  + rand * 3; % place agents at decent altitudes for sensing
                    end

                    % Make sure that the candidate position is within the
                    % domain
                    if ~tc.domain.contains(candidatePos)
                        continue;
                    end

                    % Make sure that the candidate position does not crowd
                    % the sensing objective and create boring scenarios
                    if agentsCrowdObjective(tc.domain.objective, candidatePos, mean(tc.domain.dimensions) / 2)
                        continue;
                    end

                    % Make sure that there exist unobstructed lines of sight at
                    % appropriate ranges to form a connected communications 
                    % graph between the agents
                    connections = false(1, ii - 1);
                    for jj = 1:(ii - 1)
                        if norm(tc.agents{jj}.pos - candidatePos) <= min(tc.commsRanges([ii, jj]))
                            % Check new agent position against all existing
                            % agent positions for communications range
                            connections(jj) = true;
                            for kk = 1:size(tc.obstacles, 1)
                                if tc.obstacles{kk}.containsLine(tc.agents{jj}.pos, candidatePos)
                                    connections(jj) = false;
                                end
                            end
                        end
                    end

                    % New agent must be connected to an existing agent to
                    % be valid
                    if ii ~= 1 && ~any(connections)
                        continue;
                    end

                    % Initialize candidate agent collision geometry
                    candidateGeometry = spherical;
                    candidateGeometry = candidateGeometry.initialize(candidatePos, tc.collisionRanges(ii), REGION_TYPE.COLLISION);

                    % Initialize candidate agent sensor model
                    tc.sensor = tc.sensor.initialize(tc.alphaDistMin + rand * (tc.alphaDistMax - tc.alphaDistMin), tc.betaDistMin + rand * (tc.betaDistMax - tc.betaDistMin), tc.alphaTiltMin + rand * (tc.alphaTiltMax - tc.alphaTiltMin), tc.betaTiltMin + rand * (tc.betaTiltMax - tc.betaTiltMin));

                    % Initialize candidate agent
                    newAgent = tc.agents{ii}.initialize(candidatePos, candidateGeometry, tc.sensor, tc.commsRanges(ii), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize); 

                    % Make sure candidate agent doesn't collide with
                    % domain
                    violation = false;
                    for jj = 1:size(newAgent.collisionGeometry.vertices, 1)
                        % Check if collision geometry exits domain
                        if ~tc.domain.contains(newAgent.collisionGeometry.vertices(jj, 1:3))
                            violation = true;
                            break;
                        end
                    end
                    if violation
                        continue;
                    end

                    % Make sure candidate doesn't collide with obstacles
                    violation = false;
                    for kk = 1:size(tc.obstacles, 1)
                        if geometryIntersects(tc.obstacles{kk}, newAgent.collisionGeometry)
                            violation = true;
                            break;
                        end
                    end
                    if violation
                        continue;
                    end

                    % Make sure candidate doesn't collide with existing
                    % agents
                    violation = false;
                    for kk = 1:(ii - 1)
                        if geometryIntersects(tc.agents{kk}.collisionGeometry, newAgent.collisionGeometry)
                            violation = true;
                            break;
                        end
                    end

                    % Make sure candidate clears domain floor
                    if newAgent.pos(3) - newAgent.collisionGeometry.radius <= tc.minAlt
                        violation = true;
                    end
                    
                    if violation
                        continue;
                    end

                    % Candidate agent is valid, store to pass in to sim
                    initInvalid = false;
                    tc.agents{ii} = newAgent;
                end
            end

            % Initialize the simulation
            if useRoutingTopology
                tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo, tc.useDoubleIntegrator, tc.dampingCoeff, tc.useFixedTopology, tc.optimizeSensorPointing, true, -30.0, 2.0, 290, 2.4e9, 20e6, true);
            else
                tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo, tc.useDoubleIntegrator, tc.dampingCoeff, tc.useFixedTopology, tc.optimizeSensorPointing);
            end
        end
        function miSim_run_rf_sensor(tc)
            % randomly create obstacles
            nGeom = tc.minNumObstacles + randi(tc.maxNumObstacles - tc.minNumObstacles);
            tc.obstacles = cell(nGeom, 1);

            % Iterate over obstacles to initialize
            for ii = 1:size(tc.obstacles, 1)
                badCandidate = true;
                while badCandidate
                    % Instantiate a rectangular prism obstacle inside the domain
                    tc.obstacles{ii} = rectangularPrism;
                    tc.obstacles{ii} = tc.obstacles{ii}.initializeRandom(REGION_TYPE.OBSTACLE, sprintf("Obstacle %d", ii), tc.minObstacleSize, tc.maxObstacleSize, tc.domain, tc.minAlt);
    
                    % Check if the obstacle collides with an existing obstacle
                    if ~tc.obstacleCollisionCheck(tc.obstacles(1:(ii - 1)), tc.obstacles{ii})
                        badCandidate = false;
                    end
                end
            end

            % Add agents individually, ensuring that each addition does not
            % invalidate the initialization setup
            for ii = 1:size(tc.agents, 1)
                initInvalid = true;
                while initInvalid
                    candidatePos = [tc.domain.objective.groundPos, 0];
                    % Generate a random position for the agent based on
                    % existing agent positions
                    if ii == 1
                        while agentsCrowdObjective(tc.domain.objective, candidatePos, mean(tc.domain.dimensions) / 2)
                            candidatePos = tc.domain.random();
                            candidatePos(3) = min([tc.domain.maxCorner(3) * 0.95, tc.minAlt + rand * (tc.alphaDistMax * (1.1)  - 0.5)]); % place agents at decent altitudes for sensing
                        end
                    else
                        candidatePos = tc.agents{randi(ii - 1)}.pos + sign(randn([1, 3])) .* (rand(1, 3) .* tc.commsRanges(ii)/sqrt(2));
                        candidatePos(3) = min([tc.domain.maxCorner(3) * 0.95, tc.minAlt + rand * (tc.alphaDistMax * (1.1)  - 0.5)]); % place agents at decent altitudes for sensing
                    end

                    % Make sure that the candidate position is within the
                    % domain
                    if ~tc.domain.contains(candidatePos)
                        continue;
                    end

                    % Make sure that the candidate position does not crowd
                    % the sensing objective and create boring scenarios
                    if agentsCrowdObjective(tc.domain.objective, candidatePos, mean(tc.domain.dimensions) / 2)
                        continue;
                    end

                    % Make sure that there exist unobstructed lines of sight at
                    % appropriate ranges to form a connected communications 
                    % graph between the agents
                    connections = false(1, ii - 1);
                    for jj = 1:(ii - 1)
                        if norm(tc.agents{jj}.pos - candidatePos) <= min(tc.commsRanges([ii, jj]))
                            % Check new agent position against all existing
                            % agent positions for communications range
                            connections(jj) = true;
                            for kk = 1:size(tc.obstacles, 1)
                                if tc.obstacles{kk}.containsLine(tc.agents{jj}.pos, candidatePos)
                                    connections(jj) = false;
                                end
                            end
                        end
                    end

                    % New agent must be connected to an existing agent to
                    % be valid
                    if ii ~= 1 && ~any(connections)
                        continue;
                    end

                    % Initialize candidate agent collision geometry
                    % candidateGeometry = rectangularPrism;
                    % candidateGeometry = candidateGeometry.initialize([candidatePos - tc.collisionRanges(ii) * ones(1, 3); candidatePos + tc.collisionRanges(ii) * ones(1, 3)], REGION_TYPE.COLLISION);
                    candidateGeometry = spherical;
                    candidateGeometry = candidateGeometry.initialize(candidatePos, tc.collisionRanges(ii), REGION_TYPE.COLLISION);

                    % Initialize candidate agent sensor model
                    tc.sensor = rfSensor;
                    tilt = 0; azimuth = 0;
                    tc.sensor = tc.sensor.initialize(tc.P_TX * 1 + rand * 4, tc.T_0, tc.BW, tc.f_c, tc.G_RX_dBi, tc.beamwidthExponent + randi(100), tilt, azimuth, tc.lossExponent);

                    % Initialize candidate agent
                    newAgent = tc.agents{ii}.initialize(candidatePos, candidateGeometry, tc.sensor, tc.commsRanges(ii), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
                    
                    % Make sure candidate agent doesn't collide with
                    % domain
                    violation = false;
                    for jj = 1:size(newAgent.collisionGeometry.vertices, 1)
                        % Check if collision geometry exits domain
                        if ~tc.domain.contains(newAgent.collisionGeometry.vertices(jj, 1:3))
                            violation = true;
                            break;
                        end
                    end
                    if violation
                        continue;
                    end

                    % Make sure candidate doesn't collide with obstacles
                    violation = false;
                    for kk = 1:size(tc.obstacles, 1)
                        if geometryIntersects(tc.obstacles{kk}, newAgent.collisionGeometry)
                            violation = true;
                            break;
                        end
                    end
                    if violation
                        continue;
                    end

                    % Make sure candidate doesn't collide with existing
                    % agents
                    violation = false;
                    for kk = 1:(ii - 1)
                        if geometryIntersects(tc.agents{kk}.collisionGeometry, newAgent.collisionGeometry)
                            violation = true;
                            break;
                        end
                    end

                    % Make sure candidate clears domain floor
                    if newAgent.pos(3) - newAgent.collisionGeometry.radius <= tc.minAlt
                        violation = true;
                    end

                    if violation
                        continue;
                    end

                    % Candidate agent is valid, store to pass in to sim
                    initInvalid = false;
                    tc.agents{ii} = newAgent;
                end
            end

            % Initialize the simulation
            tc.optimizeSensorPointing = true;
            tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo, tc.useDoubleIntegrator, tc.dampingCoeff, tc.useFixedTopology, tc.optimizeSensorPointing);

            % Write out initialization state
            tc.testClass.writeInits();

            % Run simulation loop
            tc.testClass = tc.testClass.run();
        end
        function miSim_run(tc)
            % Test flag: set true to exercise the min-max-workload routing
            % overlay on this random scenario. Routing requires SINR comms, so
            % the flag switches the whole SINR stack on (agents keep their
            % default 0.1 W txPower); the permissive -30 dB threshold keeps
            % random geometries fully connected.
            useRoutingTopology = true;

            % randomly create obstacles
            nGeom = tc.minNumObstacles + randi(tc.maxNumObstacles - tc.minNumObstacles);
            tc.obstacles = cell(nGeom, 1);

            % Iterate over obstacles to initialize
            for ii = 1:size(tc.obstacles, 1)
                badCandidate = true;
                while badCandidate
                    % Instantiate a rectangular prism obstacle inside the domain
                    tc.obstacles{ii} = rectangularPrism;
                    tc.obstacles{ii} = tc.obstacles{ii}.initializeRandom(REGION_TYPE.OBSTACLE, sprintf("Obstacle %d", ii), tc.minObstacleSize, tc.maxObstacleSize, tc.domain, tc.minAlt);
    
                    % Check if the obstacle collides with an existing obstacle
                    if ~tc.obstacleCollisionCheck(tc.obstacles(1:(ii - 1)), tc.obstacles{ii})
                        badCandidate = false;
                    end
                end
            end

            % Add agents individually, ensuring that each addition does not
            % invalidate the initialization setup
            for ii = 1:size(tc.agents, 1)
                initInvalid = true;
                while initInvalid
                    candidatePos = [tc.domain.objective.groundPos, 0];
                    % Generate a random position for the agent based on
                    % existing agent positions
                    if ii == 1
                        while agentsCrowdObjective(tc.domain.objective, candidatePos, mean(tc.domain.dimensions) / 2)
                            candidatePos = tc.domain.random();
                            candidatePos(3) = min([tc.domain.maxCorner(3) * 0.95, tc.minAlt + rand * (tc.alphaDistMax * (1.1)  - 0.5)]); % place agents at decent altitudes for sensing
                        end
                    else
                        candidatePos = tc.agents{randi(ii - 1)}.pos + sign(randn([1, 3])) .* (rand(1, 3) .* tc.commsRanges(ii)/sqrt(2));
                        candidatePos(3) = min([tc.domain.maxCorner(3) * 0.95, tc.minAlt + rand * (tc.alphaDistMax * (1.1)  - 0.5)]); % place agents at decent altitudes for sensing
                    end

                    % Make sure that the candidate position is within the
                    % domain
                    if ~tc.domain.contains(candidatePos)
                        continue;
                    end

                    % Make sure that the candidate position does not crowd
                    % the sensing objective and create boring scenarios
                    if agentsCrowdObjective(tc.domain.objective, candidatePos, mean(tc.domain.dimensions) / 2)
                        continue;
                    end

                    % Make sure that there exist unobstructed lines of sight at
                    % appropriate ranges to form a connected communications 
                    % graph between the agents
                    connections = false(1, ii - 1);
                    for jj = 1:(ii - 1)
                        if norm(tc.agents{jj}.pos - candidatePos) <= min(tc.commsRanges([ii, jj]))
                            % Check new agent position against all existing
                            % agent positions for communications range
                            connections(jj) = true;
                            for kk = 1:size(tc.obstacles, 1)
                                if tc.obstacles{kk}.containsLine(tc.agents{jj}.pos, candidatePos)
                                    connections(jj) = false;
                                end
                            end
                        end
                    end

                    % New agent must be connected to an existing agent to
                    % be valid
                    if ii ~= 1 && ~any(connections)
                        continue;
                    end

                    % Initialize candidate agent collision geometry
                    % candidateGeometry = rectangularPrism;
                    % candidateGeometry = candidateGeometry.initialize([candidatePos - tc.collisionRanges(ii) * ones(1, 3); candidatePos + tc.collisionRanges(ii) * ones(1, 3)], REGION_TYPE.COLLISION);
                    candidateGeometry = spherical;
                    candidateGeometry = candidateGeometry.initialize(candidatePos, tc.collisionRanges(ii), REGION_TYPE.COLLISION);

                    % Initialize candidate agent sensor model
                    tc.sensor = tc.sensor.initialize(tc.alphaDistMin + rand * (tc.alphaDistMax - tc.alphaDistMin), tc.betaDistMin + rand * (tc.betaDistMax - tc.betaDistMin), tc.alphaTiltMin + rand * (tc.alphaTiltMax - tc.alphaTiltMin), tc.betaTiltMin + rand * (tc.betaTiltMax - tc.betaTiltMin));

                    % Initialize candidate agent
                    newAgent = tc.agents{ii}.initialize(candidatePos, candidateGeometry, tc.sensor, tc.commsRanges(ii), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
                    
                    % Make sure candidate agent doesn't collide with
                    % domain
                    violation = false;
                    for jj = 1:size(newAgent.collisionGeometry.vertices, 1)
                        % Check if collision geometry exits domain
                        if ~tc.domain.contains(newAgent.collisionGeometry.vertices(jj, 1:3))
                            violation = true;
                            break;
                        end
                    end
                    if violation
                        continue;
                    end

                    % Make sure candidate doesn't collide with obstacles
                    violation = false;
                    for kk = 1:size(tc.obstacles, 1)
                        if geometryIntersects(tc.obstacles{kk}, newAgent.collisionGeometry)
                            violation = true;
                            break;
                        end
                    end
                    if violation
                        continue;
                    end

                    % Make sure candidate doesn't collide with existing
                    % agents
                    violation = false;
                    for kk = 1:(ii - 1)
                        if geometryIntersects(tc.agents{kk}.collisionGeometry, newAgent.collisionGeometry)
                            violation = true;
                            break;
                        end
                    end

                    % Make sure candidate clears domain floor
                    if newAgent.pos(3) - newAgent.collisionGeometry.radius <= tc.minAlt
                        violation = true;
                    end

                    if violation
                        continue;
                    end

                    % Candidate agent is valid, store to pass in to sim
                    initInvalid = false;
                    tc.agents{ii} = newAgent;
                end
            end

            % Initialize the simulation
            if useRoutingTopology
                tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo, tc.useDoubleIntegrator, tc.dampingCoeff, tc.useFixedTopology, tc.optimizeSensorPointing, true, -30.0, 2.0, 290, 2.4e9, 20e6, true);
            else
                tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo, tc.useDoubleIntegrator, tc.dampingCoeff, tc.useFixedTopology, tc.optimizeSensorPointing);
            end

            % Write out initialization state
            tc.testClass.writeInits();

            % Run simulation loop
            tc.testClass = tc.testClass.run();
        end
        function test_basic_partitioning(tc)
            % place agents a fixed distance +/- X from the domain's center
            d = 1;

            % Initialize agent collision geometry
            tc.agents = {agent; agent; agent};
            geometry1 = spherical;
            geometry2 = geometry1;
            geometry3 = geometry1;
            geometry1 = geometry1.initialize(tc.domain.center + [d, 0, 0], tc.collisionRanges(1), REGION_TYPE.COLLISION);
            geometry2 = geometry2.initialize(tc.domain.center - [d, 0, 0], tc.collisionRanges(2), REGION_TYPE.COLLISION);
            geometry3 = geometry3.initialize(tc.domain.center - [0, d, 0], tc.collisionRanges(3), REGION_TYPE.COLLISION);
            
            % Initialize agent sensor model with fixed parameters
            tc.sensor = tc.sensor.initialize(tc.domain.maxCorner(3) / 2, 9, 22.5, 9);

            % Initialize agents
            tc.commsRanges = 3 * d * ones(size(tc.agents));
            tc.agents{1} = tc.agents{1}.initialize(tc.domain.center + [d, 0, 0], geometry1, tc.sensor, tc.commsRanges(1), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
            tc.agents{2} = tc.agents{2}.initialize(tc.domain.center - [d, 0, 0], geometry2, tc.sensor, tc.commsRanges(2), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
            tc.agents{3} = tc.agents{3}.initialize(tc.domain.center - [0, d, 0], geometry3, tc.sensor, tc.commsRanges(3), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);

            % Initialize the simulation
            tc.obstacles = cell(0, 1);
            tc.makePlots = false;
            tc.makeVideo = false;
            tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo, tc.useDoubleIntegrator, tc.dampingCoeff, tc.useFixedTopology, tc.optimizeSensorPointing);
        
            centerIdx = floor(size(tc.testClass.partitioning, 1) / 2);
            tc.verifyEqual(tc.testClass.partitioning(centerIdx, centerIdx:(centerIdx + 2)), [2, 3, 1]); % all three near center
            tc.verifyLessThan(sum(tc.testClass.partitioning == 1, "all"), sum(tc.testClass.partitioning == 0, "all")); % more non-assignments than partition 1 assignments
            tc.verifyLessThan(sum(tc.testClass.partitioning == 2, "all"), sum(tc.testClass.partitioning == 1, "all")); % more partition 1 assignments than partition 2 assignments
            tc.verifyLessThan(sum(tc.testClass.partitioning == 3, "all"), sum(tc.testClass.partitioning == 2, "all")); % more partition 3 assignments than partition 2 assignments
            tc.verifyEqual(unique(tc.testClass.partitioning), [0; 1; 2; 3;]);
        end
        function test_single_partition(tc)
            % Initialize agent collision geometry
            tc.agents = {agent};
            geometry1 = spherical;
            geometry1 = geometry1.initialize([tc.domain.center(1:2), 3], tc.collisionRanges(1), REGION_TYPE.COLLISION);
            
            % Initialize agent sensor model with fixed parameters
            tc.sensor = tc.sensor.initialize(tc.minDimension / 2, 3, 20, 3);

            % Initialize agents
            tc.agents{1} = tc.agents{1}.initialize([tc.domain.center(1:2), 3], geometry1, tc.sensor, tc.commsRanges(1), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);

            % Initialize the simulation
            tc.obstacles = cell(0, 1);
            tc.makePlots = false;
            tc.makeVideo = false;
            tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo, tc.useDoubleIntegrator, tc.dampingCoeff, tc.useFixedTopology, tc.optimizeSensorPointing);
            close(tc.testClass.fPerf);

            tc.verifyEqual(unique(tc.testClass.partitioning), [0; 1]);
            tc.verifyLessThan(sum(tc.testClass.partitioning == 1, "all"), sum(tc.testClass.partitioning == 0, "all"));
        end
        function test_single_agent_gradient_ascent(tc)
            % make basic domain
            tc.minDimension = 10; % domain size
            tc.domain = tc.domain.initialize([zeros(1, 3);tc.minDimension* ones(1, 3)], REGION_TYPE.DOMAIN, "Domain");

            % make basic sensing objective
            tc.domain.objective = tc.domain.objective.initialize(objectiveFunctionWrapper([7, 6]), tc.domain, tc.discretizationStep, tc.protectedRange, tc.opticalPartitioningMin, [7, 6]);
        
            % Initialize agent collision geometry
            tc.agents = {agent};
            geometry1 = spherical;
            geometry1 = geometry1.initialize([tc.domain.center(1:2)-tc.domain.dimensions(1)/4, 3], tc.collisionRanges(1), REGION_TYPE.COLLISION);
            
            % Initialize agent sensor model with fixed parameters
            tc.sensor = tc.sensor.initialize(tc.minDimension / 2, 3, 20, 3);

            % Initialize agents
            tc.maxIter = 75;
            tc.agents{1} = tc.agents{1}.initialize([tc.domain.center(1:2)-tc.domain.dimensions(1)/4, 3], geometry1, tc.sensor, tc.commsRanges(1), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);

            % Initialize the simulation
            tc.obstacles = cell(0, 1);
            tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo, tc.useDoubleIntegrator, tc.dampingCoeff, tc.useFixedTopology, tc.optimizeSensorPointing);
            
            % Run the simulation
            tc.testClass = tc.testClass.run();
        end
        function test_single_agent_gradient_ascent_tilted(tc)
            % make basic domain
            tc.minDimension = 10; % domain size
            tc.domain = tc.domain.initialize([zeros(1, 3);tc.minDimension* ones(1, 3)], REGION_TYPE.DOMAIN, "Domain");

            % make basic sensing objective
            tc.domain.objective = tc.domain.objective.initialize(objectiveFunctionWrapper([7, 6]), tc.domain, tc.discretizationStep, tc.protectedRange, tc.opticalPartitioningMin, [7, 6]);
        
            % Initialize agent collision geometry
            tc.agents = {agent};
            geometry1 = spherical;
            geometry1 = geometry1.initialize([tc.domain.center(1:2)-tc.domain.dimensions(1)/4, 3], tc.collisionRanges(1), REGION_TYPE.COLLISION);
            
            % Initialize agent sensor model with fixed parameters
            tc.sensor = tc.sensor.initialize(tc.minDimension / 2, 3, 20, 3, 25, 155);

            % Initialize agents
            tc.maxIter = 75;
            tc.agents{1} = tc.agents{1}.initialize([tc.domain.center(1:2)-tc.domain.dimensions(1)/4, 3], geometry1, tc.sensor, tc.commsRanges(1), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);

            % Initialize the simulation
            tc.obstacles = cell(0, 1);
            tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo, tc.useDoubleIntegrator, tc.dampingCoeff, tc.useFixedTopology, tc.optimizeSensorPointing);
            
            % Run the simulation
            tc.testClass = tc.testClass.run();
        end
        function test_single_agent_gradient_ascent_tilted_RF_sensor(tc)
            % make basic domain
            tc.minDimension = 10; % domain size
            tc.domain = tc.domain.initialize([zeros(1, 3);tc.minDimension* ones(1, 3)], REGION_TYPE.DOMAIN, "Domain");

            % make basic sensing objective
            tc.domain.objective = tc.domain.objective.initialize(objectiveFunctionWrapper([8, 3], reshape([15, 4; 4, 15], [1 2 2])), tc.domain, tc.discretizationStep, tc.protectedRange, tc.sinrPartitioningMin, [7, 6]);
        
            % Initialize agent collision geometry
            tc.agents = {agent};
            geometry1 = spherical;
            geometry1 = geometry1.initialize([tc.domain.center(1:2)-tc.domain.dimensions(1)/4, 3], tc.collisionRanges(1), REGION_TYPE.COLLISION);

            tc.sensor = rfSensor;
            tc.sensor = tc.sensor.initialize(tc.P_TX, tc.T_0, tc.BW, tc.f_c, tc.G_RX_dBi, tc.beamwidthExponent, 45, 45, tc.lossExponent);

            % Initialize agents
            tc.maxIter = 75;
            tc.agents{1} = tc.agents{1}.initialize([tc.domain.center(1:2)-tc.domain.dimensions(1)/4, 3], geometry1, tc.sensor, tc.commsRanges(1), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);

            % Initialize the simulation
            tc.obstacles = cell(0, 1);
            tc.minAlt = 0.5;
            tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo, tc.useDoubleIntegrator, tc.dampingCoeff, tc.useFixedTopology, tc.optimizeSensorPointing);
            
            % Run the simulation
            tc.testClass = tc.testClass.run();
        end
        function test_single_agent_gradient_ascent_sensor_pointing(tc)
            % make basic domain
            tc.minDimension = 10; % domain size
            tc.domain = tc.domain.initialize([zeros(1, 3);tc.minDimension* ones(1, 3)], REGION_TYPE.DOMAIN, "Domain");

            % make basic sensing objective
            tc.domain.objective = tc.domain.objective.initialize(objectiveFunctionWrapper([7, 6]), tc.domain, tc.discretizationStep, tc.protectedRange, tc.opticalPartitioningMin, [7, 6]);
        
            % Initialize agent collision geometry
            tc.agents = {agent};
            geometry1 = spherical;
            geometry1 = geometry1.initialize([tc.domain.center(1:2)-tc.domain.dimensions(1)/4, 3], tc.collisionRanges(1), REGION_TYPE.COLLISION);
            
            % Initialize agent sensor model with fixed parameters
            tc.sensor = tc.sensor.initialize(tc.minDimension / 2, 3, 20, 3);

            % Initialize agents
            tc.maxIter = 75;
            tc.agents{1} = tc.agents{1}.initialize([tc.domain.center(1:2)-tc.domain.dimensions(1)/4, 3], geometry1, tc.sensor, tc.commsRanges(1), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);

            % Initialize the simulation
            tc.optimizeSensorPointing = true;
            tc.obstacles = cell(0, 1);
            tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo, tc.useDoubleIntegrator, tc.dampingCoeff, tc.useFixedTopology, tc.optimizeSensorPointing);

            % Run the simulation
            tc.testClass = tc.testClass.run();
        end
        function test_single_agent_gradient_ascent_rf_sensor_pointing(tc)
            % make basic domain
            tc.minDimension = 10; % domain size
            tc.domain = tc.domain.initialize([zeros(1, 3);tc.minDimension* ones(1, 3)], REGION_TYPE.DOMAIN, "Domain");

            % make basic sensing objective
            tc.domain.objective = tc.domain.objective.initialize(objectiveFunctionWrapper([7, 6]), tc.domain, tc.discretizationStep, tc.protectedRange, tc.sinrPartitioningMin, [7, 6]);
        
            % Initialize agent collision geometry
            tc.agents = {agent};
            geometry1 = spherical;
            geometry1 = geometry1.initialize([tc.domain.center(1:2)-tc.domain.dimensions(1)/4, 3], tc.collisionRanges(1), REGION_TYPE.COLLISION);
            
            % Initialize agent sensor model

            tc.sensor = rfSensor;
            tc.sensor = tc.sensor.initialize(tc.P_TX, tc.T_0, tc.BW, tc.f_c, tc.G_RX_dBi, tc.beamwidthExponent, 0, 0, tc.lossExponent);

            % Initialize agents
            tc.maxIter = 75;
            tc.agents{1} = tc.agents{1}.initialize([tc.domain.center(1:2)-tc.domain.dimensions(1)/4, 3], geometry1, tc.sensor, tc.commsRanges(1), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);

            % Initialize the simulation
            tc.optimizeSensorPointing = true;
            tc.obstacles = cell(0, 1);
            tc.minAlt = 0.5;
            tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo, tc.useDoubleIntegrator, tc.dampingCoeff, tc.useFixedTopology, tc.optimizeSensorPointing);
            
            % Run the simulation
            tc.testClass = tc.testClass.run();
        end
        function test_collision_avoidance(tc)
            % No obstacles
            % Fixed agent initial conditions
            % Exaggerated large collision geometries to test CA
            % make basic domain
            tc.minDimension = 10; % domain size
            tc.domain = tc.domain.initialize([zeros(1, 3);tc.minDimension* ones(1, 3)], REGION_TYPE.DOMAIN, "Domain");

            % make basic sensing objective
            tc.domain.objective = tc.domain.objective.initialize(objectiveFunctionWrapper([3, 7]), tc.domain, tc.discretizationStep, tc.protectedRange, tc.opticalPartitioningMin, [3, 7]);
        
            % Initialize agent collision geometry
            tc.agents = {agent; agent};
            tc.collisionRanges = 1.5 * ones(size(tc.agents));
            d = [2.5, 0, 0];
            geometry1 = spherical;
            geometry2 = spherical;
            geometry1 = geometry1.initialize(tc.domain.center + d, tc.collisionRanges(1), REGION_TYPE.COLLISION);
            geometry2 = geometry2.initialize(tc.domain.center - d, tc.collisionRanges(2), REGION_TYPE.COLLISION);
            
            % Initialize agent sensor model with fixed parameters
            tc.sensor = tc.sensor.initialize(tc.minDimension / 2, 3, 15, 3);

            % Initialize agents
            tc.maxIter = 25;
            tc.commsRanges = 5 * ones(size(tc.agents));
            tc.agents{1} = tc.agents{1}.initialize(tc.domain.center + d, geometry1, tc.sensor, tc.commsRanges(1), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
            tc.agents{2} = tc.agents{2}.initialize(tc.domain.center - d, geometry2, tc.sensor, tc.commsRanges(2), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);

            % Initialize the simulation
            tc.obstacles = cell(0, 1);
            tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo, tc.useDoubleIntegrator, tc.dampingCoeff, tc.useFixedTopology, tc.optimizeSensorPointing);
            
            % Run the simulation
            tc.testClass.run();
        end
        function test_obstacle_avoidance(tc)
            % Right now, the communications constraint is violated here

            % Fixed single obstacle
            % Fixed two agents initial conditions
            % Exaggerated large collision geometries
            % make basic domain
            tc.minDimension = 10; % domain size
            tc.domain = tc.domain.initialize([zeros(1, 3);tc.minDimension* ones(1, 3)], REGION_TYPE.DOMAIN, "Domain");

            % make basic sensing objective
            tc.domain.objective = tc.domain.objective.initialize(objectiveFunctionWrapper([8, 5.2195]), tc.domain, tc.discretizationStep, tc.protectedRange, tc.opticalPartitioningMin, [8, 5.2195]);
        
            % Initialize agent collision geometry
            tc.agents = {agent; agent;};
            tc.collisionRanges = 1.1 * ones(size(tc.agents));
            d = [3, 0, 0];

            yOffset = 1;
            % choice of 0 leads to the agents getting stuck attempting to go around the obstacle on both sides
            % choice of 1 leads to one agent easily going around while the other gets stuck and the communications link is broken

            geometry1 = spherical;
            geometry2 = geometry1;
            geometry1 = geometry1.initialize(tc.domain.center - d + [0, tc.collisionRanges(1) * 1.1 - yOffset, 0], tc.collisionRanges(1), REGION_TYPE.COLLISION);
            geometry2 = geometry2.initialize(tc.domain.center - d - [0, tc.collisionRanges(2) * 1.1 + yOffset, 0], tc.collisionRanges(2), REGION_TYPE.COLLISION);
            
            % Initialize agent sensor model with fixed parameters
            tc.sensor = tc.sensor.initialize(tc.minDimension / 2, 3, 15, 3);
            
            % Initialize obstacles
            obstacleLength = 1;
            tc.obstacles{1} = rectangularPrism;
            tc.obstacles{1} = tc.obstacles{1}.initialize([tc.domain.center(1:2) - obstacleLength, tc.minAlt; tc.domain.center(1:2) + obstacleLength, tc.domain.maxCorner(3)], REGION_TYPE.OBSTACLE, "Obstacle 1");

            % Initialize agents
            tc.commsRanges = (2 * tc.collisionRanges(1) + obstacleLength) * 0.9 * ones(size(tc.agents)); % defined such that they cannot go around the obstacle on both sides
            tc.agents{1} = tc.agents{1}.initialize(tc.domain.center - d + [0, tc.collisionRanges(1) * 1.1 - yOffset, 0], geometry1, tc.sensor, tc.commsRanges(1), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
            tc.agents{2} = tc.agents{2}.initialize(tc.domain.center - d - [0, tc.collisionRanges(2)  *1.1 + yOffset, 0], geometry2, tc.sensor, tc.commsRanges(2), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
            
            % Initialize the simulation
            tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo, tc.useDoubleIntegrator, tc.dampingCoeff, tc.useFixedTopology, tc.optimizeSensorPointing);
            
            % Run the simulation
            tc.testClass.run();
        end
        function test_communications_constraint_SINR_threshold(tc)
            % No obstacles
            % Fixed two agents initial conditions
            % Negligible collision geometries
            % Non-standard domain with two objectives that will try to pull the
            % agents apart
            tc.minDimension = 10; % domain size
            tc.domain = tc.domain.initialize([zeros(1, 3);tc.minDimension* ones(1, 3)], REGION_TYPE.DOMAIN, "Domain");

            % make basic sensing objective
            tc.domain.objective = tc.domain.objective.initialize(objectiveFunctionWrapper([2, 8; 8, 8]), tc.domain, tc.discretizationStep, tc.protectedRange);
        
            % Initialize agent collision geometry
            tc.agents = {agent; agent;};
            tc.collisionRanges = .25 * ones(size(tc.agents));
            d = [1, 0, 0];
            geometry1 = spherical;
            geometry2 = geometry1;
            geometry1 = geometry1.initialize(tc.domain.center + d, tc.collisionRanges(1), REGION_TYPE.COLLISION);
            geometry2 = geometry2.initialize(tc.domain.center - d, tc.collisionRanges(2), REGION_TYPE.COLLISION);
            
            % Initialize agent sensor model
            tc.sensor = sigmoidSensor;
            tc.sensor = tc.sensor.initialize(tc.minDimension / 2, 3, 15, 3);
            
            % Initialize obstacles
            tc.obstacles = {};
            
            % Initialize agents
            tc.maxIter = 50;
            tc.commsRanges = 4 * ones(size(tc.agents)); % unused in SINR mode; kept for parity with the fixed-radius variant
            tc.agents{1} = tc.agents{1}.initialize(tc.domain.center + d, geometry1, tc.sensor, tc.commsRanges(1), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
            tc.agents{2} = tc.agents{2}.initialize(tc.domain.center - d, geometry2, tc.sensor, tc.commsRanges(2), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);

            % SINR communications model parameters. With isotropic links and only
            % two agents (no interference) the link is noise-limited:
            %   SINR = Ptx / ((4*pi*fc/c)^2 * d^n * kB*T*B)
            % Pick the threshold so the SINR connectivity boundary sits at an
            % effective range of ~4 m, mirroring the commsRange used by the
            % fixed-radius variant of this test (so the two agents still cannot
            % reach their objectives without breaking connectivity).
            useSinrComms     = true;
            txPower          = [0.01, 0.01];     % transmit power per agent (W)
            pathLossExponent = 2.0;
            ambientTemp      = 290;     % K
            centerFreq       = 2.4e9;   % Hz
            bandwidth        = 20e6;    % Hz

            effRange         = 4.0;     % desired effective comms range (m)
            K_pl   = (4 * pi * centerFreq / 3e8)^2;            % free-space path-loss reference
            noiseW = 1.380649e-23 * ambientTemp * bandwidth;   % thermal noise (W)
            sinrThreshold = 10 * log10(mean(txPower) / (K_pl * effRange^pathLossExponent * noiseW)); % dB

            tc.agents{1}.txPower = txPower(1);
            tc.agents{2}.txPower = txPower(2);

            % Initialize the simulation in SINR comms mode
            tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo, tc.useDoubleIntegrator, tc.dampingCoeff, tc.useFixedTopology, tc.optimizeSensorPointing, useSinrComms, sinrThreshold, pathLossExponent, ambientTemp, centerFreq, bandwidth);

            % Run the simulation
            tc.testClass = tc.testClass.run();
        end
        function test_communications_constraint_SINR_threshold_3_agents(tc)
            % No obstacles
            % Fixed three agents initial conditions
            % Negligible collision geometries
            % Non-standard domain with two objectives that will try to pull the
            % agents apart
            tc.minDimension = 10; % domain size
            tc.domain = tc.domain.initialize([zeros(1, 3);tc.minDimension* ones(1, 3)], REGION_TYPE.DOMAIN, "Domain");

            % make basic sensing objective
            tc.domain.objective = tc.domain.objective.initialize(objectiveFunctionWrapper([2, 8; 8, 8]), tc.domain, tc.discretizationStep, tc.protectedRange);

            % Initialize agent collision geometry
            tc.agents = {agent; agent; agent;};
            tc.collisionRanges = .25 * ones(size(tc.agents));
            d = [2.0, 0, 0];  % 2 m collinear spacing (above the 1 m path-loss floor)
            geometry1 = spherical;
            geometry2 = geometry1;
            geometry3 = geometry1;
            geometry1 = geometry1.initialize(tc.domain.center - d, tc.collisionRanges(1), REGION_TYPE.COLLISION);
            geometry2 = geometry2.initialize(tc.domain.center, tc.collisionRanges(2), REGION_TYPE.COLLISION);
            geometry3 = geometry3.initialize(tc.domain.center + d, tc.collisionRanges(3), REGION_TYPE.COLLISION);

            % Initialize agent sensor model
            tc.sensor = sigmoidSensor;
            tc.sensor = tc.sensor.initialize(tc.minDimension / 2, 3, 15, 3);

            % Initialize obstacles
            tc.obstacles = {};

            % Initialize agents
            tc.maxIter = 50;
            tc.commsRanges = 4 * ones(size(tc.agents)); % unused in SINR mode; kept for parity with the fixed-radius variant
            tc.agents{1} = tc.agents{1}.initialize(tc.domain.center - d, geometry1, tc.sensor, tc.commsRanges(1), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
            tc.agents{2} = tc.agents{2}.initialize(tc.domain.center, geometry2, tc.sensor, tc.commsRanges(2), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
            tc.agents{3} = tc.agents{3}.initialize(tc.domain.center + d, geometry3, tc.sensor, tc.commsRanges(3), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);

            % SINR communications model parameters. Three collinear, equally
            % spaced agents with homogeneous transmit power form an
            % INTERFERENCE-limited network (co-agent interference swamps thermal
            % noise), unlike the two-agent noise-limited case. Two structural
            % facts set the connectivity boundaries:
            %   * A chain link (1-2 or 2-3) is limited by the MIDDLE agent, which
            %     receives one end while the other end interferes at the SAME
            %     range: signal = interference  ->  0 dB.
            %   * The end-to-end (1,3) link has its transmitter at 2x the range
            %     while the middle agent interferes at 1x:  (1/2)^n  ->  -6.02 dB.
            % Homogeneous power is required for the symmetry: unequal power would
            % push one chain link's middle-receiver SINR below 0 dB and break it.
            % Placing the threshold between the two boundaries (a couple dB above
            % the -6 dB direct link, a few dB below the 0 dB chain links) makes
            % exactly the two adjacent links {1-2, 2-3} feasible and rejects 1-3,
            % giving the desired 1 -> 2 -> 3 line. The +3 dB CBF padding then
            % still lands below the 0 dB chain binding, so the QP starts feasible.
            useSinrComms     = true;
            txPower          = [0.1, 0.1, 0.1];   % homogeneous transmit power (W)
            pathLossExponent = 2.0;
            ambientTemp      = 290;     % K
            centerFreq       = 2.4e9;   % Hz
            bandwidth        = 20e6;    % Hz

            endBindingDb  = 10 * log10(0.5^pathLossExponent);  % (1,3) link SINR: (1/2)^n = -6.02 dB
            sinrThreshold = endBindingDb + 2.0;                % ~ -4.02 dB: 2 dB above the rejected
                                                               % 1-3 link, ~4 dB below the 0 dB chain

            tc.agents{1}.txPower = txPower(1);
            tc.agents{2}.txPower = txPower(2);
            tc.agents{3}.txPower = txPower(3);

            % Initialize the simulation in SINR comms mode
            tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo, tc.useDoubleIntegrator, tc.dampingCoeff, tc.useFixedTopology, tc.optimizeSensorPointing, useSinrComms, sinrThreshold, pathLossExponent, ambientTemp, centerFreq, bandwidth);

            % The initial SINR topology must be exactly the 1 -> 2 -> 3 line:
            % two links (1-2 and 2-3), with the direct 1-3 link rejected.
            tc.assertEqual(tc.testClass.constraintAdjacencyMatrix, logical( ...
                [ 1, 1, 0; ...
                  1, 1, 1; ...
                  0, 1, 1;]));

            % Run the simulation
            tc.testClass = tc.testClass.run();
        end
        function test_routing_topology_convergence_demo(tc)
            % Demonstration of the Capacity-Aware Lesser Sink Neighbor
            % (CALSN) topology + routing algorithm: four agents with
            % sigmoid sensors in a cube domain converge on a single
            % sensing objective while the communication links evolve.
            %
            % Agents start scattered in a quadrilateral near one corner;
            % the objective sits in the opposite corner. Base stations are
            % agents 1 and 4. As the fleet migrates and contracts around
            % the objective, pairwise SINRs — and with them the Shannon
            % capacities, base-station potentials, parent choices, and
            % capacity-proportional flow splits — change every step, so
            % the network-graph tile shows the maintained (dotted) and
            % flow-carrying (solid, arrowed, flow-labelled) links
            % re-routing dynamically. validate() enforces the connected-
            % topology invariant on every step of the run.
            tc.minDimension = 10; % cube domain edge length
            tc.domain = tc.domain.initialize([zeros(1, 3);tc.minDimension* ones(1, 3)], REGION_TYPE.DOMAIN, "Domain");

            % Single sensing objective in the far corner
            tc.domain.objective = tc.domain.objective.initialize(objectiveFunctionWrapper([7, 7]), tc.domain, tc.discretizationStep, tc.protectedRange);

            % Four agents scattered (non-collinear) near the (low-x, low-y)
            % corner, co-altitude at z = 2. Base 1 trails the fleet and
            % base 4 leads it, with the relays staggered between: relay 3
            % starts too far from base 1 for a direct link to be worth
            % using, so its initial basin-1 route is MULTI-HOP via relay 2
            % and re-parents to the base directly as the fleet contracts.
            tc.agents = {agent; agent; agent; agent;};
            tc.collisionRanges = .25 * ones(size(tc.agents));
            positions = [1.0, 2.0, 2; ...
                         3.0, 3.5, 2; ...
                         5.5, 3.0, 2; ...
                         5.0, 5.5, 2;];

            % Initialize agent sensor model (sigmoid)
            tc.sensor = sigmoidSensor;
            tc.sensor = tc.sensor.initialize(tc.minDimension / 2, 3, 15, 3);

            % Initialize obstacles
            tc.obstacles = {};

            % Initialize agents: 50 steps is long enough to watch the
            % fleet converge on the objective and settle (the gradient
            % step decays linearly to zero over maxIter)
            tc.maxIter = 80;
            tc.commsRanges = 4 * ones(size(tc.agents)); % unused in SINR mode
            for aa = 1:4
                geometry = spherical;
                geometry = geometry.initialize(positions(aa, :), tc.collisionRanges(aa), REGION_TYPE.COLLISION);
                tc.agents{aa} = tc.agents{aa}.initialize(positions(aa, :), geometry, tc.sensor, tc.commsRanges(aa), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
                tc.agents{aa}.txPower = 0.1;
            end

            % SINR communications model. The threshold must sit >= 3 dB
            % below the worst link CALSN may select, because the CBF holds
            % every maintained link at threshold + 3 dB in BOTH directions:
            % a link selected inside that 3 dB band is born with a violated
            % barrier and can deadlock the QP (agents hold position, the
            % geometry never changes, and the same link is re-selected
            % forever). The staggered start's weakest candidate link (the
            % long 1-4 base-to-base diagonal) sits at ~ -9 dB and the
            % weakest link CALSN actually selects at ~ -4 dB, so -14 dB
            % leaves the -11 dB CBF floor a comfortable margin.
            useSinrComms       = true;
            pathLossExponent   = 2.0;
            ambientTemp        = 290;     % K
            centerFreq         = 2.4e9;   % Hz
            bandwidth          = 20e6;    % Hz
            sinrThreshold      = -14.0;   % dB
            useRoutingTopology = true;

            % Initialize the simulation with the CALSN topology selector
            tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo, tc.useDoubleIntegrator, tc.dampingCoeff, tc.useFixedTopology, tc.optimizeSensorPointing, useSinrComms, sinrThreshold, pathLossExponent, ambientTemp, centerFreq, bandwidth, useRoutingTopology);

            % Initial routing invariants: bases at zero potential, relays
            % finite; maintained topology one connected component; both
            % generated units delivered to the bases with nothing unrouted
            phi = tc.testClass.routingPotentials;
            tc.assertEqual(phi([1, 4]), [0; 0]);
            tc.assertTrue(all(isfinite(phi([2, 3]))) && all(phi([2, 3]) > 0));
            tc.assertEqual(max(conncomp(graph(tc.testClass.constraintAdjacencyMatrix))), 1);
            F = tc.testClass.routingFlows;
            for aa = [2, 3]
                tc.assertEqual(sum(F(aa, :)) - sum(F(:, aa)), 1.0, "AbsTol", 1e-9);
            end
            bases = [1, 4];
            tc.assertEqual(sum(F(:, bases), "all") - sum(F(bases, :), "all"), 2.0, "AbsTol", 1e-9);
            tc.assertTrue(all(tc.testClass.routingUnrouted == 0));

            % Run the simulation (per-step CALSN re-solve; validate()
            % checks feasibility- and maintained-graph connectivity at
            % every iteration)
            tc.testClass = tc.testClass.run();

            % Convergence: total sensing performance improved over the run
            tc.assertGreaterThan(tc.testClass.performance(end), tc.testClass.performance(2));

            % Dynamic evolution: the maintained topology changed at least
            % once relative to the initial configuration
            hist = tc.testClass.constraintAdjacencyHist;
            changed = false;
            for kk = 2:size(hist, 3)
                if any(hist(:, :, kk) ~= hist(:, :, 1), "all")
                    changed = true;
                    break;
                end
            end
            tc.assertTrue(changed, "Maintained topology never changed during the run");

            % Final routing state is still a valid delivery tree: one
            % connected component, conservation at the generating UAVs,
            % both units delivered, nothing unrouted
            tc.assertEqual(max(conncomp(graph(tc.testClass.constraintAdjacencyMatrix))), 1);
            F = tc.testClass.routingFlows;
            for aa = [2, 3]
                tc.assertEqual(sum(F(aa, :)) - sum(F(:, aa)), 1.0, "AbsTol", 1e-9);
            end
            tc.assertEqual(sum(F(:, bases), "all") - sum(F(bases, :), "all"), 2.0, "AbsTol", 1e-9);
            tc.assertTrue(all(tc.testClass.routingUnrouted == 0));
        end
        function test_routing_topology_convergence_demo_rfSensor(tc)
            % Demonstration of the Capacity-Aware Lesser Sink Neighbor
            % (CALSN) topology + routing algorithm: four agents with
            % sigmoid sensors in a cube domain converge on a single
            % sensing objective while the communication links evolve.
            %
            % Agents start scattered in a quadrilateral near one corner;
            % the objective sits in the opposite corner. Base stations are
            % agents 1 and 4. As the fleet migrates and contracts around
            % the objective, pairwise SINRs — and with them the Shannon
            % capacities, base-station potentials, parent choices, and
            % capacity-proportional flow splits — change every step, so
            % the network-graph tile shows the maintained (dotted) and
            % flow-carrying (solid, arrowed, flow-labelled) links
            % re-routing dynamically. validate() enforces the connected-
            % topology invariant on every step of the run.
            tc.minDimension = 10; % cube domain edge length
            tc.domain = tc.domain.initialize([zeros(1, 3);tc.minDimension* ones(1, 3)], REGION_TYPE.DOMAIN, "Domain");

            % Single sensing objective in the far corner
            tc.domain.objective = tc.domain.objective.initialize(objectiveFunctionWrapper([7, 7]), tc.domain, tc.discretizationStep, tc.protectedRange);

            % Four agents scattered (non-collinear) near the (low-x, low-y)
            % corner, co-altitude at z = 2. Base 1 trails the fleet and
            % base 4 leads it, with the relays staggered between: relay 3
            % starts too far from base 1 for a direct link to be worth
            % using, so its initial basin-1 route is MULTI-HOP via relay 2
            % and re-parents to the base directly as the fleet contracts.
            tc.agents = {agent; agent; agent; agent;};
            tc.collisionRanges = .25 * ones(size(tc.agents));
            positions = [1.0, 2.0, 2; ...
                         3.0, 3.5, 2; ...
                         5.5, 3.0, 2; ...
                         5.0, 5.5, 2;];

            % Initialize agent sensor model (sigmoid)
            tc.sensor = rfSensor;
            tilt = 0; azimuth = 0;
            tc.sensor = tc.sensor.initialize(tc.P_TX * 1 + rand * 4, tc.T_0, tc.BW, tc.f_c, tc.G_RX_dBi, tc.beamwidthExponent + randi(100), tilt, azimuth, tc.lossExponent);

            % Initialize obstacles
            tc.obstacles = {};

            % Initialize agents: 50 steps is long enough to watch the
            % fleet converge on the objective and settle (the gradient
            % step decays linearly to zero over maxIter)
            tc.maxIter = 80;
            tc.commsRanges = 4 * ones(size(tc.agents)); % unused in SINR mode
            for aa = 1:4
                geometry = spherical;
                geometry = geometry.initialize(positions(aa, :), tc.collisionRanges(aa), REGION_TYPE.COLLISION);
                tc.agents{aa} = tc.agents{aa}.initialize(positions(aa, :), geometry, tc.sensor, tc.commsRanges(aa), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
                tc.agents{aa}.txPower = 0.1;
            end

            % SINR communications model. The threshold must sit >= 3 dB
            % below the worst link CALSN may select, because the CBF holds
            % every maintained link at threshold + 3 dB in BOTH directions:
            % a link selected inside that 3 dB band is born with a violated
            % barrier and can deadlock the QP (agents hold position, the
            % geometry never changes, and the same link is re-selected
            % forever). The staggered start's weakest candidate link (the
            % long 1-4 base-to-base diagonal) sits at ~ -9 dB and the
            % weakest link CALSN actually selects at ~ -4 dB, so -14 dB
            % leaves the -11 dB CBF floor a comfortable margin.
            useSinrComms       = true;
            pathLossExponent   = 2.0;
            ambientTemp        = 290;     % K
            centerFreq         = 2.4e9;   % Hz
            bandwidth          = 20e6;    % Hz
            sinrThreshold      = -14.0;   % dB
            useRoutingTopology = true;

            % Initialize the simulation with the CALSN topology selector
            tc.optimizeSensorPointing = true;
            tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo, tc.useDoubleIntegrator, tc.dampingCoeff, tc.useFixedTopology, tc.optimizeSensorPointing, useSinrComms, sinrThreshold, pathLossExponent, ambientTemp, centerFreq, bandwidth, useRoutingTopology);

            % Initial routing invariants: bases at zero potential, relays
            % finite; maintained topology one connected component; both
            % generated units delivered to the bases with nothing unrouted
            phi = tc.testClass.routingPotentials;
            tc.assertEqual(phi([1, 4]), [0; 0]);
            tc.assertTrue(all(isfinite(phi([2, 3]))) && all(phi([2, 3]) > 0));
            tc.assertEqual(max(conncomp(graph(tc.testClass.constraintAdjacencyMatrix))), 1);
            F = tc.testClass.routingFlows;
            for aa = [2, 3]
                tc.assertEqual(sum(F(aa, :)) - sum(F(:, aa)), 1.0, "AbsTol", 1e-9);
            end
            bases = [1, 4];
            tc.assertEqual(sum(F(:, bases), "all") - sum(F(bases, :), "all"), 2.0, "AbsTol", 1e-9);
            tc.assertTrue(all(tc.testClass.routingUnrouted == 0));

            % Run the simulation (per-step CALSN re-solve; validate()
            % checks feasibility- and maintained-graph connectivity at
            % every iteration)
            tc.testClass = tc.testClass.run();

            % Convergence: total sensing performance improved over the run
            tc.assertGreaterThan(tc.testClass.performance(end), tc.testClass.performance(2));

            % Dynamic evolution: the maintained topology changed at least
            % once relative to the initial configuration
            hist = tc.testClass.constraintAdjacencyHist;
            changed = false;
            for kk = 2:size(hist, 3)
                if any(hist(:, :, kk) ~= hist(:, :, 1), "all")
                    changed = true;
                    break;
                end
            end
            tc.assertTrue(changed, "Maintained topology never changed during the run");

            % Final routing state is still a valid delivery tree: one
            % connected component, conservation at the generating UAVs,
            % both units delivered, nothing unrouted
            tc.assertEqual(max(conncomp(graph(tc.testClass.constraintAdjacencyMatrix))), 1);
            F = tc.testClass.routingFlows;
            for aa = [2, 3]
                tc.assertEqual(sum(F(aa, :)) - sum(F(:, aa)), 1.0, "AbsTol", 1e-9);
            end
            tc.assertEqual(sum(F(:, bases), "all") - sum(F(bases, :), "all"), 2.0, "AbsTol", 1e-9);
            tc.assertTrue(all(tc.testClass.routingUnrouted == 0));
        end
        function test_communications_constraint_fixed_radius(tc)
            % No obstacles
            % Fixed two agents initial conditions
            % Negligible collision geometries
            % Non-standard domain with two objectives that will try to pull the
            % agents apart
            tc.minDimension = 10; % domain size
            tc.domain = tc.domain.initialize([zeros(1, 3);tc.minDimension* ones(1, 3)], REGION_TYPE.DOMAIN, "Domain");

            % make basic sensing objective
            tc.domain.objective = tc.domain.objective.initialize(objectiveFunctionWrapper([2, 8; 8, 8]), tc.domain, tc.discretizationStep, tc.protectedRange);

            % Initialize agent collision geometry
            tc.agents = {agent; agent;};
            tc.collisionRanges = .25 * ones(size(tc.agents));
            d = [1, 0, 0];
            geometry1 = spherical;
            geometry2 = geometry1;
            geometry1 = geometry1.initialize(tc.domain.center + d, tc.collisionRanges(1), REGION_TYPE.COLLISION);
            geometry2 = geometry2.initialize(tc.domain.center - d, tc.collisionRanges(2), REGION_TYPE.COLLISION);

            % Initialize agent sensor model
            tc.sensor = sigmoidSensor;
            tc.sensor = tc.sensor.initialize(tc.minDimension / 2, 3, 15, 3);

            % Initialize obstacles
            tc.obstacles = {};

            % Initialize agents
            tc.maxIter = 50;
            tc.commsRanges = 4 * ones(size(tc.agents)); % defined such that they cannot reach their objective without breaking connectivity
            tc.agents{1} = tc.agents{1}.initialize(tc.domain.center + d, geometry1, tc.sensor, tc.commsRanges(1), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
            tc.agents{2} = tc.agents{2}.initialize(tc.domain.center - d, geometry2, tc.sensor, tc.commsRanges(2), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);

            % Initialize the simulation
            tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo, tc.useDoubleIntegrator, tc.dampingCoeff, tc.useFixedTopology, tc.optimizeSensorPointing);

            % Run the simulation
            tc.testClass = tc.testClass.run();
        end
        function test_obstacle_permits_comms_LOS(tc)
            % Fixed single obstacle
            % Fixed two agents initial conditions
            % Exaggerated large communications radius
            % make basic domain
            tc.minDimension = 10; % domain size
            tc.domain = tc.domain.initialize([zeros(1, 3); tc.minDimension* ones(1, 3)], REGION_TYPE.DOMAIN, "Domain");

            % make basic sensing objective
            tc.domain.objective = tc.domain.objective.initialize(objectiveFunctionWrapper([8, 5]), tc.domain, tc.discretizationStep, tc.protectedRange, tc.opticalPartitioningMin, [8, 5]);
        
            % Initialize agent collision geometry
            tc.agents = {agent; agent;};
            tc.collisionRanges = .25 * ones(size(tc.agents));
            d = 2;
            geometry1 = spherical;
            geometry2 = geometry1;
            geometry1 = geometry1.initialize(tc.domain.center - [d, 0, 0], tc.collisionRanges(1), REGION_TYPE.COLLISION);
            geometry2 = geometry2.initialize(tc.domain.center - [0, d, 0], tc.collisionRanges(2), REGION_TYPE.COLLISION);
            
            % Initialize agent sensor model
            tc.sensor = tc.sensor.initialize(tc.minDimension / 2, 3, 15, 3);

            % Initialize agents
            tc.maxIter = 125;
            tc.commsRanges = 5 * ones(size(tc.agents));
            tc.agents{1} = tc.agents{1}.initialize(tc.domain.center - [d, 0, 0], geometry1, tc.sensor, tc.commsRanges(1), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
            tc.agents{2} = tc.agents{2}.initialize(tc.domain.center - [0, d, 0], geometry2, tc.sensor, tc.commsRanges(2), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
            
            % Initialize obstacles
            obstacleLength = 1.5;
            tc.obstacles{1} = rectangularPrism;
            tc.obstacles{1} = tc.obstacles{1}.initialize([tc.domain.center(1:2) - obstacleLength, 0; tc.domain.center(1:2) + obstacleLength, tc.domain.maxCorner(3)], REGION_TYPE.OBSTACLE, "Obstacle 1");

            % Initialize the simulation
            tc.minAlt = 0;
            tc.makePlots = false;
            tc.makeVideo = false;
            tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo, tc.useDoubleIntegrator, tc.dampingCoeff, tc.useFixedTopology, tc.optimizeSensorPointing);

            % Communications link should be established
            tc.assertEqual(tc.testClass.adjacency, logical(true(2)));
        end
        function test_LNA_case_1(tc)
            % based on example in meeting 
            % no obstacles
            % fixed 5 agents initial conditions
            % unit communicaitons radius
            % negligible collision radius
            % make basic domain
            tc.minDimension = 10; % domain size
            tc.domain = tc.domain.initialize([zeros(1, 3);tc.minDimension* ones(1, 3)], REGION_TYPE.DOMAIN, "Domain");

            % make basic sensing objective
            tc.domain.objective = tc.domain.objective.initialize(objectiveFunctionWrapper([8, 5]), tc.domain, tc.discretizationStep, tc.protectedRange, tc.opticalPartitioningMin, [8, 5]);
        
            % Initialize agent collision geometry
            tc.agents = {agent; agent; agent; agent; agent;};
            tc.collisionRanges = .01 * ones(size(tc.agents));
            d = 1;
            geometry5 = spherical;
            geometry1 = geometry5.initialize(tc.domain.center + [d, 0, 0], tc.collisionRanges(1), REGION_TYPE.COLLISION);
            geometry2 = geometry5.initialize(tc.domain.center, tc.collisionRanges(2), REGION_TYPE.COLLISION);
            geometry3 = geometry5.initialize(tc.domain.center + [-d, d, 0], tc.collisionRanges(3), REGION_TYPE.COLLISION);
            geometry4 = geometry5.initialize(tc.domain.center + [-2*d, d, 0], tc.collisionRanges(4), REGION_TYPE.COLLISION);
            geometry5 = geometry5.initialize(tc.domain.center + [0, d, 0], tc.collisionRanges(5), REGION_TYPE.COLLISION);
            
            % Initialize agent sensor model
            tc.sensor = tc.sensor.initialize(tc.minDimension / 2, 3, 15, 3);

            % Initialize agents
            tc.maxIter = 125;
            tc.commsRanges = ones(size(tc.agents));
            tc.agents{1} = tc.agents{1}.initialize(tc.domain.center + [d, 0, 0], geometry1, tc.sensor, tc.commsRanges(1), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
            tc.agents{2} = tc.agents{2}.initialize(tc.domain.center, geometry2, tc.sensor, tc.commsRanges(2), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
            tc.agents{3} = tc.agents{3}.initialize(tc.domain.center + [-d, d, 0], geometry3, tc.sensor, tc.commsRanges(3), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
            tc.agents{4} = tc.agents{4}.initialize(tc.domain.center + [-2*d, d, 0], geometry4, tc.sensor, tc.commsRanges(4), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
            tc.agents{5} = tc.agents{5}.initialize(tc.domain.center + [0, d, 0], geometry5, tc.sensor, tc.commsRanges(5), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);

            % Initialize the simulation
            tc.minAlt = 0;
            tc.makePlots = false;
            tc.makeVideo = false;
            tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo, tc.useDoubleIntegrator, tc.dampingCoeff, tc.useFixedTopology, tc.optimizeSensorPointing);

            % Constraint adjacency matrix defined by LNA should be as follows
            tc.assertEqual(tc.testClass.constraintAdjacencyMatrix, logical( ...
                [ 1, 1, 0, 0, 0; ...
                  1, 1, 0, 0, 1; ...
                  0, 0, 1, 1, 1;
                  0, 0, 1, 1, 0;
                  0, 1, 1, 0, 1;]));
        end
        function test_LNA_case_2(tc)
            % based on example in paper Asynchronous Local Construction of Bounded-Degree Network Topologies Using Only Neighborhood Information
            % No obstacles
            % Fixed 7 agents initial conditions
            % unitary communicaitons radius
            % negligible collision radius
            % make basic domain
            tc.minDimension = 10; % domain size
            tc.domain = tc.domain.initialize([zeros(1, 3); tc.minDimension* ones(1, 3)], REGION_TYPE.DOMAIN, "Domain");

            % make basic sensing objective
            tc.domain.objective = tc.domain.objective.initialize(objectiveFunctionWrapper([8, 5]), tc.domain, tc.discretizationStep, tc.protectedRange, tc.opticalPartitioningMin, [8, 5]);
        
            % Initialize agent collision geometry
            tc.agents = {agent; agent; agent; agent; agent; agent; agent;};
            tc.collisionRanges = .01 * ones(size(tc.agents));
            d = 1;
            geometry7 = spherical;
            geometry1 = geometry7.initialize(tc.domain.center + [-0.9 * d/sqrt(2), 0.9 * d/sqrt(2), 0], tc.collisionRanges(1), REGION_TYPE.COLLISION);
            geometry2 = geometry7.initialize(tc.domain.center + [-0.5 * d, 0.25 * d, 0], tc.collisionRanges(2), REGION_TYPE.COLLISION);
            geometry3 = geometry7.initialize(tc.domain.center + [0.9 * d, 0, 0], tc.collisionRanges(3), REGION_TYPE.COLLISION);
            geometry4 = geometry7.initialize(tc.domain.center + [0.9 * d/sqrt(2), -0.9 * d/sqrt(2), 0], tc.collisionRanges(4), REGION_TYPE.COLLISION);
            geometry5 = geometry7.initialize(tc.domain.center + [0, 0.9 * d, 0], tc.collisionRanges(5), REGION_TYPE.COLLISION);
            geometry6 = geometry7.initialize(tc.domain.center, tc.collisionRanges(6), REGION_TYPE.COLLISION);
            geometry7 = geometry7.initialize(tc.domain.center + [d/2, d/2, 0], tc.collisionRanges(7), REGION_TYPE.COLLISION);
            
            % Initialize agent sensor model
            tc.sensor = tc.sensor.initialize(tc.minDimension / 2, 3, 15, 3);

            % Initialize agents
            tc.maxIter = 125;
            tc.commsRanges = d * ones(size(tc.agents));
            tc.agents{1} = tc.agents{1}.initialize(tc.domain.center + [-0.9 * d/sqrt(2), 0.9 * d/sqrt(2), 0], geometry1, tc.sensor, tc.commsRanges(1), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
            tc.agents{2} = tc.agents{2}.initialize(tc.domain.center + [-0.5 * d, 0.25 * d, 0], geometry2, tc.sensor, tc.commsRanges(2), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
            tc.agents{3} = tc.agents{3}.initialize(tc.domain.center + [0.9 * d, 0, 0], geometry3, tc.sensor, tc.commsRanges(3), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
            tc.agents{4} = tc.agents{4}.initialize(tc.domain.center + [0.9 * d/sqrt(2), -0.9 * d/sqrt(2), 0], geometry4, tc.sensor, tc.commsRanges(4), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
            tc.agents{5} = tc.agents{5}.initialize(tc.domain.center + [0, 0.9 * d, 0], geometry5, tc.sensor, tc.commsRanges(5), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
            tc.agents{6} = tc.agents{6}.initialize(tc.domain.center, geometry6, tc.sensor, tc.commsRanges(6), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);
            tc.agents{7} = tc.agents{7}.initialize(tc.domain.center + [d/2, d/2, 0], geometry7, tc.sensor, tc.commsRanges(7), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);

            % Initialize the simulation
            tc.minAlt = 0;
            tc.makePlots = false;
            tc.makeVideo = false;
            tc.testClass = tc.testClass.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makePlots, tc.makeVideo, tc.useDoubleIntegrator, tc.dampingCoeff, tc.useFixedTopology, tc.optimizeSensorPointing);

            % Constraint adjacency matrix defined by LNA should be as follows
            tc.assertEqual(tc.testClass.constraintAdjacencyMatrix, logical( ...
                [ 1, 1, 0, 0, 0, 0, 0; ...
                  1, 1, 0, 0, 1, 0, 0; ...
                  0, 0, 1, 1, 0, 0, 0;
                  0, 0, 1, 1, 0, 1, 0;
                  0, 1, 0, 0, 1, 1, 0;
                  0, 0, 0, 1, 1, 1, 1;
                  0, 0, 0, 0, 0, 1, 1; ]));
        end
        function miSim_initializeFromInits(tc)
            % Build a minimal valid simulation, write it to a matfile, reload
            % via initializeFromInits, assert round-trip consistency, then
            % delete the file.  No plotting or video at any stage.

            % Obstacles
            nGeom = tc.minNumObstacles + randi(tc.maxNumObstacles - tc.minNumObstacles);
            tc.obstacles = cell(nGeom, 1);
            for ii = 1:nGeom
                badCandidate = true;
                while badCandidate
                    tc.obstacles{ii} = rectangularPrism;
                    tc.obstacles{ii} = tc.obstacles{ii}.initializeRandom(REGION_TYPE.OBSTACLE, ...
                        sprintf("Obstacle %d", ii), tc.minObstacleSize, tc.maxObstacleSize, ...
                        tc.domain, tc.minAlt);
                    if ~tc.obstacleCollisionCheck(tc.obstacles(1:(ii - 1)), tc.obstacles{ii})
                        badCandidate = false;
                    end
                end
            end

            % Agents
            nAgents = tc.minAgents;
            tc.agents = cell(nAgents, 1);
            tc.collisionRanges = tc.minCollisionRange + rand(nAgents, 1) * (tc.maxCollisionRange - tc.minCollisionRange);
            tc.commsRanges = tc.minCommsRange + rand(nAgents, 1) * (tc.maxCommsRange - tc.minCommsRange);

            for ii = 1:nAgents
                initInvalid = true;
                while initInvalid
                    if ii == 1
                        candidatePos = tc.domain.random();
                        candidatePos(3) = tc.minAlt + rand * 3;
                        while agentsCrowdObjective(tc.domain.objective, candidatePos, mean(tc.domain.dimensions) / 2)
                            candidatePos = tc.domain.random();
                            candidatePos(3) = tc.minAlt + rand * 3;
                        end
                    else
                        candidatePos = tc.agents{randi(ii - 1)}.pos + sign(randn([1, 3])) .* (rand(1, 3) .* tc.commsRanges(ii) / sqrt(2));
                        candidatePos(3) = tc.minAlt + rand * 3;
                    end

                    if ~tc.domain.contains(candidatePos), continue; end
                    if agentsCrowdObjective(tc.domain.objective, candidatePos, mean(tc.domain.dimensions) / 2), continue; end

                    % Connectivity check
                    connections = false(1, ii - 1);
                    for jj = 1:(ii - 1)
                        if norm(tc.agents{jj}.pos - candidatePos) <= min(tc.commsRanges([ii, jj]))
                            connections(jj) = true;
                            for kk = 1:size(tc.obstacles, 1)
                                if tc.obstacles{kk}.containsLine(tc.agents{jj}.pos, candidatePos)
                                    connections(jj) = false;
                                end
                            end
                        end
                    end
                    if ii ~= 1 && ~any(connections), continue; end

                    geom = spherical;
                    geom = geom.initialize(candidatePos, tc.collisionRanges(ii), REGION_TYPE.COLLISION);
                    tc.sensor = sigmoidSensor;
                    tc.sensor = tc.sensor.initialize( ...
                        tc.alphaDistMin + rand * (tc.alphaDistMax - tc.alphaDistMin), ...
                        tc.betaDistMin  + rand * (tc.betaDistMax  - tc.betaDistMin), ...
                        tc.alphaTiltMin + rand * (tc.alphaTiltMax - tc.alphaTiltMin), ...
                        tc.betaTiltMin  + rand * (tc.betaTiltMax  - tc.betaTiltMin));
                    newAgent = agent;
                    newAgent = newAgent.initialize(candidatePos, geom, tc.sensor, tc.commsRanges(ii), tc.maxIter, tc.initialStepSize, tc.initialMaxAngleStepSize);

                    % Domain / obstacle / agent collision checks
                    violation = false;
                    for jj = 1:size(newAgent.collisionGeometry.vertices, 1)
                        if ~tc.domain.contains(newAgent.collisionGeometry.vertices(jj, 1:3))
                            violation = true; break;
                        end
                    end
                    if violation, continue; end
                    for kk = 1:size(tc.obstacles, 1)
                        if geometryIntersects(tc.obstacles{kk}, newAgent.collisionGeometry)
                            violation = true; break;
                        end
                    end
                    if violation, continue; end
                    for kk = 1:(ii - 1)
                        if geometryIntersects(tc.agents{kk}.collisionGeometry, newAgent.collisionGeometry)
                            violation = true; break;
                        end
                    end
                    if newAgent.pos(3) - newAgent.collisionGeometry.radius <= tc.minAlt
                        violation = true;
                    end
                    if violation, continue; end

                    initInvalid = false;
                    tc.agents{ii} = newAgent;
                end
            end

            % Initialize first sim (no plots / video)
            sim1 = miSim;
            sim1 = sim1.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, ...
                tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, false, false, ...
                tc.useDoubleIntegrator, tc.dampingCoeff, tc.useFixedTopology, tc.optimizeSensorPointing);

            % Write inits and build file path
            sim1.writeInits();
            initsFile = fullfile(matlab.project.rootProject().RootFolder, "sandbox", ...
                strcat(sim1.artifactName, "_miSimInits.mat"));

            % Load via initializeFromInits
            sim2 = miSim;
            sim2 = sim2.initializeFromInits(initsFile);

            % Assertions
            tc.assertEqual(size(sim2.agents, 1), size(sim1.agents, 1));
            tc.assertEqual(sim2.maxIter, sim1.maxIter);
            tc.assertEqual(sim2.barrierGain, sim1.barrierGain);

            % Cleanup
            delete(initsFile);
        end
    end

    methods
        function c = obstacleCollisionCheck(~, obstacles, obstacle)
            % Check if the obstacle intersects with any other obstacles
            c = false;
            for ii = 1:size(obstacles, 1)
                if geometryIntersects(obstacles{ii}, obstacle)
                    c = true;
                end
            end
        end
    end
end