#include "util/mini_math.h"
#include <iostream>
#include <vector>
#include <cassert>
#include <cmath>

// Checks if two floats are approximately equal
bool near(float a, float b, float e = 0.001f) { return std::abs(a - b) < e; }

// Generic test runner for any vector type (vec2, vec3, vec4)
template<typename T>
void test_vector_ops(int n) {
    T a, b;
    // Set values
    for(int i=0; i<n; ++i) { a[i] = (float)(i + 1); b[i] = 10.0f; }

    // Add
    T c = a + b;
    for(int i=0; i<n; ++i) assert(near(c[i], (float)(i + 1) + 10.0f));

    // Scale
    T s = a * 2.0f;
    for(int i=0; i<n; ++i) assert(near(s[i], (float)(i + 1) * 2.0f));

    // Dot Product
    // vec3(1,2,3) . vec3(1,2,3) = 1+4+9 = 14
    float expected_dot = 0;
    for(int i=0; i<n; ++i) expected_dot += a[i] * a[i];
    assert(near(T::dot(a, a), expected_dot));

    // Norm (Length)
    assert(near(a.norm(), std::sqrt(expected_dot)));

    // Normalize
    T n_vec = a.normalize();
    assert(near(n_vec.norm(), 1.0f));

    // Lerp
    T l = lerp(a, b, 0.3f);
    for(int i=0; i<n; ++i) assert(near(l[i], .7 * (i+1) + .3 * 10.0f));
}

// Specific test for padding alignment in vec3
void test_vec3_special() {
    std::cout << "Testing vec3 alignment..." << std::endl;
    // Verify sizeof is 16 bytes (4 floats) due to padding for WebGPU
    assert(sizeof(vec3) == 16);

    vec3 v(1, 0, 0);
    vec3 v2(0, 1, 0);

    // Cross Product
    vec3 c = vec3::cross(v, v2);
    assert(near(c.x, 0) && near(c.y, 0) && near(c.z, 1));
}

// Tests quaternion rotation, look_at, and slerp
void test_quat() {
    std::cout << "Testing Quat..." << std::endl;

    // Rotation (Rodrigues)
    vec3 v(1, 0, 0);
    quat q = quat::from_axis({0, 1, 0}, 1.5708f); // 90 deg Y
    vec3 r = q.rotate(v);
    assert(near(r.x, 0) && near(r.z, -1));

    // Look At
    // Looking from origin to +X, with +Y as up.
    // The local forward vector (0,0,-1) should be transformed to (1,0,0)
    quat l = quat::look_at({0,0,0}, {10,0,0}, {0,1,0});
    vec3 f = l.rotate({0,0,-1});
    assert(near(f.x, 1.0f) && near(f.y, 0.0f) && near(f.z, 0.0f));

    // Slerp Midpoint
    quat q1(0,0,0,1);
    quat q2 = quat::from_axis({0,1,0}, 1.5708f); // 90 deg
    quat mid = slerp(q1, q2, 0.5f); // 45 deg
    assert(near(mid.y, 0.3826f)); // sin(pi/8)
}

// Tests WebGPU specific matrices
void test_matrices() {
    std::cout << "Testing Matrices..." << std::endl;
    float n = 0.1f, f = 100.0f;
    mat4 p = mat4::perspective(0.785f, 1.0f, n, f);

    // Check WebGPU Z-range [0, 1]
    // Z_ndc = (m10 * Z_view + m14) / -Z_view
    float z_near = (p.m[10] * -n + p.m[14]) / n;
    float z_far  = (p.m[10] * -f + p.m[14]) / f;
    assert(near(z_near, 0.0f));
    assert(near(z_far, 1.0f));

    // Test mat4::look_at
    vec3 eye(0, 0, 5);
    vec3 target(0, 0, 0);
    vec3 up(0, 1, 0);
    mat4 view = mat4::look_at(eye, target, up);
    // Point (0,0,0) in world should be at (0,0,-5) in view space
    assert(near(view.m[14], -5.0f));
}

// Tests easing curves
void test_ease() {
    std::cout << "Testing Easing..." << std::endl;
    // Boundary tests
    assert(near(ease::out_cubic(0.0f), 0.0f));
    assert(near(ease::out_cubic(1.0f), 1.0f));
    assert(near(ease::in_out_quad(0.0f), 0.0f));
    assert(near(ease::in_out_quad(1.0f), 1.0f));

    // Midpoint/Logic tests
    assert(ease::out_cubic(0.5f) > 0.5f); // Out curves should exceed linear value early
    assert(near(ease::in_out_quad(0.5f), 0.5f)); // Symmetric curves hit 0.5 at 0.5
}

// Tests spring solver
void test_spring() {
    std::cout << "Testing Spring..." << std::endl;
    float p = 0, v = 0;
    // Simulate approx 1 sec with 0.5s smooth time
    for(int i=0; i<60; ++i) spring::solve(p, v, 10.0f, 0.5f, 0.016f);
    assert(p > 8.5f);

    // Test vector spring
    vec3 vp(0,0,0), vv(0,0,0), vt(10,0,0);
    spring::solve(vp, vv, vt, 0.5f, 0.016f * 60.0f); // 1 huge step approx
    assert(vp.x > 1.0f); // Should have moved significantly
}

int main() {
    std::cout << "Testing vec2..." << std::endl;
    test_vector_ops<vec2>(2);

    std::cout << "Testing vec3..." << std::endl;
    test_vector_ops<vec3>(3);
    test_vec3_special();

    std::cout << "Testing vec4..." << std::endl;
    test_vector_ops<vec4>(4);

    test_quat();
    test_matrices();
    test_ease();
    test_spring();

    std::cout << "--- ALL TESTS PASSED ---" << std::endl;
    return 0;
}