#include <math/vec3.h>

#include <math/float.h>

#include "test.h"

#include <stdio.h>

static const float eps = 1e-7;

static inline void print_vec3(vec3 v) {
  printf("{ %f, %f, %f }\n", v.x, v.y, v.z);
}

/// Slerp between two vectors forming an acute angle.
TEST_CASE(vec3_slerp_acute_angle) {
  const R angle1 = 0;
  const R angle2 = PI / 4;
  const R t      = 0.5;

  const vec3 a = vec3_make(cos(angle1), sin(angle1), 0);
  const vec3 b = vec3_make(cos(angle2), sin(angle2), 0);

  const vec3 result = vec3_slerp(a, b, t);

  const R    angle3   = lerp(angle1, angle2, t);
  const vec3 expected = vec3_make(cos(angle3), sin(angle3), 0.0);
  TEST_TRUE(vec3_eq(result, expected, eps));
}

/// Slerp between two vectors forming an obtuse angle (negative dot product).
///
/// The interpolation must follow the shortest path between both vectors.
TEST_CASE(vec3_slerp_obtuse_angle) {
  const R angle1 = 0;
  const R angle2 = 3 * PI / 4;
  const R t      = 0.5;

  const vec3 a = vec3_make(cos(angle1), sin(angle1), 0);
  const vec3 b = vec3_make(cos(angle2), sin(angle2), 0);

  const vec3 result = vec3_slerp(a, b, t);

  const R    angle3   = lerp(angle1, angle2, t);
  const vec3 expected = vec3_make(cos(angle3), sin(angle3), 0.0);
  TEST_TRUE(vec3_eq(result, expected, eps));
}

/// Slerp between two vectors forming a reflex angle.
///
/// The interpolation must follow the shortest path between both vectors.
TEST_CASE(vec3_slerp_reflex_angle) {
  const R angle1 = 0;
  const R angle2 = 5 * PI / 4;
  const R t      = 0.5;

  const vec3 a = vec3_make(cos(angle1), sin(angle1), 0);
  const vec3 b = vec3_make(cos(angle2), sin(angle2), 0);

  const vec3 result = vec3_slerp(a, b, t);

  // slerp goes from a to b following the shortest path, which is down from a
  // towards b. The resulting angle is therefore +PI of the angle we get from
  // lerping the two input angles.
  const R    angle3   = lerp(angle1, angle2, t) + PI;
  const vec3 expected = vec3_make(cos(angle3), sin(angle3), 0.0);
  TEST_TRUE(vec3_eq(result, expected, 1e-5));
}