%% Intersection of Ellipses
% Consider the two ellipses given by x(t)=-5+10\cos t,\quad y(t)=6\sin t and x(t)=8\cos t, \quad y(t)=1+12\sin t.
% First plot the ellipses defined in functions curve1 and curve2. The function nlsystem provides the differences between the x and y values at two different t values.

clear
t=0:0.1:2*pi+.1;
y1=curve1(t);
y2=curve2(t);
plot(y1(1,:),y1(2,:),'b',y2(1,:),y2(2,:),'r')
axis equal

nlsystem([1,2])

%
% Use levenberg to find t values for a point of intersection given a guess [t_1,t_2]=[1,2].

x1 = [1;2];   
x = levenberg(@nlsystem,x1);
t1=x(:,end)

%
% The coordinates of the points for these values agree:

curve1(t1(1))
curve2(t1(2))

%
% We can plot the points on the curves.

hold on
x1=curve1(t1(1));
x2=curve2(t1(2));
plot(x1(1),x1(2),'ok',x2(1),x2(2),'.b')
hold off

%
% Another guess leads to the second point of intersection.

x1 = [4;4];   
x = levenberg(@nlsystem,x1);
t1=x(:,end)

curve1(t1(1))
curve2(t1(2))

%
% We plot the second point of intersection.

hold on
x1=curve1(t1(1));
x2=curve2(t1(2));
plot(x1(1),x1(2),'or',x2(1),x2(2),'.g')
hold off

%

function f = nlsystem(x)  
    f = (curve1(x(1))-curve2(x(2)));
end

function f = curve1(t)
f(1,:) = 10*cos(t)-5;
f(2,:) = 6*sin(t);
end

function f = curve2(t)
f(1,:) = 8*cos(t);
f(2,:) = 1+12*sin(t);
end

function x = levenberg(f,x1,tol)
% LEVENBERG   Quasi-Newton method for nonlinear systems.
% Input:
%   f         objective function 
%   x1        initial root approximation
%   tol       stopping tolerance (default is 1e-12)
% Output       
%   x         array of approximations (one per column)

% Operating parameters.
if nargin < 3, tol = 1e-12; end
ftol = tol;  xtol = tol;  maxiter = 40;

x = x1(:);     fk = f(x1);
k = 1;  s = Inf;        
Ak = fdjac(f,x(:,1),fk);   % start with FD Jacobian
jac_is_new = true;
I = eye(length(x));

lambda = 10; 
while (norm(s) > xtol) && (norm(fk) > ftol) && (k < maxiter)
    % Compute the proposed step.
    B = Ak'*Ak + lambda*I;
    z = Ak'*fk;
    s = -(B\z);

    xnew = x(:,k) + s;   fnew = f(xnew);
    
    % Do we accept the result?
    if norm(fnew) < norm(fk)    % accept
        y = fnew - fk;
        x(:,k+1) = xnew;  fk = fnew;  
        k = k+1;
        
        lambda = lambda/10;  % get closer to Newton
        % Broyden update of the Jacobian.
        Ak = Ak + (y-Ak*s)*(s'/(s'*s));
        jac_is_new = false;
    else                       % don't accept
        % Get closer to steepest descent.
        lambda = lambda*4;
        % Re-initialize the Jacobian if it's out of date.
        if ~jac_is_new
            Ak = fdjac(f,x(:,k),fk);
            jac_is_new = true;
        end
    end
end

if (norm(fk) > 1e-3)
    warning('Iteration did not find a root.')
end

end

function J = fdjac(f,x0,y0)
% FDJAC   Finite-difference approximation of a Jacobian.
% Input:
%   f        function to be differentiated
%   x0       evaluation point (n-vector)
%   y0       value of f at x0 (m-vector)
% Output       
%   J        approximate Jacobian (m-by-n)

delta = sqrt(eps);   % FD step size
m = length(y0);  
n = length(x0);
J = zeros(m,n);
I = eye(n);
for j = 1:n
    J(:,j) = ( f(x0+delta*I(:,j)) - y0) / delta;
end

end