%==========================================================
%   Gauss-Newton method
%==========================================================
function [Q, iter, sec] = gauss_newton(x, y, z, Q)
    tic;
    nfunc = size(x, 1);
    iter = 0;
    while(1)
        a = Q(1);
        b = Q(2);
        c = Q(3);
        r = Q(4);
    
        % A matrix (Jacobian)
        A = zeros(nfunc, 4);
        for i = 1:nfunc
            A(i, 1:4) = [-2*(x(i)-a)   -2*(y(i)-b)   -2*(z(i)-c)   -2*r];
        end       

        % function value
        f = zeros(nfunc, 1);
        for i = 1:nfunc
            f(i) = (x(i)-a)^2 + (y(i)-b)^2 + (z(i)-c)^2 - r^2;
        end 

        % calculate Pseudo-Inverse solution {dq}
        dq = (A' * A) \ (A' * -f);
        
        %update
        Q = Q + dq';
        iter = iter + 1;

        % check convergence
        if(max(abs(dq)) < 1.e-6), break, end 
        if(iter > 20), break, end
    end
    sec = toc;
end