siz = size(point);
N = siz(1);
cc = cos(angle);

syms fx fy cx cy c;
syms p1x p1y p2x p2y;
p1 = [p1x; p1y; 1];
p2 = [p2x; p2y; 1];
KK = [fx 0 cx;
     0 fy cy;
     0 0 1];
invKK = inv(KK);

% The equation to be minimized
Eq1 = transpose(invKK * p1) * (invKK * p2) - ...
      c * sqrt(transpose(invKK * p1) * (invKK * p1) * ...
               transpose(invKK * p2) * (invKK * p2));
Eq1 = simplify(Eq1);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
col = 'rgbcmky';
sty = ['- '; ': '; '-.'; '--'];
cN = length(col);
siz = size(sty);
sN = siz(1);

%%
%% f_x - c_x plot
%%
figure;
hold on;
for n = 1:N
  Eq2 = subs(subs(subs(subs(subs(Eq1, p1x, point(n,1)), p1y, point(n,2)), ...
                  p2x, point(n,3)), p2y, point(n,4)), c, cc(n));
  tmpEq = subs(subs(Eq2, fy, K(2,2)), cy, K(2,3));
  h = ezplot(tmpEq, [0,imgsize(2),imgsize(3),imgsize(4)]);
  set(h, 'LineStyle', sty(mod(n-1,sN)+1,:));
  set(h, 'Color', col(mod(n-1, cN)+1));
end
plot(K(1,3), K(1,1), 'o');
axis([0,imgsize(2),imgsize(3),imgsize(4)]);
%axis equal;
title('');
xlabel('c_x',  'FontName', 'Helvetica', ...
       'FontSize', 12);
ylabel('f_x', 'FontName', 'Helvetica', ...
       'FontSize', 12);
set(gca,'FontName', 'Helvetica')

%%
%% f_y - c_y plot (f_x - c_y plot)
%%
figure;
hold on;
for n = 1:N
  Eq2 = subs(subs(subs(subs(subs(Eq1, p1x, point(n,1)), p1y, point(n,2)), ...
                  p2x, point(n,3)), p2y, point(n,4)), c, cc(n));
  %tmpEq = subs(subs(Eq2, fy, K(2,2)), cx, K(1,3));
  tmpEq = subs(subs(Eq2, fx, K(1,1)), cx, K(1,3));
  h = ezplot(tmpEq, [0,imgsize(1),imgsize(3),imgsize(4)]);
  set(h, 'LineStyle', sty(mod(n-1,sN)+1,:));
  set(h, 'Color', col(mod(n-1, cN)+1));
end
%plot(K(2,3), K(1,1), 'o');
plot(K(2,3), K(2,2), 'o');
axis([0,imgsize(1),imgsize(3),imgsize(4)]);
%axis equal;
title('');
xlabel('c_y', 'FontName', 'Helvetica', ...
       'FontSize', 12);
% ylabel('f_x', 'FontName', 'Helvetica', ...
%        'FontSize', 12);
ylabel('f_y', 'FontName', 'Helvetica', ...
       'FontSize', 12);
set(gca,'FontName', 'Helvetica')

%%
%% x - y plot
%%
figure;
hold on;
for n = 1:N
  p = [point(n,1:2); point(n,3:4)];
  h = plot(p(:,1), p(:,2), ':*');
  set(h, 'LineStyle', sty(mod(n-1,sN)+1,:));
  set(h, 'Color', col(mod(n-1, cN)+1));
end
plot(K(1,3), K(2,3), 'o');
axis equal;
axis([0,imgsize(2),0,imgsize(1)]);
%title('');
xlabel('x', 'FontName', 'Helvetica', ...
       'FontSize', 12);
ylabel('y', 'FontName', 'Helvetica', ...
       'FontSize', 12);
set(gca,'FontName', 'Helvetica')
set(gca,'YDir','reverse');
