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)
        "cube.stl" => Properties(2.0^3, center, I_mat .* 2^2 / 6), # l = 2
        "sphere.stl" => Properties(4 / 3 * pi, center, I_mat .* 2 / 5), # 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), # h, d, w = 2, 4, 8
        "slender_y.stl" => Properties(10 * π * 0.1^2, center, diagm([1, 0, 1]) .* (10^2 / 12)), # 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])), # l_z = 10, r = 1
    )
    @testset "$model" for (model, control) in models
        path = "test_assets\\$model"
        stl = load(path)

        @testset "get_mass_properties" 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
        end
        @testset "Compare volumes with scaling" begin
            for scale in 1:5
                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