#include "qemu_test_harness.h"
#include <math.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

// Include the generated header
#include "arm_math.h"
#include "exp_rs.h"

// Define necessary types and macros based on compilation mode
#if defined(DEF_USE_F32) || (defined(USE_F32) && !defined(USE_F64))

#define SIN sinf
#define COS cosf
#define SQRT sqrtf
#define FABS fabsf
#define TEST_NAME "F32"
#define FORMAT_SPEC "%.6f"
// Use the TEST_PRECISION from exp_rs.h instead of redefining it
#undef TEST_PRECISION
#define TEST_PRECISION 1e-6f
#define TOLERANCE 1e-6f

// Custom implementation of arm_sin_f32 using standard library
static inline float custom_arm_sin_f32(float x) { return sinf(x); }
#define ARM_SIN custom_arm_sin_f32

// Custom implementation of arm_cos_f32 using standard library
static inline float custom_arm_cos_f32(float x) { return cosf(x); }
#define ARM_COS custom_arm_cos_f32

// Custom implementation of arm_sqrt_f32 using standard library
static inline void custom_arm_sqrt_f32(float in, float *out) {
  *out = sqrtf(in);
}
#define ARM_SQRT(x, result) custom_arm_sqrt_f32(x, result)

#elif defined(DEF_USE_F64) || defined(USE_F64)

#define SIN sin
#define COS cos
#define SQRT sqrt
#define FABS fabs
#define TEST_NAME "F64"
#define FORMAT_SPEC "%.12f"
// Use the TEST_PRECISION from exp_rs.h instead of redefining it
#undef TEST_PRECISION
#define TEST_PRECISION 1e-10
#define TOLERANCE 1e-12

// Custom implementation of arm_sin_f64 using standard library
static inline double custom_arm_sin_f64(double x) { return sin(x); }
#define ARM_SIN custom_arm_sin_f64

// Custom implementation of arm_cos_f64 using standard library
static inline double custom_arm_cos_f64(double x) { return cos(x); }
#define ARM_COS custom_arm_cos_f64

// Custom implementation of arm_sqrt_f64 using standard library
static inline void custom_arm_sqrt_f64(double in, double *out) {
  *out = sqrt(in);
}
#define ARM_SQRT(x, result) custom_arm_sqrt_f64(x, result)

#else
#error "Neither USE_F32 nor USE_F64 is defined."
#endif

// Using the EvalResult struct directly

// Debug helper to print values with description
void debug_print_value(const char *desc, Real value) {
#if defined(USE_F32)
  qemu_printf("%s: %f (0x%08X)\n", desc, value, *(uint32_t *)&value);
#else
  union {
    double d;
    uint64_t i;
  } conv;
  conv.d = value;
  qemu_printf("%s: %f (0x%016llX)\n", desc, value, conv.i);
#endif
}

// Test CMSIS-DSP math functions
test_result_t test_cmsis_dsp_functions() {
  qemu_printf("Testing CMSIS-DSP %s Functions\n", TEST_NAME);

  // Test values
  Real test_values[] = {0.0, 0.5, 1.0, 1.5, 2.0, 2.5, 3.0, 4.0, 5.0, 6.0};
  int num_values = sizeof(test_values) / sizeof(test_values[0]);
  int passed = 0;
  int total_tests = 0;

  // Test sin function
  qemu_printf("\nTesting sin function (%s):\n", TEST_NAME);
  for (int i = 0; i < num_values; i++) {
    Real x = test_values[i];
    Real cmsis_result = ARM_SIN(x);
    Real math_result = SIN(x);
    Real diff = FABS(cmsis_result - math_result);

    qemu_printf("  sin(" FORMAT_SPEC "): CMSIS=" FORMAT_SPEC
                ", libm=" FORMAT_SPEC ", diff=" FORMAT_SPEC "\n",
                x, cmsis_result, math_result, diff);

    if (diff <= TOLERANCE) {
      passed++;
    }
    total_tests++;
  }

  // Test cos function
  qemu_printf("\nTesting cos function (%s):\n", TEST_NAME);
  for (int i = 0; i < num_values; i++) {
    Real x = test_values[i];
    Real cmsis_result = ARM_COS(x);
    Real math_result = COS(x);
    Real diff = FABS(cmsis_result - math_result);

    qemu_printf("  cos(" FORMAT_SPEC "): CMSIS=" FORMAT_SPEC
                ", libm=" FORMAT_SPEC ", diff=" FORMAT_SPEC "\n",
                x, cmsis_result, math_result, diff);

    if (diff <= TOLERANCE) {
      passed++;
    }
    total_tests++;
  }

  // Test sqrt function
  qemu_printf("\nTesting sqrt function (%s):\n", TEST_NAME);
  for (int i = 0; i < num_values; i++) {
    Real x = test_values[i];
    if (x < 0.0)
      continue; // Skip negative values for sqrt

    Real cmsis_result;
    ARM_SQRT(x, &cmsis_result);
    Real math_result = SQRT(x);
    Real diff = FABS(cmsis_result - math_result);

    qemu_printf("  sqrt(" FORMAT_SPEC "): CMSIS=" FORMAT_SPEC
                ", libm=" FORMAT_SPEC ", diff=" FORMAT_SPEC "\n",
                x, cmsis_result, math_result, diff);

    if (diff <= TOLERANCE) {
      passed++;
    }
    total_tests++;
  }

  qemu_printf("\nPassed %d/%d tests\n", passed, total_tests);
  return (passed == total_tests) ? TEST_PASS : TEST_FAIL;
}

