diff --git a/utils/analysis/findwingphase.m b/utils/analysis/findwingphase.m
index 121631cf7f30eb843ce24a84e0f50657dd5354ef..f0c8c9803ee4fe5ddbbe02448a9fdec6b3be595a 100644
--- a/utils/analysis/findwingphase.m
+++ b/utils/analysis/findwingphase.m
@@ -1,4 +1,4 @@
-function idx = findwingphase(ResData, phase)
+function idx = findwingphase(ResData, phaseDeg)
     % FINDWINDPHASE Return the indexes where the wings are at a certain phase
     %   Todo
 
@@ -9,13 +9,26 @@ function idx = findwingphase(ResData, phase)
     % MIT License
     %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
+    % Constants and defautls
+    PHASE_TOL = 5e-2; % Tolerance on phase
+    DEBUG = true;
+
+    % Abbreviations
+    sampling = 1 / diff(ResData.front.arduShort(1:2, 1));
+
     % Find flapping frequencies and period from data
     trueFreq(1) = getmainfrequency(ResData.front.arduShort(:, 1), ResData.front.arduShort(:, 2));
     trueFreq(2) = getmainfrequency(ResData.aft.arduShort(:, 1), ResData.aft.arduShort(:, 2));
+    period = 1 ./ [trueFreq(1), trueFreq(1)]; % Signal perdio, [s]
+    nPeriodIdx = period * sampling; % Number of indexes in a period
 
     % Determine period offset that match phase
+    % TODO: Some sort of average instead of based on first wing only
+    periodOffsetIdx = round((phaseDeg / 360) * nPeriodIdx(1));
 
-    % Find peaks in the wing angles
+    syncedPeaksLocs = findsyncedpeaks(ResData.front.arduShort(:, 2), ...
+                                      ResData.aft.arduShort(:, 2), ...
+                                      nPeriodIdx(1), periodOffsetIdx, PHASE_TOL);
 
     % Find indexes with proper phase difference
 
@@ -30,8 +43,7 @@ function trueFreq = getmainfrequency(time, signal)
     DEBUG = false;
 
     % Signal parameters
-    dt = diff(time);
-    sampling = 1 / dt(1);
+    sampling = 1 / abs(diff(time(1:2)));
     if mod(length(signal), 2) ~= 0
         signal = signal(1:end - 1);
     end
@@ -58,3 +70,46 @@ function trueFreq = getmainfrequency(time, signal)
     end
 
 end
+
+function peakLocs = findallpeaks(signal)
+    % FINDANGLEPEAKS Find peaks in the angle signal
+
+    maxAngle = max(signal);
+
+    [~, peakLocs] = findpeaks(signal, 'MinPeakHeight', maxAngle / 2);
+
+end
+
+function peakLocs = findsyncedpeaks(signal1, signal2, nPeriodIdx, offset, tol)
+    % FINDANGLEPEAKS Find peaks in the angle signal
+
+    % Constants and defaults
+    DEBUG = true;
+
+    % Find peaks in the wing angles
+    peakLocs1 = findallpeaks(signal1);
+    peakLocs2 = findallpeaks(signal2);
+    npeaks = min(length(peakLocs1), length(peakLocs2));
+
+    % Trim to same number of peaks
+    peakLocs1 = peakLocs1(1:npeaks);
+    peakLocs2 = peakLocs2(1:npeaks);
+
+    % Peaks where offset is good
+    diffPeakVals = mod(peakLocs1 - peakLocs2, nPeriodIdx) - offset; % Difference in peaks %FIXME
+    peakLocs1 = peakLocs1(abs(diffPeakVals) <= tol * nPeriodIdx); % FIXME
+
+    if DEBUG
+        figure;
+        hold on;
+        plot(signal1);
+        plot(signal2);
+        for i = 1:length(peakLocs1)
+            plot([peakLocs1(i), peakLocs1(i)], ylim, '-k', 'Linewidth', 1.2);
+        end
+        hold off;
+        grid on;
+        legend('Front', 'Aft');
+    end
+
+end