#include <math/quat.h>

#include <math/float.h>

#include "test.h"

#include <stdio.h>

static const float eps = 1e-7;

static inline void print_quat(quat q) {
  printf("{ %f, %f, %f, %f }\n", q.x, q.y, q.z, q.w);
}

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(quat_slerp_acute_angle) {
  const R angle1 = 0;
  const R angle2 = PI / 4;
  const R t      = 0.5;

  const quat a = qmake_rot(angle1, 0, 0, 1);
  const quat b = qmake_rot(angle2, 0, 0, 1);

  const quat c      = qslerp(a, b, t);
  const vec3 result = qrot(c, vec3_make(1, 0, 0));

  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(quat_slerp_obtuse_angle) {
  const R angle1 = 0;
  const R angle2 = 3 * PI / 4;
  const R t      = 0.5;

  const quat a = qmake_rot(angle1, 0, 0, 1);
  const quat b = qmake_rot(angle2, 0, 0, 1);

  const quat c      = qslerp(a, b, t);
  const vec3 result = qrot(c, vec3_make(1, 0, 0));

  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(quat_slerp_reflex_angle) {
  const R angle1 = 0;
  const R angle2 = 5 * PI / 4;
  const R t      = 0.5;

  const quat a = qmake_rot(angle1, 0, 0, 1);
  const quat b = qmake_rot(angle2, 0, 0, 1);

  const quat c      = qslerp(a, b, t);
  const vec3 result = qrot(c, vec3_make(1, 0, 0));

  // Because it's a reflex angle, we expect the rotation to follow the short
  // path from 'a' down clockwise to 'b'. Could add +PI to the result of lerp(),
  // but that adds more error than negating cos and sin.
  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));
}

TEST_CASE(quat_mat4_from_quat) {
  const R    angle = PI / 8;
  const quat q     = qmake_rot(angle, 0, 0, 1);

  const mat4 m = mat4_from_quat(q);
  const vec3 p = mat4_mul_vec3(m, vec3_make(1, 0, 0), /*w=*/1);

  TEST_TRUE(vec3_eq(p, vec3_make(cos(angle), sin(angle), 0), eps));
}