more test for hue periodicity

lib/pbio/src/color/util.c

@@ -33,13 +33,16 @@ int32_t pbio_get_hsv_cost(const pbio_color_hsv_t *x, const pbio_color_hsv_t *c)
 }
 
 int32_t pbio_get_cone_cost(const pbio_color_hsv_t *a, const pbio_color_hsv_t *b) {
-    // normalize h to radians, s/v to (0,1)
+
     fix16_t by100 = fix16_div(fix16_one, fix16_from_int(100));
+
+    // normalize h to radians, s/v to (0,1)
     fix16_t a_h = fix16_deg_to_rad(fix16_from_int(a->h));
-    fix16_t b_h = fix16_deg_to_rad(fix16_from_int(b->h));
     fix16_t a_s = fix16_mul(fix16_from_int(a->s), by100);
-    fix16_t b_s = fix16_mul(fix16_from_int(b->s), by100);
     fix16_t a_v = fix16_mul(fix16_from_int(a->v), by100);
+
+    fix16_t b_h = fix16_deg_to_rad(fix16_from_int(b->h));
+    fix16_t b_s = fix16_mul(fix16_from_int(b->s), by100);
     fix16_t b_v = fix16_mul(fix16_from_int(b->v), by100);
 
     // x, y and z deltas between cartesian coordinates of a and b in HSV cone

lib/pbio/test/src/color.c

@@ -461,6 +461,36 @@ static void test_color_hsv_cost(void *env) {
     color_b.v = 70;
 
     tt_want_int_op(pbio_get_cone_cost(&color_a, &color_b), ==, dist);
+
+    // hues 360 and 0 should be the same
+    color_a.h = 360;
+    color_a.s = 100;
+    color_a.v = 100;
+
+    color_b.h = 0;
+    color_b.s = 100;
+    color_b.v = 100;
+    tt_want_int_op(pbio_get_cone_cost(&color_a, &color_b), ==, 0);
+
+    // distance between hues 359 and 1 should be smaller than hues 1 and 5
+    color_a.h = 359;
+    color_a.s = 100;
+    color_a.v = 100;
+
+    color_b.h = 1;
+    color_b.s = 100;
+    color_b.v = 100;
+    dist = pbio_get_cone_cost(&color_a, &color_b);
+
+    color_a.h = 1;
+    color_a.s = 100;
+    color_a.v = 100;
+
+    color_b.h = 5;
+    color_b.s = 100;
+    color_b.v = 100;
+
+    tt_want_int_op(pbio_get_cone_cost(&color_a, &color_b), >, dist);
 }
 
 struct testcase_t pbio_color_tests[] = {