From fde33154fe0face7ccb9cedee92f984b619b66e1 Mon Sep 17 00:00:00 2001
From: lolco <bjorndijkstra2005@gmail.com>
Date: Wed, 3 Dec 2025 14:45:16 +0100
Subject: [PATCH] new version

---
 student_code/my_baby.py | 163 ++++++++++++++++++++++++----------------
 1 file changed, 98 insertions(+), 65 deletions(-)

diff --git a/student_code/my_baby.py b/student_code/my_baby.py
index c7f3f95..4e4e2de 100644
--- a/student_code/my_baby.py
+++ b/student_code/my_baby.py
@@ -1,6 +1,7 @@
 import numpy as np                          # For convolution function
 import matplotlib.pyplot as plt             # For plotting purposes
 import scipy
+from fontTools.misc.plistlib import end_true
 # import samplerate
 
 from scipy import signal
@@ -17,63 +18,85 @@ from scipy.fft import fft, ifft             # For fft and ifft
 
 # ch3 function
 def ch3(x,y,Lhat,epsi):
-
-    # your code here (read the above text carefully for all the steps):
-    N = max(len(x), len(y)) + 50
-    x = np.concatenate((x, np.zeros(N - len(x)))) # Zero padding
-    y = np.concatenate((y, np.zeros(N - len(y))))
+    N = 2**int(np.ceil(np.log2(len(x)+len(y))))
+    x = np.pad(x, (0,N - len(x))) # Zero padding
+    y = np.pad(y, (0,N - len(y))) # Zero padding
 
     X = fft(x) # FFTs to find X[k] and Y[k]
     Y = fft(y)
 
-    H = Y / X # Computation of H[k]
-
-    threshold = epsi * max(np.absolute(X))
-    ii = np.absolute(X) < threshold
-
-    H[ii] = 0
+    H = (Y*np.conj(X))/(np.abs(X)**2+epsi)
 
     h = np.real(ifft(H)) # IFFT to find h[n]
-    h[np.abs(h) < 1e-12] = 0
-    # Truncation to length Lhat (optional and actually not recommended before you inspect the entire h)
-    h = h[0:Lhat]
-
+    h = np.fft.fftshift(h)
     return h
 
-def signal_Crop(y1):
-   scale = 300
-   pk = np.argmax(y1)
-   start = int(max(pk - scale,0))
-   end = int(pk + scale)
-   return start, end
+def peak_ref(y):
+  N, C = y.shape
+  peaks = np.zeros(C, dtype=int)
+  ref_ch = 4
+  ref_sig = np.abs(y[:,ref_ch])
+  ref_pk, _ = find_peaks(ref_sig, height= 0.5*np.max(ref_sig))
+
+  if len(ref_pk) == 0:
+      ref_peak = np.argmax(ref_sig)
+  else:
+      ref_peak = ref_pk[100]
+
+  peaks[ref_ch] = ref_peak
+
+  for ch in range(C):
+      if ch == ref_ch:
+          continue
+      sig = np.abs(y[:,ch])
+
+      start = max(0, ref_peak - 1500)
+      end = min(N, ref_peak + 1500)
+
+      local = sig[start:end]
+      pk, _ = find_peaks(local, height= 0.9*np.max(sig))
+
+      if len(pk) == 0:
+          local_peak = np.argmax(local)
+      else:
+          local_peak = np.argmax(local)
+      peaks[ch] = start + local_peak
+  return peaks
+
 
 def sig_comp(h1,Fs):
-  h1_peak, _ = find_peaks(h1, height = np.max(h1)*0.99)
+  center = len(h1) // 2
+  h1_peak = np.argmax(np.abs(h1))
 
-  peak_1 = h1_peak[0]
-
-  sample_range = np.array([peak_1])
+  sample_range = h1_peak - center
   time_dif = sample_range / Fs
-  distance = abs(time_dif) * 34300
+  distance = time_dif * 34300 #cm
+  return distance
 
-  return distance, peak_1
+def distance_calc(y1,y2,epsi,Fs, peak1, peak2):
 
-def distance_calc(y1,y2,epsi,Fs):
-  start1, end1 = signal_Crop(y1)
-  start2, end2 = signal_Crop(y2)
+  min_p = min(peak1, peak2)
+  max_p = max(peak1, peak2)
 
-  start_true= min(start1, start2)
-  end_true = max(end1, end2)
+  start = max(min_p - 800, 0)
+  end = min(max_p + 800, len(y1))
 
-  y1_crop = y1[start_true:end_true]
-  y2_crop = y2[start_true:end_true]
-
-  Lhat = len(y1_crop)
+  y1_crop = y1[start:end]
+  y2_crop = y2[start:end]
 
