diff --git a/Assets/Scripts/Services/EphysLinkService.cs b/Assets/Scripts/Services/EphysLinkService.cs index df6a84e9..4774733c 100644 --- a/Assets/Scripts/Services/EphysLinkService.cs +++ b/Assets/Scripts/Services/EphysLinkService.cs @@ -33,7 +33,7 @@ public class EphysLinkService private readonly Vector2 _pitchRange = new(0, 90); // Visualization loop interval (ms) - private const int VISUALIZATION_UPDATE_INTERVAL_MS = 10; // about 60 Hz + private const int VISUALIZATION_UPDATE_INTERVAL_MS = 100; // about 60 Hz #endregion @@ -61,6 +61,17 @@ public class EphysLinkService private readonly HashSet _runningDemoLoops = new(); private const float DEMO_SPEED = 0.5f; // mm/s + + // Reusable list to avoid allocations in UpdateVisualizationProbePosition + private readonly List<( + string Name, + Vector3 SurfaceAPMLDV, + float Depth, + Vector3 ForwardT, + Vector3 Angles, + Vector2 PitchRange + )> _visualizationUpdateRequests = new(); + #endregion public EphysLinkService(StoreService storeService) @@ -80,8 +91,9 @@ public EphysLinkService(StoreService storeService) private async void OnSceneStateChanged(SceneState sceneState) { // Handle demo loop state changes. - foreach (var manipulatorState in sceneState.Manipulators) + for (int i = 0; i < sceneState.Manipulators.Count; i++) { + var manipulatorState = sceneState.Manipulators[i]; var isRunning = _runningDemoLoops.Contains(manipulatorState.Id); switch (manipulatorState.IsDemoRunning) @@ -622,24 +634,20 @@ private async Task VisualizationUpdateLoop(CancellationToken token) private async Task UpdateVisualizationProbePosition(SceneState sceneState) { - List<( - string Name, - Vector3 SurfaceAPMLDV, - float Depth, - Vector3 ForwardT, - Vector3 Angles, - Vector2 PitchRange - )> requests = new(); - - foreach ( - var manipulatorState in sceneState.Manipulators.Where(state => - !string.IsNullOrEmpty(state.VisualizationProbeName) - ) - ) + // Clear and reuse the list to avoid allocations + _visualizationUpdateRequests.Clear(); + + // Iterate through manipulators directly to avoid LINQ allocations + foreach (var manipulatorState in sceneState.Manipulators) { + // Skip if no visualization probe name + if (string.IsNullOrEmpty(manipulatorState.VisualizationProbeName)) + continue; + // Skip if the probe or manager couldn't be found. + var state = manipulatorState; var visualizationProbeManager = ProbeManager.Instances.FirstOrDefault(manager => - manager.name == manipulatorState.VisualizationProbeName + manager.name == state.VisualizationProbeName ); if ( sceneState.Probes.FirstOrDefault(state => @@ -647,7 +655,7 @@ var manipulatorState in sceneState.Manipulators.Where(state => ) == null || visualizationProbeManager == null ) - return; + continue; // Get the current position of the manipulator. var positionResponse = await GetPosition(manipulatorState.Id); @@ -719,7 +727,7 @@ var manipulatorState in sceneState.Manipulators.Where(state => { // Set the probe position in the store. case 3: - requests.Add( + _visualizationUpdateRequests.Add( ( manipulatorState.VisualizationProbeName, transformedAPMLDV, @@ -731,7 +739,7 @@ var manipulatorState in sceneState.Manipulators.Where(state => ); break; case 4: - requests.Add( + _visualizationUpdateRequests.Add( ( manipulatorState.VisualizationProbeName, transformedAPMLDV, @@ -750,10 +758,10 @@ var manipulatorState in sceneState.Manipulators.Where(state => } // Dispatch all position updates in one go (if any). - if (requests.Any()) + if (_visualizationUpdateRequests.Count > 0) _storeService.Store.Dispatch( SceneActions.BULK_SET_PROBE_POSITION_AND_ANGLES_BY, - requests + _visualizationUpdateRequests ); } @@ -869,7 +877,7 @@ private async Task RunDemoLoop(string manipulatorId) { while (true) { - // Get current manipulator state. + // Get current manipulator state once at the start of each iteration. var sceneState = _storeService.Store.GetState(SliceNames.SCENE_SLICE); var manipulatorState = sceneState.Manipulators.FirstOrDefault(m => m.Id == manipulatorId @@ -889,10 +897,14 @@ private async Task RunDemoLoop(string manipulatorId) return; } + // Cache coordinates to avoid repeated state retrievals + var homeCoordinate = manipulatorState.DemoHomeCoordinate; + var targetCoordinate = manipulatorState.DemoTargetCoordinate; + // Step 1: Move to home position. var homeRequest = new SetPositionRequest( manipulatorId, - manipulatorState.DemoHomeCoordinate, + homeCoordinate, DEMO_SPEED ); var homeResponse = await SetPosition(homeRequest); @@ -904,12 +916,8 @@ private async Task RunDemoLoop(string manipulatorId) return; } - // Refresh state and check if demo is still running. - sceneState = _storeService.Store.GetState(SliceNames.SCENE_SLICE); - manipulatorState = sceneState.Manipulators.FirstOrDefault(m => - m.Id == manipulatorId - ); - if (manipulatorState is not { IsDemoRunning: true }) + // Check if demo is still running after movement. + if (!CheckDemoStillRunning(manipulatorId)) { _runningDemoLoops.Remove(manipulatorId); return; @@ -917,10 +925,10 @@ private async Task RunDemoLoop(string manipulatorId) // Step 2: Move to target position on X and Z axes only (keep Y and W from home). var intermediatePosition = new Vector4( - manipulatorState.DemoTargetCoordinate.x, - manipulatorState.DemoHomeCoordinate.y, - manipulatorState.DemoTargetCoordinate.z, - manipulatorState.DemoHomeCoordinate.w + targetCoordinate.x, + homeCoordinate.y, + targetCoordinate.z, + homeCoordinate.w ); var intermediateRequest = new SetPositionRequest( manipulatorId, @@ -936,12 +944,8 @@ private async Task RunDemoLoop(string manipulatorId) return; } - // Refresh state and check if demo is still running. - sceneState = _storeService.Store.GetState(SliceNames.SCENE_SLICE); - manipulatorState = sceneState.Manipulators.FirstOrDefault(m => - m.Id == manipulatorId - ); - if (manipulatorState is not { IsDemoRunning: true }) + // Check if demo is still running after movement. + if (!CheckDemoStillRunning(manipulatorId)) { _runningDemoLoops.Remove(manipulatorId); return; @@ -950,7 +954,7 @@ private async Task RunDemoLoop(string manipulatorId) // Step 3: Move to target position on Y and W axes only (complete movement to target). var targetRequest = new SetPositionRequest( manipulatorId, - manipulatorState.DemoTargetCoordinate, + targetCoordinate, DEMO_SPEED ); var targetResponse = await SetPosition(targetRequest); @@ -962,12 +966,8 @@ private async Task RunDemoLoop(string manipulatorId) return; } - // Refresh state and check if demo is still running. - sceneState = _storeService.Store.GetState(SliceNames.SCENE_SLICE); - manipulatorState = sceneState.Manipulators.FirstOrDefault(m => - m.Id == manipulatorId - ); - if (manipulatorState is not { IsDemoRunning: true }) + // Check if demo is still running after movement. + if (!CheckDemoStillRunning(manipulatorId)) { _runningDemoLoops.Remove(manipulatorId); return; @@ -975,10 +975,10 @@ private async Task RunDemoLoop(string manipulatorId) // Step 4: Move back to intermediate position (X,Z only, keeping Y,W from home). var returnToIntermediatePosition = new Vector4( - manipulatorState.DemoTargetCoordinate.x, - manipulatorState.DemoHomeCoordinate.y, - manipulatorState.DemoTargetCoordinate.z, - manipulatorState.DemoHomeCoordinate.w + targetCoordinate.x, + homeCoordinate.y, + targetCoordinate.z, + homeCoordinate.w ); var returnToIntermediateRequest = new SetPositionRequest( manipulatorId, @@ -994,22 +994,30 @@ private async Task RunDemoLoop(string manipulatorId) return; } - // Refresh state and check if demo is still running before next iteration. - sceneState = _storeService.Store.GetState(SliceNames.SCENE_SLICE); - manipulatorState = sceneState.Manipulators.FirstOrDefault(m => - m.Id == manipulatorId - ); - if (manipulatorState is { IsDemoRunning: true }) + // Check if demo is still running before next iteration. + if (!CheckDemoStillRunning(manipulatorId)) { - // Loop continues to next iteration (back to home). - continue; + _runningDemoLoops.Remove(manipulatorId); + return; } - - _runningDemoLoops.Remove(manipulatorId); - return; } } + /// + /// Helper method to check if demo is still running for a manipulator. + /// Reduces code duplication and state retrieval overhead. + /// + /// ID of the manipulator to check. + /// True if demo is still running, false otherwise. + private bool CheckDemoStillRunning(string manipulatorId) + { + var sceneState = _storeService.Store.GetState(SliceNames.SCENE_SLICE); + var manipulatorState = sceneState.Manipulators.FirstOrDefault(m => + m.Id == manipulatorId + ); + return manipulatorState is { IsDemoRunning: true }; + } + #endregion } }