// CMSIS-DSP Performance Benchmark
test_result_t benchmark_cmsis_dsp() {
  qemu_printf("\n=== CMSIS-DSP %s Performance Benchmark ===\n", TEST_NAME);

  const int ITERATIONS = 1000;
  Real test_value = 1.5;

  // ===== Benchmark sin function =====
  // Benchmark CMSIS-DSP sin
  uint32_t start = qemu_get_tick_count();
  volatile Real cmsis_sin_sum = 0.0;
  for (int i = 0; i < ITERATIONS; i++) {
    cmsis_sin_sum += ARM_SIN(test_value);
  }
  uint32_t end = qemu_get_tick_count();
  uint32_t cmsis_sin_time = end - start;

  // We're no longer comparing to standard library since our custom
  // implementation is just a wrapper over the standard library anyway

  // Report sin results
  qemu_printf("sin benchmark (%d iterations):\n", ITERATIONS);
  qemu_printf("  CMSIS-DSP %s: %u ticks, result=" FORMAT_SPEC "\n", TEST_NAME,
              cmsis_sin_time, cmsis_sin_sum);

  // ===== Benchmark cos function =====
  // Benchmark CMSIS-DSP cos
  start = qemu_get_tick_count();
  volatile Real cmsis_cos_sum = 0.0;
  for (int i = 0; i < ITERATIONS; i++) {
    cmsis_cos_sum += ARM_COS(test_value);
  }
  end = qemu_get_tick_count();
  uint32_t cmsis_cos_time = end - start;

  // We're no longer comparing to standard library since our custom
  // implementation is just a wrapper over the standard library anyway

  // Report cos results
  qemu_printf("\ncos benchmark (%d iterations):\n", ITERATIONS);
  qemu_printf("  CMSIS-DSP %s: %u ticks, result=" FORMAT_SPEC "\n", TEST_NAME,
              cmsis_cos_time, cmsis_cos_sum);

  // ===== Benchmark sqrt function =====
  // Benchmark CMSIS-DSP sqrt
  start = qemu_get_tick_count();
  volatile Real cmsis_sqrt_sum = 0.0;
  Real sqrt_result;
  for (int i = 0; i < ITERATIONS; i++) {
    ARM_SQRT(test_value, &sqrt_result);
    cmsis_sqrt_sum += sqrt_result;
  }
  end = qemu_get_tick_count();
  uint32_t cmsis_sqrt_time = end - start;

  // We're no longer comparing to standard library since our custom
  // implementation is just a wrapper over the standard library anyway

  // Report sqrt results
  qemu_printf("\nsqrt benchmark (%d iterations):\n", ITERATIONS);
  qemu_printf("  CMSIS-DSP %s: %u ticks, result=" FORMAT_SPEC "\n", TEST_NAME,
              cmsis_sqrt_time, cmsis_sqrt_sum);

  return TEST_PASS;
}

// Verify that our math functions work
void verify_math_functions() {
  qemu_print("Testing custom CMSIS-DSP function implementations...\n");

#if defined(USE_F32)
  // Test a few values with our custom implementations
  float test_sin = ARM_SIN(1.0f);
  float test_cos = ARM_COS(1.0f);
  float expected_sin = sinf(1.0f);
  float expected_cos = cosf(1.0f);

  qemu_printf("ARM_SIN(1.0) = %.6f, expected %.6f, diff = %.6f\n", test_sin,
              expected_sin, fabsf(test_sin - expected_sin));
  qemu_printf("ARM_COS(1.0) = %.6f, expected %.6f, diff = %.6f\n", test_cos,
              expected_cos, fabsf(test_cos - expected_cos));

  float test_sqrt_in = 4.0f;
  float test_sqrt_out;
  ARM_SQRT(test_sqrt_in, &test_sqrt_out);
  qemu_printf("ARM_SQRT(4.0) = %.6f, expected 2.0\n", test_sqrt_out);

#elif defined(USE_F64)
  // Test a few values with our custom implementations
  double test_sin = ARM_SIN(1.0);
  double test_cos = ARM_COS(1.0);
  double expected_sin = sin(1.0);
  double expected_cos = cos(1.0);

  qemu_printf("ARM_SIN(1.0) = %.12f, expected %.12f, diff = %.12f\n", test_sin,
              expected_sin, fabs(test_sin - expected_sin));
  qemu_printf("ARM_COS(1.0) = %.12f, expected %.12f, diff = %.12f\n", test_cos,
              expected_cos, fabs(test_cos - expected_cos));

  double test_sqrt_in = 4.0;
  double test_sqrt_out;
  ARM_SQRT(test_sqrt_in, &test_sqrt_out);
  qemu_printf("ARM_SQRT(4.0) = %.12f, expected 2.0\n", test_sqrt_out);
#endif

  qemu_print("Custom math function tests complete.\n");
}

// Main test function
test_result_t test_cmsis_dsp() {
  qemu_printf("CMSIS-DSP %s Test\n", TEST_NAME);
  qemu_printf("This test validates CMSIS-DSP %s functions\n\n", TEST_NAME);

  // Verify our custom math functions work
  verify_math_functions();

  // Test CMSIS-DSP functions directly
  test_result_t result = test_cmsis_dsp_functions();

  // Run benchmarks
  benchmark_cmsis_dsp();

  return result;
}

// Test case definition
static const test_case_t tests[] = {
    {"cmsis_dsp_test", test_cmsis_dsp},
};

int main(void) {
  int failed = run_tests(tests, sizeof(tests) / sizeof(tests[0]));
  qemu_exit(failed ? EXIT_FAILURE : EXIT_SUCCESS);
  return failed ? 1 : 0;
}
