using Test

using stlProcess
import stlProcess.Properties

using FileIO
using MeshIO

using LinearAlgebra

function _check_volume(triangles; scale=1)
    """
    Slow, inaccurate  algorithm just used to test the other algorithms
    Reference:
        https://people.eecs.berkeley.edu/~wkahan/VtetLang.pdf
    """

    volume = 0.0
    for tri in triangles
        a, b, c = tri .* scale

        W = sqrt(sum((a .- b) .^ 2))
        V = sqrt(sum((b .- c) .^ 2))
        U = sqrt(sum((c .- a) .^ 2))

        v = sqrt(sum(a .^ 2))
        u = sqrt(sum(b .^ 2))
        w = sqrt(sum(c .^ 2))

        X = (w - U + v) * (U + v + w)
        Y = (u - V + w) * (V + w + u)
        Z = (v - W + u) * (W + u + v)

        x = (U - v + w) * (v - w + U)
        y = (V - w + u) * (w - u + V)
        z = (W - u + v) * (u - v + W)

        ξ = sqrt(x * Y * Z)
        η = sqrt(y * Z * X)
        ζ = sqrt(z * X * Y)
        λ = sqrt(x * y * z)

        volume += sqrt((ξ + η + ζ - λ) * (λ + ξ + η - ζ) * (η + ζ + λ - ξ) * (ζ + λ + ξ - η)) / (192 * u * v * w)
    end

    return volume
end

@testset "3D Models" begin
    I_mat = Matrix(I, 3, 3)
    center = [0, 0, 0]

    models = Dict(
        # Inertia math: https://en.wikipedia.org/wiki/List_of_moments_of_inertia#List_of_3D_inertia_tensors
        # Properties(volume, center_of_gravity, inertia, surface_area, characteristic_length)
        "cube.stl" => Properties(2.0^3, center, I_mat .* 2^2 / 6, 6 * 2^2, 2, [2, 2, 2]), # l = 2
        "sphere.stl" => Properties(4 / 3 * pi, center, I_mat .* 2 / 5, 4π, 2, [2, 2, 2]), # r = 1
        "2_4_8_cuboid.stl" => Properties(
            2 * 4 * 8,
            center,
            diagm([2^2 + 4^2, 8^2 + 2^2, 8^2 + 4^2]) ./ 12,
            2(2 * 4 + 2 * 8 + 4 * 8),
            (2 + 4 + 8) / 3,
            [2, 4, 8],
        ), # h, d, w = 2, 4, 8
        # "slender_y.stl" => Properties(
        #     10 * π * 0.1^2,
        #     center,
        #     diagm([1, 0, 1]) .* (10^2 / 12),
        #     2π * 0.05 * (0.1 + 10),
        #     0.55,#TODO: figure out why this math doesnt work (10 + 0.2 + 0.2) / 3
        # ), # l_z = 10, r = 0.1
        # "cylinder.stl" => Properties(
        #     10 * π * 1^2,
        #     center,
        #     diagm([(3 + 10^2) / 12, (3 + 10^2) / 12, 1^2 / 2]),
        #     2π * 1 * (1 + 10),
        #     2.24,#TODO: figure out why this math doesnt work (10 + 2 + 2) / 3,
        # ), # l_z = 10, r = 1
    )
    @testset "$model" for (model, control) in models
        path = "test_assets/$model"
        stl = load(path)

        @testset "get_mass_properties for $model" begin
            props = get_mass_properties(stl)

            @test props.volume ≈ control.volume atol = 0.5
            @test props.center_of_gravity ≈ control.center_of_gravity atol = 0.01
            @test eigvals(props.inertia) ≈ eigvals(control.inertia) atol = 0.1
            @test props.surface_area ≈ control.surface_area atol = 0.5
            @test props.characteristic_length ≈ control.characteristic_length atol = 0.5
            @test props.sb_values ≈ control.sb_values atol = 0.01
        end
        @testset "Compare volumes with scaling for $model" begin
            for scale in 1:4
                props = get_mass_properties(stl; scale=scale)
                volume = _check_volume(stl; scale=scale)
                @test props.volume ≈ volume atol = 0.1
            end
        end
        @testset "find_volume" begin
            for scale in [0.5, 1, 1.0, 10, 2 / 3]
                @test fast_volume(stl; scale=find_scale(stl; desired_volume=scale)) ≈ scale
            end
        end
    end
end