/*
    Centralized test suite.
*/

#include "bits.h"
#include "fix.h"
#include "mat.h"
#include "std.h"
#include <stdbool.h>

#define TEST(p_e)                                                              \
  do {                                                                         \
    if (!(p_e)) {                                                              \
      com_std_print('c', COM_DEF_STRLIT(p_e) " failed!");                      \
      anyfail = true;                                                          \
    }                                                                          \
  } while (0)

static bool com_test_fix(void) {
  bool anyfail = false;
  {
    double max_sqrt_deviation = 0.0;
    com_fix_t sqrt_accumulator = 0;
    com_fix_t sqrt_test_step = COM_FIX_FRACUNIT >> 2; /* Quarter step */
    for (int i = 0; i < COM_FIX_FRACUNIT << 1; ++i) {
      com_fix_t sqrt = com_fix_sqrt(sqrt_accumulator);
      double const deviation =
          com_std_sqrt(com_fix_as_float(sqrt_accumulator)) -
          com_fix_as_float(sqrt);
      if (deviation > max_sqrt_deviation)
        max_sqrt_deviation = deviation;
      sqrt_accumulator += sqrt_test_step;
    }
    com_std_print('c', "Max sqrt deviation: ", 'd', max_sqrt_deviation);
  }

  {
    double max_sin_deviation = 0.0;
    com_fix_t sin_accumulator = -COM_FIX_PI * 4;
    com_fix_t sin_test_step = (COM_FIX_PI << 1) >> 10;
    for (int i = 0; i < 1024 * 32; ++i) {
      com_fix_t sin = com_fix_sin(sin_accumulator);
      double const deviation =
          com_std_fabs(com_std_sin(com_fix_as_float(sin_accumulator)) -
                       com_fix_as_float(sin));
      if (deviation > max_sin_deviation)
        max_sin_deviation = deviation;
      sin_accumulator += sin_test_step;
    }
    com_std_print('c', "Max sin deviation: ", 'd', max_sin_deviation);
  }

  {
    double max_cos_deviation = 0.0;
    com_fix_t cos_accumulator = -COM_FIX_PI * 4;
    com_fix_t cos_test_step = (COM_FIX_PI << 1) >> 10;
    for (int i = 0; i < 1024 * 32; ++i) {
      com_fix_t cos = com_fix_cos(cos_accumulator);
      double const deviation =
          com_std_fabs(com_std_cos(com_fix_as_float(cos_accumulator)) -
                       com_fix_as_float(cos));
      if (deviation > max_cos_deviation)
        max_cos_deviation = deviation;
      cos_accumulator += cos_test_step;
    }
    com_std_print('c', "Max cos deviation: ", 'd', max_cos_deviation);
  }

  {
    double max_sin_deviation = 0.0;
    double max_cos_deviation = 0.0;
    com_fix_t sincos_accumulator = -COM_FIX_PI * 4;
    com_fix_t sincos_test_step = (COM_FIX_PI << 1) >> 10;
    for (int i = 0; i < 1024 * 32; ++i) {
      com_fix_t sin, cos;
      com_fix_sincos(sincos_accumulator, &sin, &cos);
      double const sin_deviation =
          com_std_fabs(com_std_sin(com_fix_as_float(sincos_accumulator)) -
                       com_fix_as_float(sin));
      double const cos_deviation =
          com_std_fabs(com_std_cos(com_fix_as_float(sincos_accumulator)) -
                       com_fix_as_float(cos));
      if (sin_deviation > max_sin_deviation)
        max_sin_deviation = sin_deviation;
      if (cos_deviation > max_cos_deviation)
        max_cos_deviation = cos_deviation;
      sincos_accumulator += sincos_test_step;
    }
    com_std_print('c', "Max sincos deviations: ", 'd', max_sin_deviation, 'd',
                  max_cos_deviation);
  }

  return anyfail;
}

static bool com_test_bits(void) {
  bool anyfail = false;

  TEST(com_bits_nalign(0, 4) == 0);
  TEST(com_bits_nalign(1, 4) == 4);
  TEST(com_bits_nalign(2, 4) == 4);
  TEST(com_bits_nalign(3, 4) == 4);
  TEST(com_bits_nalign(4, 4) == 4);
  TEST(com_bits_nalign(5, 4) == 8);

  return anyfail;
}

static bool com_test_mat(void) {
  bool anyfail = false;

  com_mat_t const m0 = com_mat_identity();
  com_mat_t m1 = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16};
  for (int i = 0; i < 16; ++i)
    m1.a[i] *= COM_FIX_FRACUNIT;

  com_mat_t const mul = com_mat_mul(m1, m0);
  com_mat_t const mul_reordered = com_mat_mul_reodered(com_mat_reoder(m1), m0);

  for (int i = 0; i < 16; ++i) {
    TEST(mul.a[i] == mul_reordered.a[i]);
    if (mul.a[i] != mul_reordered.a[i])
      break;
  }

  return anyfail;
}

static bool com_test_run(void) {
  bool anyfail = false;
  if (com_test_fix()) {
    com_std_print('c', "com_fix tests failed!");
    anyfail = true;
  }
  if (com_test_bits()) {
    com_std_print('c', "com_bits tests failed!");
    anyfail = true;
  }
  if (com_test_mat()) {
    com_std_print('c', "com_mat tests failed!");
    anyfail = true;
  }
  return !anyfail;
}

#undef TEST