function [COM, I] = mass_properties(poly, mass)
% Assumes 
%  poly.vertices is m x 3
%  poly.triangles is n x 3
%  poly.normals will be n x 3 (computed below)

if size(poly.vertices,2)~=3 | size(poly.triangles,2)~=3
    error('Bad array sizes');
end


% Unit mass by default
if nargin < 2
    mass = 1;
end

% Compute normals
poly = massprop_compute_poly_normals(poly);

% Compute volume integrals
[volume, T1, T2, TP] = massprop_compute_volume_integrals(poly);

% Compute uniform density given mass
density = mass / volume;

disp(sprintf('get_center_of_mass_and_inertia_tensor: mass = %f, volume = %f, density = %f', mass, volume, density));

% Use computed integrals to figure out center of mass and intertia tensor
COM = T1 / volume;
I = zeros(3,3);
I(1,1) = density * (T2(2) + T2(3));
I(2,2) = density * (T2(3) + T2(1));
I(3,3) = density * (T2(1) + T2(2));
I(2,1) = -density * TP(1);
I(3,2) = -density * TP(2);
I(3,1) = -density * TP(3);

% Fill in symmetric side...
I(1,2) = I(2,1);
I(1,3) = I(3,1);
I(2,3) = I(3,2);

% Translate inertia tensor to center of mass (it's currently computed with respect to the origin
I = I - mass * (dot(COM,COM)*eye(3) - COM'*COM);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [pintegrals] = massprop_compute_projection_integrals(poly, i, A, B, C)

pintegrals.P1 = 0;
pintegrals.Pa = 0;
pintegrals.Pb = 0;
pintegrals.Paa = 0;
pintegrals.Pab = 0;
pintegrals.Pbb = 0;
pintegrals.Paaa = 0;
pintegrals.Paab = 0;
pintegrals.Pabb = 0;
pintegrals.Pbbb = 0;

vertices = [poly.vertices(poly.triangles(i,1),:); poly.vertices(poly.triangles(i,2),:); poly.vertices(poly.triangles(i,3),:)];

for v=1:3
    a0 = vertices(v,A);
    b0 = vertices(v,B);
    a1 = vertices(mod(v,3)+1,A);
    b1 = vertices(mod(v,3)+1,B);

    da = a1-a0;
    db = b1-b0;

    a0_2 = a0*a0;
    b0_2 = b0*b0;
    a0_3 = a0_2*a0;
    a0_4 = a0_3*a0;
    b0_3 = b0_2*b0;
    b0_4 = b0_3*b0;
    a1_2 = a1*a1;
    a1_3 = a1_2*a1;
    b1_2 = b1*b1;
    b1_3 = b1_2*b1;

    C1 = a1+a0;
    Ca = a1*C1+a0_2;
    Cb = b1*(b1+b0)+b0_2;
    Caa = a1*Ca+a0_3;
    Caaa = a1*Caa+a0_4;
    Cbb = b1*Cb+b0_3;
    Cbbb = b1*Cbb+b0_4;
    Cab = 3*a1_2+2*a1*a0+a0_2;
    Kab = a1_2+2*a1*a0+3*a0_2;
    Caab = a0*Cab+4*a1_3;
    Kaab = a1*Kab+4*a0_3;
    Cabb = 4*b1_3+3*b1_2*b0+2*b1*b0_2+b0_3;
    Kabb = b1_3+2*b1_2*b0+3*b1*b0_2+4*b0_3;

    pintegrals.P1   = pintegrals.P1 + db*C1;
    pintegrals.Pa   = pintegrals.Pa + db*Ca;
    pintegrals.Pb   = pintegrals.Pb + da*Cb;
    pintegrals.Paa  = pintegrals.Paa + db*Caa;
    pintegrals.Paaa = pintegrals.Paaa + db*Caaa;
    pintegrals.Pbb  = pintegrals.Pbb + da*Cbb;
    pintegrals.Pbbb = pintegrals.Pbbb + da*Cbbb;
    pintegrals.Pab  = pintegrals.Pab + db*(b1*Cab+b0*Kab);
    pintegrals.Paab = pintegrals.Paab + db*(b1*Caab+b0*Kaab);
    pintegrals.Pabb = pintegrals.Pabb + da*(a1*Cabb+a0*Kabb);
end

pintegrals.P1   = pintegrals.P1 / 2;

pintegrals.Pa   = pintegrals.Pa / 6;
pintegrals.Paa  = pintegrals.Paa / 12;
pintegrals.Paaa = pintegrals.Paaa / 20;

pintegrals.Pb   = -pintegrals.Pb / 6;
pintegrals.Pbb  = -pintegrals.Pbb / 12;
pintegrals.Pbbb = -pintegrals.Pbbb / 20;

pintegrals.Pab  = pintegrals.Pab / 24;
pintegrals.Paab = pintegrals.Paab / 60;
pintegrals.Pabb = -pintegrals.Pabb / 60;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [integrals] = massprop_compute_face_integrals(poly, i, A, B, C)

[pint] = massprop_compute_projection_integrals(poly, i, A, B, C);

N = poly.normals(i,:);
w = -dot(N, poly.vertices(poly.triangles(i,1),:));

k1 = 1/N(C);
k2 = k1*k1;
k3 = k2*k1;
k4 = k3*k1;

integrals.Fa = k1*pint.Pa;
integrals.Fb = k1*pint.Pb;
integrals.Fc = -k2*(N(A)*pint.Pa + N(B)*pint.Pb + w*pint.P1);
integrals.Faa = k1*pint.Paa;
integrals.Fbb = k1*pint.Pbb;
integrals.Fcc = k3*(N(A)*N(A)*pint.Paa + 2*N(A)*N(B)*pint.Pab + N(B)*N(B)*pint.Pbb + w*(2*(N(A)*pint.Pa+N(B)*pint.Pb)+w*pint.P1));
integrals.Fab = k1*pint.Pab;
integrals.Fbc = -k2*(N(A)*pint.Pab + N(B)*pint.Pbb + w*pint.Pb);
integrals.Fca = -k2*(N(A)*pint.Paa + N(B)*pint.Pab +w*pint.Pa);
integrals.Faaa = k1*pint.Paaa;
integrals.Fbbb = k1*pint.Pbbb;
integrals.Fccc = -k4*(N(A)*N(A)*N(A)*pint.Paaa + 3*N(A)*N(A)*N(B)*pint.Paab + 3*N(A)*N(B)*N(B)*pint.Pabb + ...
                 N(B)*N(B)*N(B)*pint.Pbbb + 3*w*(N(A)*N(A)*pint.Paa + 2*N(A)*N(B)*pint.Pab + N(B)*N(B)*pint.Pbb) + ...
                 w*w*(3*(N(A)*pint.Pa+N(B)*pint.Pb) + w*pint.P1));
integrals.Faab = k1*pint.Paab;
integrals.Fbbc = -k2*(N(A)*pint.Pabb + N(B)*pint.Pbbb + w*pint.Pbb);
integrals.Fcca = k3*(N(A)*N(A)*pint.Paaa + 2*N(A)*N(B)*pint.Paab + N(B)*N(B)*pint.Pabb + w*(2*(N(A)*pint.Paa+N(B)*pint.Pab) + w*pint.Pa));

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [volume, T1, T2, TP] = massprop_compute_volume_integrals(poly)

volume=0;
T1=[0 0 0];
T2=[0 0 0];
TP=[0 0 0];

% TODO: reset data (Fa, etc.)

for i=1:length(poly.triangles)
    
    N = poly.normals(i,:);

    % Get dominant axis (C) for normal
    [dummy, C] = max(abs(N));
    A = mod(C,3)+1;
    B = mod(A,3)+1;

    % disp(sprintf('Triangle %d normal %f %f %f dominant = %d, A=%d B=%f', i, N(1), N(2), N(3), C, A, B));

    [integrals] = massprop_compute_face_integrals(poly, i, A, B, C);

    if A==1
        F = integrals.Fa;
    elseif B==1
        F = integrals.Fb;
    else
        F = integrals.Fc;
    end

    volume = volume + N(1) * F;

    T1(A) = T1(A) + N(A)*integrals.Faa;
    T1(B) = T1(B) + N(B)*integrals.Fbb;
    T1(C) = T1(C) + N(C)*integrals.Fcc;
    T2(A) = T2(A) + N(A)*integrals.Faaa;
    T2(B) = T2(B) + N(B)*integrals.Fbbb;
    T2(C) = T2(C) + N(C)*integrals.Fccc;
    TP(A) = TP(A) + N(A)*integrals.Faab;
    TP(B) = TP(B) + N(B)*integrals.Fbbc;
    TP(C) = TP(C) + N(C)*integrals.Fcca;

end

T1 = T1 / 2;
T2 = T2 / 3;
TP = TP / 2;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [poly] = massprop_compute_poly_normals(poly)

n = size(poly.triangles,1);
poly.normals = zeros(n,3);

for i=1:n
    % Computes outward pointing normal using ccw order
    v1 = poly.vertices(poly.triangles(i,1),:);
    v2 = poly.vertices(poly.triangles(i,2),:);
    v3 = poly.vertices(poly.triangles(i,3),:);
    N = cross(v2-v1, v3-v2);
    N = N / norm(N);

%    disp(sprintf('Triangle %d v1 = %f %f %f, v2 = %f %f %f, v3 = %f %f %f, normal = %f %f %f', ...
%        i, v1(1),v1(2),v1(3), v2(1),v2(2),v2(3), v3(1),v3(2),v3(3), N(1),N(2),N(3)));

    poly.normals(i,:) = N;
end
