codegen fixes, bug fixes, gets running on testbed environment

@miSim/lesserNeighbor.m

@@ -9,42 +9,48 @@ function obj = lesserNeighbor(obj)
     % initialize solution with self-connections only
     constraintAdjacencyMatrix = logical(eye(size(obj.agents, 1)));
 
-    for ii = 1:size(obj.agents, 1)
+    nAgents = size(obj.agents, 1);
+    for ii = 1:nAgents
         % Find lesser neighbors of each agent
         % Lesser neighbors of ii are jj < ii in range of ii
-        lesserNeighbors = [];
+        lnBuf = zeros(1, nAgents);
+        lnCount = 0;
         for jj = 1:(ii - 1)
             if obj.adjacency(ii, jj)
-                lesserNeighbors = [lesserNeighbors, jj];
+                lnCount = lnCount + 1;
+                lnBuf(lnCount) = jj;
             end
         end
-        obj.agents{ii}.lesserNeighbors = lesserNeighbors;
-        
+        obj.agents{ii}.lesserNeighbors = lnBuf(1:lnCount);
+
         % Early exit for isolated agents
-        if isempty(obj.agents{ii}.lesserNeighbors)
+        if lnCount == 0
             continue
         end
 
         % Focus on subgraph defined by lesser neighbors
         subgraphAdjacency = obj.adjacency(obj.agents{ii}.lesserNeighbors, obj.agents{ii}.lesserNeighbors);
 
-        % Find connected components in each agent's subgraph
-        % TODO: rewrite this using matlab "conncomp" function?
-        visited = false(size(subgraphAdjacency, 1), 1);
-        components = {};
-        for jj = 1:size(subgraphAdjacency, 1)
+        % Find connected components; store only the max global index per
+        % component (the only value needed below) to avoid a cell array
+        visited = false(1, lnCount);
+        maxInComponent = zeros(1, lnCount);
+        numComponents = 0;
+        for jj = 1:lnCount
             if ~visited(jj)
                 reachable = bfs(subgraphAdjacency, jj);
                 visited(reachable) = true;
-                components{end+1} = obj.agents{ii}.lesserNeighbors(reachable);
+                numComponents = numComponents + 1;
+                maxInComponent(numComponents) = max(obj.agents{ii}.lesserNeighbors(reachable));
             end
         end
 
         % Connect to the greatest index in each connected component in the
         % lesser neighborhood of this agent
-        for jj = 1:size(components, 2)
-            constraintAdjacencyMatrix(ii, max(components{jj})) = true;
-            constraintAdjacencyMatrix(max(components{jj}), ii) = true;
+        for jj = 1:numComponents
+            maxIdx = maxInComponent(jj);
+            constraintAdjacencyMatrix(ii, maxIdx) = true;
+            constraintAdjacencyMatrix(maxIdx, ii) = true;
         end
     end
     obj.constraintAdjacencyMatrix = constraintAdjacencyMatrix | constraintAdjacencyMatrix';
@@ -53,24 +59,34 @@ end
 function cComp = bfs(subgraphAdjacency, startIdx)
     n = size(subgraphAdjacency, 1);
     visited = false(1, n);
-    queue = startIdx;
-    cComp = startIdx;
+
+    % Pre-allocated queue and component buffer with head/tail pointers
+    % to avoid element deletion and dynamic array growth
+    queue    = zeros(1, n);
+    cCompBuf = zeros(1, n);
+    qHead = 1;
+    qTail = 2;
+    queue(1)    = startIdx;
+    cCompBuf(1) = startIdx;
+    cSize = 1;
     visited(startIdx) = true;
-    
-    while ~isempty(queue)
-        current = queue(1);
-        queue(1) = [];
-        
+
+    while qHead < qTail
+        current = queue(qHead);
+        qHead = qHead + 1;
+
         % Find all neighbors of current node in the subgraph
         neighbors = find(subgraphAdjacency(current, :));
-        
-        for neighbor = neighbors
+        for kk = 1:numel(neighbors)
+            neighbor = neighbors(kk);
             if ~visited(neighbor)
                 visited(neighbor) = true;
-                cComp = [cComp, neighbor];
-                queue = [queue, neighbor];
+                cCompBuf(cSize + 1) = neighbor;
+                cSize = cSize + 1;
+                queue(qTail) = neighbor;
+                qTail = qTail + 1;
             end
         end
     end
-    cComp = sort(cComp);
+    cComp = sort(cCompBuf(1:cSize));
 end