using MeshIO
using FileIO
using GeometryBasics

using LinearAlgebra


stl = load(raw"C:\Users\albig\Downloads\cubeblender.stl") 

function get_mass_properties(triangles)
	x = [ [v[1] for v in tri] for tri in triangles] |> x -> reduce(hcat,x)'
	y = [ [v[2] for v in tri] for tri in triangles] |> x -> reduce(hcat,x)'
	z = [ [v[3] for v in tri] for tri in triangles] |> x -> reduce(hcat,x)'


	function subexpression(x)
		w0, w1, w2 = x[:, 1], x[:, 2], x[:, 3]
		temp0 = w0 + w1
		f1 = temp0 + w2
		temp1 = w0 .* w0
		temp2 = temp1 .+ w1 .* temp0
		f2 = temp2 .+ w2 .* f1
		f3 = w0 .* temp1 .+ w1 .* temp2 .+ w2 .* f2
		g0 = f2 .+ w0 .* (f1 .+ w0)
		g1 = f2 .+ w1 .* (f1 .+ w1)
		g2 = f2 .+ w2 .* (f1 .+ w2)
		return f1, f2, f3, g0, g1, g2
	end

	x0, x1, x2 = x[:, 1], x[:, 2], x[:, 3]
	y0, y1, y2 = y[:, 1], y[:, 2], y[:, 3]
	z0, z1, z2 = z[:, 1], z[:, 2], z[:, 3]
	a1, b1, c1 = x1 - x0, y1 - y0, z1 - z0
	a2, b2, c2 = x2 - x0, y2 - y0, z2 - z0
	d0, d1, d2 = b1 .* c2 .- b2 .* c1, a2 .* c1 .- a1 .* c2, a1 .* b2 .- a2 .* b1

	f1x, f2x, f3x, g0x, g1x, g2x = subexpression(x)
	f1y, f2y, f3y, g0y, g1y, g2y = subexpression(y)
	f1z, f2z, f3z, g0z, g1z, g2z = subexpression(z)

	intg = zeros(10)
	intg[1] = sum(d0 .* f1x)
	intg[2:4] = [sum(d0 .* f2x), sum(d1 .* f2y), sum(d2 .* f2z)]
	intg[5:7] = [sum(d0 .* f3x), sum(d1 .* f3y), sum(d2 .* f3z)]
	intg[8] = sum(d0 .* (y0 .* g0x + y1 .* g1x + y2 .* g2x))
	intg[9] = sum(d1 .* (z0 .* g0y + z1 .* g1y + z2 .* g2y))
	intg[10] = sum(d2 .* (x0 .* g0z + x1 .* g1z + x2 .* g2z))
	intg = intg ./ [6, 24, 24, 24, 60, 60, 60, 120, 120, 120]
	volumee = intg[1]
	cog = intg[2:4] / volumee
	cogsq = cog .^ 2
	inertia = zeros((3, 3))
	inertia[1, 1] = intg[6] + intg[7] - volumee .* (cogsq[2] + cogsq[3])
	inertia[2, 2] = intg[5] + intg[7] - volumee .* (cogsq[3] + cogsq[1])
	inertia[3, 3] = intg[5] + intg[6] - volumee .* (cogsq[1] + cogsq[2])
	inertia[1, 2] = inertia[2, 1] = -(intg[8] - volumee .* cog[1] .* cog[2])
	inertia[2, 3] = inertia[3, 2] = -(intg[9] - volumee .* cog[2] .* cog[3])
	inertia[1, 3] = inertia[3, 1] = -(intg[10] - volumee .* cog[3] .* cog[1])
	return volume, cog, inertia
end
volumee
cog
inertia