diff --git a/openptv_python/imgcoord.py b/openptv_python/imgcoord.py
index b4914ab..6ddbb70 100644
--- a/openptv_python/imgcoord.py
+++ b/openptv_python/imgcoord.py
@@ -37,16 +37,26 @@ def flat_image_coord(
         cal.ext_par, mm, cal.glass_par, orig_pos, cal_t.ext_par
     )
 
+    # print(f"pos_t {pos_t}")
+    # print(f"cross_p {cross_p}")
+    # print(f"cros_c {cross_c}")
+
     x_t, y_t = multimed_nlay(cal_t, mm, pos_t)
+    # print(f"x_t {x_t}, y_t {y_t}")
+
     pos_t = vec_set(x_t, y_t, pos_t[2])
     pos = back_trans_point(pos_t, mm, cal.glass_par, cross_p, cross_c)
 
+    # print(f"pos {pos}")
+
     deno = (
         cal.ext_par.dm[0][2] * (pos[0] - cal.ext_par.x0)
         + cal.ext_par.dm[1][2] * (pos[1] - cal.ext_par.y0)
         + cal.ext_par.dm[2][2] * (pos[2] - cal.ext_par.z0)
     )
 
+    # print(f"deno {deno}")
+
     if deno == 0:
         deno = 1
 
@@ -70,6 +80,8 @@ def flat_image_coord(
         / deno
     )
 
+    # print(f"x {x}, y {y}")
+
     return x, y
 
 
