function dist=system_distance(A1,C1,K1,A2,C2,K2,fun)
% 
% This Function Computes the distance (subspace angles) between two systems
% The systems are given by their system matrices A,C and their Kalman Gain K.
% 
% inputs: AX,CX,KX - matricies from armodel
%         fun - function to assign a distance to a vector of angles
%               ex: sum(sin(x).^2)
%                   max(x)
% 
% System
%     x(k+1)      =   Ax(k)   +   v(k)
%     y(k)        =   Cx(k)   +   w(k)
%
% code written by Ashok Veeraraghavan
% modified by James Sherman



%if ((sum(sum(1-(A1==A2)))    + sum(sum(1-(C1==C2)))  + sum(sum(1-(K1==K2))))==0)
    %dist    =   0;
    %else

n   =   size(A1,1);
m   =   size(C1,1);

A   =   [A1 zeros(n,3*n);zeros(n) A2-K2*C2 zeros(n,2*n);zeros(n,2*n) A2 zeros(n);zeros(n,3*n) A1-K1*C1];
C   =   [C1 -C2 C2 -C1];

Q   =   dlyap(A',C'*C);
n=n/2;
temp=   [zeros(2*n) pinv(Q(1:2*n,1:2*n))*Q(1:2*n,2*n+1:4*n);pinv(Q(2*n+1:4*n,2*n+1:4*n))*Q(2*n+1:4*n,1:2*n) zeros(2*n)];
n=2*n;
E   =   abs(eig(temp));

E   =   max(-ones(size(E)),E);
E   =   min(ones(size(E)),E);
E   =   sort(E);
%theta   =   acos(E(1:n));
theta    =   acos(E(end-n+1:end));
thetamax    =   max(theta);



dist = fun(theta);

%dist        =   sum(sin(theta).^2);
%dist        =   sin(thetamax);
%end


%keyboard

%dist    =   sum(sum((C1-C2).^2));