%% FanPall Algorithm
%
% INPUT: A - Skew symmetric matrix of even or odd dimension
%        b - imaginary part of complex conjugated eigenvalues of principal
%            submatrix. length(b) < floor(length(A)/2) and values of
%            has to be in the intervalls of the eigenvalues of A as
%            specified in the Fan-Pall Theorem. If b(end) = 0, desiered
%            subprincipal has eigenvalue 0 and odd dimension
% OUTPUT:S - Matrix of same dimension as A, such that S'*A*S has the same
%            eigenvalues as A with complex conjugated eigenvalues of the
%            principal submatrix as specified by b
%
function S = FanPall_skew(A,b)
m = floor(length(A)/2);
q = length(b);
[a,b,isOdd,~] = FanPall_check(A,b);

%%
% Note if the dimension of A is odd, the principal submatrix has dimension
% 2*m, if the dimension of A is even, the principal submatrix hat dimension
% 2*(m-1). One iteration more is needed if dimension of A is odd. The
% number of iterations do not depend on if the submatrix is needed to be
% odd ( 2*(m-1-k)+1-th row and column is zero)
%
%% CALCULATE EIGENVALUES OF SUBMATRICES FOR m-q+isOdd ITERATIONS
EvalIter(:,1) = a;
b_ = b;
k = 0;
isOdd1 = isOdd;
cnt = 0;
while 1
    k = k+1;
    for j = 1:length(a)-k-1
        idx1 = find(EvalIter(j+1,k)-b_<=10^(-6) & EvalIter(j,k)>=b_-10^(-6));
        if ~isempty(idx1) && abs(b_(idx1(1))-EvalIter(j+1,k)) < 10^-8
            [EvalIter(j,k+1),idx] = max(b_(idx1));
        else
            [EvalIter(j,k+1),idx] = max([b_(idx1)', EvalIter(j+1,k)]);
        end
        if idx ~= length(idx1)+1
            idx2 = find(b_ == EvalIter(j,k+1));
            if idx2 ==1
                cnt = cnt+1;
                b_(idx2) = [];
            end
        end
    end
    if length(a)-k-1 <=0
        j=0;
        isOdd1 = 0;
    end
    idx1 = find(EvalIter(j+2,k)<=b_ & EvalIter(j+1,k)>=b_);
    [EvalIter(j+1,k+1),idx] = max([min(b_(idx1)),EvalIter(j+2,k)]);
    %isOdd1 = 0;
    if idx ~= length(idx1)+1
        cnt = cnt+1;
    end
    if cnt >= length(b) || k >= length(a)-length(b_)
        break;
    end
end

EvalIter = EvalIter(1:m-1+isOdd,2:end);

idx = zeros(length(b),1);
cnt = 1;
for i = 1:size(EvalIter,1)
    if cnt <= length(b) && EvalIter(i,end) == b(cnt)
        idx(cnt) = i;
        cnt = cnt+1;
    end
end

%% ITERATIONS OF ORTHOGONAL MATRIX CONSTRUCTION VIA FAN PALL
% first iteration (different for odd or even dimension of A)
[S,~,~] = FanPall(A,EvalIter(:,1));

% m-q-1+isOdd iterations (only Algorithm for even dimension of A is needed)
S_ = eye(size(S,1));
for k = 1:size(EvalIter,2)-1
    % transform A into matrix with principal submatrix with eigenvalues
    % specified in EvalIter
    A_ = S'*A*S;
    % extract principal submatrix block
    %A_ = A_(1:2*(m+isOdd)-2*k,1:2*(m+isOdd)-2*k);
    A_ = A_(1:2*(m)-2*k,1:2*(m)-2*k);
    % construct orthogonal matrix for principal submatrix block
    %[S_,~,~] = FanPall(A_,EvalIter(1:m-k-1+isOdd,k+1));
    [S_,~,~] = FanPall(A_,EvalIter(1:m-k-1,k+1));
    % add eye(.) for columns and rows not considered in the submatrix
    %S_ = [S_,zeros(2*(m+isOdd)-2*k,2*k-isOdd);zeros(2*(m+isOdd)-2*k,2*k-isOdd)',eye(2*k-isOdd)];
    S_ = [S_,zeros(2*(m)-2*k,2*k+isOdd);zeros(2*(m)-2*k,2*k+isOdd)',eye(2*k+isOdd)];
    S = S*S_;
end
%% Permutation Matrix
T = zeros(length(S));
for i = 1:length(idx)
    uv1 = zeros(length(S),1);
    uv2 = zeros(length(S),1);
    uv1(2*idx(i)-1) = 1;
    uv2(2*idx(i)) = 1;
    T(:,2*i-1) = uv2;
    T(:,2*i) = uv1;
end
j = length(idx)+1;
for i = 1:m
    if any(idx(:)==i)
    else
        uv1 = zeros(length(S),1);
        uv2 = zeros(length(S),1);
        uv1(2*i-1) = 1;
        uv2(2*i) = 1;
        T(:,2*j-1) = uv2;
        T(:,2*j) = uv1;
        j = j+1;
    end
end
if isOdd
    T(end,end) = 1;
end
S = S*T;
end