sending starting positions to agents (not verified on AERPAW yet)

aerpaw/controller.m

@@ -3,17 +3,37 @@ arguments (Input)
     numClients (1, 1) int32;
 end
 
-coder.extrinsic('disp');
+coder.extrinsic('disp', 'readmatrix');
+
+% Maximum clients supported
+MAX_CLIENTS = 4;
+
+% Allocate targets array (MAX_CLIENTS x 3)
+targets = zeros(MAX_CLIENTS, 3);
+
+% Load targets from file
+if coder.target('MATLAB')
+    disp('Loading targets from file (simulation)...');
+    targetsLoaded = readmatrix('targets.txt');
+    numTargets = min(size(targetsLoaded, 1), numClients);
+    targets(1:numTargets, :) = targetsLoaded(1:numTargets, :);
+    disp(['Loaded ', num2str(numTargets), ' targets']);
+else
+    coder.cinclude('controller_impl.h');
+    % Define filename as null-terminated character array for C compatibility
+    filename = ['targets.txt', char(0)];
+    % loadTargets fills targets array (row-major: x1,y1,z1,x2,y2,z2,...)
+    coder.ceval('loadTargets', coder.ref(filename), ...
+                coder.ref(targets), int32(MAX_CLIENTS));
+end
 
 % Initialize server
 if coder.target('MATLAB')
     disp('Initializing server (simulation)...');
 else
-    coder.cinclude('controller_impl.h');
     coder.ceval('initServer');
 end
 
-
 % Accept clients
 for i = 1:numClients
     if coder.target('MATLAB')
@@ -23,33 +43,60 @@ for i = 1:numClients
     end
 end
 
-
-% Send messages to clients
+% Send target coordinates to each client
 for i = 1:numClients
+    % Get target for this client (1x3 array)
+    target = targets(i, :);
+
     if coder.target('MATLAB')
-        disp(['Sending message to client ', num2str(i)]);
+        disp(['Sending TARGET to client ', num2str(i), ': ', ...
+              num2str(target(1)), ',', num2str(target(2)), ',', num2str(target(3))]);
     else
-        coder.ceval('sendMessage', int32(i));
+        coder.ceval('sendTarget', int32(i), coder.ref(target));
     end
 end
 
-% Receive acknowledgements
-acksReceived = zeros(1, numClients, 'int32');
+% Receive TARGET acknowledgments
+targetAcks = zeros(1, numClients, 'int32');
 for i = 1:numClients
     if coder.target('MATLAB')
-        disp(['Receiving ACK from client ', num2str(i)]);
-        acksReceived(i) = 1;  % Simulate successful ACK
+        disp(['Waiting for ACK:TARGET from client ', num2str(i)]);
+        targetAcks(i) = 1;  % Simulate successful ACK
     else
-        acksReceived(i) = coder.ceval('receiveAck', int32(i));
+        targetAcks(i) = coder.ceval('receiveTargetAck', int32(i));
     end
 end
 
-
-% Digest ACKs
+% Check all ACKs received
 if coder.target('MATLAB')
-    disp(['All ACKs received: ', num2str(acksReceived)]);
+    disp(['Target ACKs received: ', num2str(targetAcks)]);
 end
 
+% Wait for READY signals (UAVs have reached their targets)
+readySignals = zeros(1, numClients, 'int32');
+for i = 1:numClients
+    if coder.target('MATLAB')
+        disp(['Waiting for READY from client ', num2str(i)]);
+        readySignals(i) = 1;  % Simulate READY
+    else
+        readySignals(i) = coder.ceval('waitForReady', int32(i));
+    end
+end
+
+% Check all READY signals received
+if coder.target('MATLAB')
+    disp(['Ready signals received: ', num2str(readySignals)]);
+    disp('All UAVs at target positions.');
+end
+
+% Send COMPLETE to all clients before closing
+for i = 1:numClients
+    if coder.target('MATLAB')
+        disp(['Sending COMPLETE to client ', num2str(i)]);
+    else
+        coder.ceval('sendFinished', int32(i));
+    end
+end
 
 % Close server
 if ~coder.target('MATLAB')