+  plt.figure(figsize=(10, 4))
+  plt.plot(y1_crop, label='Channel 1 (cropped)')
+  plt.plot(y2_crop, label='Channel 2 (cropped)')
+  plt.title(f'Cropped signals for TDOA calculation\nPeaks: {peak1}, {peak2}')
+  plt.xlabel('Samples (cropped)')
+  plt.ylabel('Amplitude')
+  plt.grid(True)
+  plt.legend()
+  plt.show()
+  Lhat = 501
   h1 = ch3(y1_crop, y2_crop, Lhat, epsi)
-  distance, peak_1 = sig_comp(h1, Fs)
-  print(distance)
-  return distance[0]
+  distance = sig_comp(h1, Fs)
+  return distance
 
 def location_calc(mic_coords, ref_index, distances, start_point = [230,230,0]):
   ref = mic_coords[ref_index]
@@ -84,44 +107,40 @@ def location_calc(mic_coords, ref_index, distances, start_point = [230,230,0]):
     res = []
     for i, idx in enumerate(other_indices):
       mic = mic_coords[idx]
-      diff = np.linalg.norm(S-mic) - np.linalg.norm(S-ref) - distances[i]
+      diff = (np.linalg.norm(S-mic) - np.linalg.norm(S-ref)) - distances[i]
       res.append(diff)
     return res
 
-  sol = least_squares(residuals, start_point)
+  sol = least_squares(residuals, start_point, bounds = ([0,0,-1],[460,460,1]))
   return sol.x
 
 def location(y,Fs):
-  y1 = y[0]
-  y2 = y[1]
-  y3 = y[2]
-  y4 = y[3]
-  y5 = y[4]
-  epsi = 0.01
-  d1 = distance_calc(y5,y1,epsi,Fs)
-  d2 = distance_calc(y5,y2,epsi,Fs)
-  d3 = distance_calc(y5,y3,epsi,Fs)
-  d4 = distance_calc(y5,y4,epsi,Fs)
+  peaks = peak_ref(y)
+  y1, y2, y3, y4, y5 = y.T
+  p1, p2, p3, p4, p5 = peaks
+
+  epsi = 0.0001
+  d1 = distance_calc(y5,y1,epsi,Fs, p5, p1)
+  d2 = distance_calc(y5,y2,epsi,Fs, p5, p2)
+  d3 = distance_calc(y5,y3,epsi,Fs, p5, p3)
+  d4 = distance_calc(y5,y4,epsi,Fs, p5,p4)
 
   mic_coords = np.array([
-   [0,0,25], # mic 1
-   [0,460,25], # mic 2
-   [460,460,25], # mic 3
-   [460,0,25], # mic 4
-   [0,230,55] # mic 5
+   [0,0,25], # mic 1 cm
+   [0,460,25], # mic 2 cm
+   [460,460,25], # mic 3 cm
+   [460,0,25], # mic 4 cm
+   [0,230,55] # mic 5 cm
   ])
 
   ref_index = 4 #used to makes mic 5 the ref
   distances = np.array([d1,d2,d3,d4])
+  print("Distances (d1, d2, d3, d4):", distances)
+  print("Peak samples:   ", peaks)
 
   source_pos = location_calc(mic_coords, ref_index, distances)
   return source_pos
 
-  ref_index = 4 #used to makes mic 5 the ref
-  distances = np.array([d1,d2,d3,d4])
-
-  source_pos = location_calc(mic_coords, ref_index, distances)
-  return source_pos
 
 if __name__ == "__main__":
     # Coordinates of the recordings
@@ -137,8 +156,22 @@ if __name__ == "__main__":
         real_y = record_y[i]
         filenames.append(f"../files/Student Recordings/record_x{real_x}_y{real_y}.wav")
 
-    # Load the first recording
-    Fs, recording = wavfile.read(filenames[0])
-    print(location(recording, Fs))
-    print(Fs)
+    for i, file in enumerate(filenames):
+        Fs, recording = wavfile.read(file)
+        recording = recording.astype(np.float64) / np.max(np.abs(recording))
+        print(f"\nRecording {i+1}: {file}")
+        source_pos = location(recording, Fs)
+        print("Estimated source position:", source_pos)
 
+        num_channels = recording.shape[1]
+        fig, axs = plt.subplots(num_channels, 1, figsize=(10, 2*num_channels), sharex=True)
+
+        for ch in range(num_channels):
+            axs[ch].plot(recording[:, ch])
+            axs[ch].set_ylabel(f"Ch {ch+1}")
+            axs[ch].grid(True)
+
+        axs[-1].set_xlabel("Samples")
+        plt.suptitle(f"Waveforms of all channels - Recording {i+1}")
+        plt.tight_layout(rect=[0, 0.03, 1, 0.95])
+        plt.show()
\ No newline at end of file