@@ -96,6 +108,8 @@ def img_coord(
     # Distort the metric coordinates using the Brown distortion model
     x, y = flat_to_dist(x, y, cal)
 
+    # print("f flat_to_dist: x = {x}, y = {y}")
+
     return x, y
 
 
diff --git a/openptv_python/parameters.py b/openptv_python/parameters.py
index dae4f47..458825d 100644
--- a/openptv_python/parameters.py
+++ b/openptv_python/parameters.py
@@ -147,8 +147,8 @@ class TrackPar:
     dvxmin: float = 0.0
     dvymax: float = 0.0
     dvymin: float = 0.0
-    dvz_max: float = 0.0
-    dvz_min: float = 0.0
+    dvzmax: float = 0.0
+    dvzmin: float = 0.0
     dsumg: float = 0.0
     dn: float = 0.0
     dnx: float = 0.0
@@ -171,8 +171,8 @@ def from_file(self, filename: str):
                 self.dvxmax = float(fpp.readline().rstrip())
                 self.dvymin = float(fpp.readline().rstrip())
                 self.dvymax = float(fpp.readline().rstrip())
-                self.dvz_min = float(fpp.readline().rstrip())
-                self.dvz_max = float(fpp.readline().rstrip())
+                self.dvzmin = float(fpp.readline().rstrip())
+                self.dvzmax = float(fpp.readline().rstrip())
                 self.add = int(fpp.readline().rstrip())
         except IOError as exc:
             raise (f"Error reading tracking parameters from {filename}") from exc
@@ -195,11 +195,11 @@ def get_dvymax(self):
 
     def get_dvz_min(self):
         """Return the minimum velocity in z direction."""
-        return self.dvz_min
+        return self.dvzmin
 
     def get_dvz_max(self):
         """Return the minimum velocity in z direction."""
-        return self.dvz_max
+        return self.dvzmax
 
     def get_dangle(self):
         """Return the maximum angle."""
diff --git a/openptv_python/track.py b/openptv_python/track.py
index 30fff54..fe25611 100644
--- a/openptv_python/track.py
+++ b/openptv_python/track.py
@@ -149,7 +149,7 @@ def track_forward_start(tr: TrackingRun):
     tr.fb.prev()
 
 
-def reset_foundpix_array(arr, arr_len, num_cams):
+def reset_foundpix_array(arr: List[Foundpix], arr_len: int, num_cams: int) -> None:
     """Set default values for foundpix objects in an array.
 
     Arguments:
@@ -168,7 +168,9 @@ def reset_foundpix_array(arr, arr_len, num_cams):
             arr[i].whichcam[cam] = 0
 
 
-def copy_foundpix_array(dest, src, arr_len, num_cams):
+def copy_foundpix_array(
+    dest: List[Foundpix], src: List[Foundpix], arr_len: int, num_cams: int
+) -> None:
     """copy_foundpix_array() copies foundpix objects from one array to another.
 
     Arguments:
@@ -285,7 +287,7 @@ def pos3d_in_bounds(pos, bounds):
     return (
         bounds.dvxmin < pos[0] < bounds.dvxmax
         and bounds.dvymin < pos[1] < bounds.dvymax
-        and bounds.dvz_min < pos[2] < bounds.dvz_max
+        and bounds.dvzmin < pos[2] < bounds.dvzmax
     )
 
 
@@ -475,39 +477,28 @@ def candsearch_in_pix_rest(
     return counter
 
 
-def searchquader(point, tpar, cpar, cal):
-    def vec_set(vec, x, y, z):
-        vec[0], vec[1], vec[2] = x, y, z
+def searchquader(
+    point: np.ndarray, tpar: TrackPar, cpar: ControlPar, cal: List[Calibration]
+) -> Tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]:
+    """Calculate the search volume in image space."""
+    mins = np.array([tpar.dvxmin, tpar.dvymin, tpar.dvzmin])
+    maxes = np.array([tpar.dvxmax, tpar.dvymax, tpar.dvzmax])
 
-    def vec_copy(dest, src):
-        dest[0], dest[1], dest[2] = src[0], src[1], src[2]
-
-    def point_to_pixel(pixel, point, cal, cpar):
-        pixel[0] = cal.camplane_u(cpar, point)
-        pixel[1] = cal.camplane_v(cpar, point)
-
-    def project_to_pixel(corners, point, mins, maxes, cal, cpar):
-        for pt in range(8):
-            vec_copy(corners[pt], point)
-            for dim in range(3):
-                if pt & 1 << dim:
-                    corners[pt][dim] += maxes[dim]
-                else:
-                    corners[pt][dim] += mins[dim]
-
-            point_to_pixel(corners[pt], corners[pt], cal, cpar)
-
-    xr, xl, yd, yu = np.zeros(4), np.zeros(4), np.zeros(4), np.zeros(4)
-    mins, maxes = np.zeros(3), np.zeros(3)
     quader = np.zeros((8, 3))
-    center = np.zeros(2)
-    corner = np.zeros(2)
-
-    vec_set(mins, tpar.dvxmin, tpar.dvymin, tpar.dvz_min)
-    vec_set(maxes, tpar.dvxmax, tpar.dvymax, tpar.dvz_max)
-
-    # 3D positions of search volume - eight corners of a box
-    project_to_pixel(quader, point, mins, maxes, cal[0], cpar)
+    xr = np.zeros(cpar.num_cams)
+    xl = np.zeros(cpar.num_cams)
+    yd = np.zeros(cpar.num_cams)
+    yu = np.zeros(cpar.num_cams)
+
+    for pt in range(8):
+        quader[pt] = point.copy()
+        # print(f" pt {pt} {quader[pt]}")
+        for dim in range(3):
+            if pt & (1 << dim):
+                quader[pt][dim] += maxes[dim]
+            else:
+                quader[pt][dim] += mins[dim]
+        # print(f" pt {pt} {quader[pt]}")
 
     # calculation of search area in each camera
     for i in range(cpar.num_cams):
@@ -518,11 +509,13 @@ def project_to_pixel(corners, point, mins, maxes, cal, cpar):
         yu[i] = cpar.imy
 
         # pixel position of a search center
-        point_to_pixel(center, point, cal[i], cpar)
+        center = point_to_pixel(point, cal[i], cpar)
+        # print(" center", center[0], center[1])
 
         # mark 4 corners of the search region in pixels
         for pt in range(8):
-            point_to_pixel(corner, quader[pt], cal[i], cpar)
+            corner = point_to_pixel(quader[pt], cal[i], cpar)
+            # print(" corner", corner[0], corner[1])
 
             if corner[0] < xl[i]:
                 xl[i] = corner[0]
@@ -542,22 +535,23 @@ def project_to_pixel(corners, point, mins, maxes, cal, cpar):
         if yd[i] > cpar.imy:
             yd[i] = cpar.imy
 
-        # eventually xr,xl,yd,yu are pixel distances relative to the point
+        # print(" xl", xl[i], " xr", xr[i], " yu", yu[i], " yd", yd[i])
+
+        # eventually xr, xl, yd, yu are pixel distances relative to the point
         xr[i] = xr[i] - center[0]
         xl[i] = center[0] - xl[i]
         yd[i] = yd[i] - center[1]
         yu[i] = center[1] - yu[i]
 
+        # print(" xl", xl[i], " xr", xr[i], " yu", yu[i], " yd", yd[i])
+
+    return xr, xl, yd, yu
 
-def sort_candidates_by_freq(item, num_cams):
-    class FoundPix:
-        def __init__(self, ftnr=0, whichcam=[0] * 4, freq=0):
-            self.ftnr = ftnr
-            self.whichcam = whichcam
-            self.freq = freq
 
+def sort_candidates_by_freq(item: Foundpix, num_cams: int):
+    """Sort candidates by frequency."""
     MAX_CANDS = 1000
-    foundpix = [FoundPix() for i in range(num_cams * MAX_CANDS)]
+    foundpix = [Foundpix() for i in range(num_cams * MAX_CANDS)]
     foundpix[: len(item)] = item
 
     different = 0
@@ -601,8 +595,8 @@ def __init__(self, ftnr=0, whichcam=[0] * 4, freq=0):
     return different
 
 
-def sort(a, b):
-    """Sorts a float array 'a' and an integer array 'b' both of length n.
+def sort(a: np.ndarray, b: np.ndarray) -> None:
+    """In-place sorts a float array 'a' and an integer array 'b' equal lengths.
 
     Arguments:
     ---------
@@ -613,11 +607,10 @@ def sort(a, b):
     -------
     Sorted arrays a and b.
     """
-    len(a)
     idx = np.argsort(a)
-    a = a[idx]
-    b = b[idx]
-    return a, b
+    a[...] = a[idx]
+    b[...] = b[idx]
+    # return a, b
 
 
 def point_to_pixel(point: np.ndarray, cal: Calibration, cpar: ControlPar) -> np.ndarray:
@@ -633,8 +626,14 @@ def point_to_pixel(point: np.ndarray, cal: Calibration, cpar: ControlPar) -> np.
     -------
     vec2d with pixel positions (x,y) in the camera.
     """
+    # print(f"point {point}")
+    # print(f"cal {cal}")
+    # print(f"cpar.mm {cpar.mm}")
+
     x, y = img_coord(point, cal, cpar.mm)
+    # print("img coord x, y", x, y)
     x, y = metric_to_pixel(x, y, cpar)
+    # print("metric to pixel x, y", x, y)
     return np.array([x, y])
 
 
diff --git a/openptv_python/tracking_run.py b/openptv_python/tracking_run.py
index 6865a58..8f1ed05 100644
--- a/openptv_python/tracking_run.py
+++ b/openptv_python/tracking_run.py
@@ -80,7 +80,7 @@ def tr_new(
     tr.lmax = np.norm(
         tpar.dvxmin - tpar.dvxmax,
         tpar.dvymin - tpar.dvymax,
-        tpar.dvz_min - tpar.dvz_max,
+        tpar.dvzmin - tpar.dvzmax,
     )
     volumedimension(
         vpar.X_lay[1],
diff --git a/openptv_python/trafo.py b/openptv_python/trafo.py
index 04b14f8..00a1bb5 100644
--- a/openptv_python/trafo.py
+++ b/openptv_python/trafo.py
@@ -1,4 +1,5 @@
 """Module for coordinate transformations."""
+from math import cos, sin, sqrt
 from typing import Tuple
 
 import numpy as np
@@ -80,14 +81,14 @@ def arr_metric_to_pixel(metric: np.ndarray, parameters: ControlPar) -> np.ndarra
     return pixel
 
 
-def distort_brown_affine(
-    x: np.float64, y: np.float64, ap: ap_52
-) -> Tuple[float, float]:
+def distort_brown_affine(x: float, y: float, ap: ap_52) -> Tuple[float, float]:
     """Distort a point using the Brown affine model."""
     if x == 0 and y == 0:
         return 0, 0
 
-    r = np.sqrt(x**2 + y**2)
+    # print(f"x {x}, y {y}")
+
+    r = sqrt(x**2 + y**2)
 
     x += (
         x * (ap.k1 * r**2 + ap.k2 * r**4 + ap.k3 * r**6)
@@ -99,8 +100,14 @@ def distort_brown_affine(
         + ap.p2 * (r**2 + 2 * y**2)
         + 2 * ap.p1 * x * y
     )
-    x1 = ap.scx * x - np.sin(ap.she) * y
-    y1 = np.cos(ap.she) * y
+
+    # print(f"x {x}, y {y}")
+    # print(f"ap.she {ap.she} ap.scx {ap.scx}")
+
+    x1 = ap.scx * x - sin(ap.she) * y
+    y1 = cos(ap.she) * y
+
+    # print(f"x1 {x1}, y1 {y1}")
 
     return x1, y1
 
@@ -219,10 +226,16 @@ def flat_to_dist(flat_x: float, flat_y: float, cal: Calibration) -> Tuple[float,
     Make coordinates relative to sensor center rather than primary point
     image coordinates, because distortion formula assumes it, [1] p.180.
     """
+    # print(f"flat_x {flat_x}, flat_y {flat_y}")
+    # print(f"cal.int {cal.int_par.xh}, {cal.int_par.yh}")
     flat_x += cal.int_par.xh
     flat_y += cal.int_par.yh
 
+    # print(f"flat_x {flat_x}, flat_y {flat_y}")
+
     dist_x, dist_y = distort_brown_affine(flat_x, flat_y, cal.added_par)
+    # print(f"dist_x {dist_x}, dist_y {dist_y}")
+
     return dist_x, dist_y
 
 
diff --git a/tests/test_parameters_bindings.py b/tests/test_parameters_bindings.py
index dcb64dd..3426155 100644
--- a/tests/test_parameters_bindings.py
+++ b/tests/test_parameters_bindings.py
@@ -116,7 +116,7 @@ class Test_TrackingParams(unittest.TestCase):
         typedef struct
     {
         double  dacc, dangle, dvxmax, dvxmin;
-        double dvymax, dvymin, dvz_max, dvz_min;
+        double dvymax, dvymin, dvzmax, dvzmin;
         int dsumg, dn, dnx, dny, add;
     } track
 
@@ -138,8 +138,8 @@ def setUp(self):
             dvxmax=4.4,
             dvymin=5.5,
             dvymax=6.6,
-            dvz_min=7.7,
-            dvz_max=8.8,
+            dvzmin=7.7,
+            dvzmax=8.8,
         )
 
     # Testing getters according to the values passed in setUp
@@ -152,8 +152,8 @@ def test_TrackingParams_getters(self):
         self.assertTrue(self.track_obj1.dvxmax == 4.4)
         self.assertTrue(self.track_obj1.dvymin == 5.5)
         self.assertTrue(self.track_obj1.dvymax == 6.6)
-        self.assertTrue(self.track_obj1.dvz_min == 7.7)
-        self.assertTrue(self.track_obj1.dvz_max == 8.8)
+        self.assertTrue(self.track_obj1.dvzmin == 7.7)
+        self.assertTrue(self.track_obj1.dvzmax == 8.8)
         self.assertTrue(self.track_obj1.add == 1)
 
     def test_TrackingParams_read_from_file(self):
diff --git a/tests/test_tracking.py b/tests/test_tracking.py
index 15dbf61..ee11fa5 100644
--- a/tests/test_tracking.py
+++ b/tests/test_tracking.py
@@ -13,22 +13,43 @@
 
 import numpy as np
 
+from openptv_python.calibration import Calibration, read_calibration
 from openptv_python.parameters import (
     ControlPar,
     TrackPar,
 )
 from openptv_python.track import (
+    Foundpix,
     angle_acc,
     candsearch_in_pix,
     candsearch_in_pix_rest,
+    copy_foundpix_array,
     pos3d_in_bounds,
     predict,
+    reset_foundpix_array,
     search_volume_center_moving,
+    searchquader,
+    sort,
 )
 from openptv_python.tracking_frame_buf import Target
 from openptv_python.vec_utils import vec_scalar_mul
 
 
+def read_all_calibration(num_cams: int = 4) -> list[Calibration]:
+    """Read all calibration files."""
+    ori_tmpl = "tests/testing_fodder/track/cal/cam%d.tif.ori"
+    added_tmpl = "tests/testing_fodder/track/cal/cam%d.tif.addpar"
+
+    calib = []
+
+    for cam in range(num_cams):
+        ori_name = ori_tmpl % (cam + 1)
+        added_name = added_tmpl % (cam + 1)
+        calib.append(read_calibration(ori_name, added_name))
+
+    return calib
+
+
 class TestPredict(unittest.TestCase):
     """Test the predict function."""
 
@@ -219,5 +240,130 @@ def test_candsearch_in_pix(self):
         )
 
 
+class TestSort(unittest.TestCase):
+    def test_sort(self):
+        test_array = np.array([1.0, 2200.2, 0.3, -0.8, 100.0], dtype=float)
+        ix_array = np.array([0, 5, 13, 2, 124], dtype=int)
+        len_array = 5
+
+        sort(test_array, ix_array)
+
+        self.assertTrue(
+            isclose(test_array[0], -0.8, rel_tol=1e-9),
+            f"Expected -0.8 but found { test_array[0] } ",
+        )
+        self.assertNotEqual(
+            ix_array[len_array - 1],
+            1,
+            f"Expected not to be 1 but found { ix_array[len_array - 1] }",
+        )
+
+    def test_copy_foundpix_array(self):
+        """Test the copy_foundpix_array function."""
+        src = [Foundpix(1, 1, [1, 0]), Foundpix(2, 5, [1, 1])]
+        arr_len = 2
+        num_cams = 2
+
+        dest = [Foundpix(0, 0, [0] * num_cams) for _ in range(arr_len)]
+
+        reset_foundpix_array(dest, len(dest), num_cams)
+
+        self.assertEqual(
+            dest[1].ftnr, -1, f"Expected dest[1].ftnr == -1 but found {dest[1].ftnr}"
+        )
+        self.assertEqual(
+            dest[0].freq, 0, f"Expected dest[0].freq == 0 but found {dest[0].freq}"
+        )
+        self.assertEqual(
+            dest[1].whichcam[0], 0, f"Expected 0 but found {dest[1].whichcam[0]}"
+        )
+
+        copy_foundpix_array(dest, src, arr_len, num_cams)
+
+        self.assertEqual(
+            dest[1].ftnr, 2, f"Expected dest[1].ftnr == 2 but found {dest[1].ftnr}"
+        )
+
+        # print("Destination foundpix array:")
+        # for i in range(arr_len):
+        #     print(f"ftnr = {dest[i].ftnr} freq = {dest[i].freq} whichcam = {dest[i].whichcam}")
+
+
+class TestSearchQuader(unittest.TestCase):
+    def setUp(self):
+        self.cpar = ControlPar()
+        self.cpar.from_file("tests/testing_fodder/track/parameters/ptv.par")
+        self.cpar.mm.n2[0] = 1.0
+        self.cpar.mm.n3 = 1.0
+
+        # self.calib = [None] * self.cpar.num_cams
+        self.calib = read_all_calibration(self.cpar.num_cams)
+
+    def test_searchquader(self):
+        """Test the searchquader function."""
+        point = np.array([185.5, 3.2, 203.9])
+
+        #  print(f"cpar = {self.cpar}")
+
+        tpar = TrackPar(
+            0.4, 120, 0.2, -0.2, 0.1, -0.1, 0.1, -0.1, 0.0, 0.0, 0.0, 0.0, 1
+        )
+        xr, xl, yd, yu = searchquader(point, tpar, self.cpar, self.calib)
+
+        # print(f"xr = {xr}, xl = {xl}, yd = {yd}, yu = {yu}")
+
+        self.assertTrue(
+            isclose(xr[0], 0.560048, rel_tol=1e-6),
+            f"Expected 0.560048 but found {xr[0]}",
+        )
+        self.assertTrue(
+            isclose(yu[1], 0.437303, rel_tol=1e-6),
+            f"Expected 0.437303 but found {yu[1]}",
+        )
+
+        # Let's test with just one camera to check borders
+        self.cpar.num_cams = 1
+        tpar1 = TrackPar(
+            0.4, 120, 0.0, -0.0, 0.0, -0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 0.0, 1
+        )
+        xr, xl, yd, yu = searchquader(point, tpar1, self.cpar, self.calib)
+
+        # print(f"xr = {xr}, xl = {xl}, yd = {yd}, yu = {yu}")
+
+        self.assertTrue(
+            isclose(xr[0], 0.0, rel_tol=1e-9), f"Expected 0.0 but found {xr[0]}"
+        )
+
+        # Test with infinitely large values of tpar that should return about half the image size
+        tpar2 = TrackPar(
+            0.4,
+            120,
+            1000.0,
+            -1000.0,
+            1000.0,
+            -1000.0,
+            1000.0,
+            -1000.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            1,
+        )
+
+        xr, xl, yd, yu = searchquader(point, tpar2, self.cpar, self.calib)
+
+        # print(f"xr = {xr}, xl = {xl}, yd = {yd}, yu = {yu}")
+
+        self.assertTrue(
+            isclose(xr[0] + xl[0], self.cpar.imx, rel_tol=1e-9),
+            f"Expected image size but found {xr[0] + xl[0]}",
+        )
+        self.assertTrue(
+            isclose(yd[0] + yu[0], self.cpar.imy, rel_tol=1e-9),
+            f"Expected {self.cpar.imy} but found {yd[0] + yu[0]}",
+        )
+
+
 if __name__ == "__main__":
     unittest.main()