function X = rootpm_real_2(A,p)
%ROOTPM_REAL  Pth root of real matrix via real Schur form.
%   X = ROOTPM_REAL_2(A,P) is a Pth root of the nonsingular matrix A
%   (A = X^P).  When A has no eigenvalues on the negative real axis
%   then X is the principal pth root, which is real when A is real.

%   The implementation is based on rootpm_real function of the Matrix
%   Function Toolbox by Nick Higham 2008.

if norm(imag(A),1), error('A must be real.'), end
if p == 1, X = A; return; end

n = length(A);
[Q,R] = schur(A,'real');

% m blocks: i'th has order s(i), starts at t(i).
[m,s,t] = quasitriang_struct(R);
[b,c] = rootpm_binary_struct(p);
emme=size(c,2)-1;c0=c(1);

U = zeros(n); 
V = cell(c0+1); W = cell(emme+1);
for k=1:c0+1 V{k}=zeros(n);end
for k=1:emme W{k}=zeros(n);end 
W{emme+1}=R;
UD = cell(p+1); 

for j=1:m
    rj = t(j):t(j)+s(j)-1;
    if s(j) == 1
       % If A is real, X will be real unless R(p,p)<0 on the next line.
       U(rj,rj) = R(rj,rj)^(1/p);
    else
       U(rj,rj) = rootpm_root_block(R(rj,rj),p);
    end
    
    % Compute first U_ii^{k} and call them UD{k+1}
    rj = t(j):t(j)+s(j)-1;
    UD{1}(rj,rj)=eye(s(j));
    for k = 1:p, UD{k+1}(rj,rj)= U(rj,rj)^k; end
    % Compute V{k}(rj,rj) and W{k}(rj,rj)
    for k = 0:c0, V{k+1}(rj,rj)=U(rj,rj)^(2^k);end;W{1}(rj,rj)=V{c0+1}(rj,rj);
    for h = 1:emme, W{h+1}(rj,rj)=W{h}(rj,rj)*V{c(h+1)+1}(rj,rj);end
    
end

for j=2:m
    rj = t(j):t(j)+s(j)-1;
    for i=j-1:-1:1
        ri = t(i):t(i)+s(i)-1;
        
        clear B;clear C;
        B = cell(c0); C = cell(emme);
        % Compute Bk's
        for k = 1:c0
            B{k} = zeros(s(i),s(j));
            for ell = i+1:j-1
                rell = t(ell):t(ell)+s(ell)-1;
                B{k} = B{k} + V{k}(ri,rell)*V{k}(rell,rj);
                % Warning! B_k has the same index as B{k}, whilst V{k} is V_{k-1}
            end
        end
        % Compute Ch's
        for h = 1:emme
            C{h} = zeros(s(i),s(j));
            for ell = i+1:j-1
                rell = t(ell):t(ell)+s(ell)-1;
                C{h} = C{h} + W{h}(ri,rell)*V{c(h+1)+1}(rell,rj); % c(h+1)=c_h, V{c(h+1)+1}=V_{c_h}
            end
        end
        % Compute rhs = right hand side
        rhs = R(ri,rj);
        pow=p-2^(c0);
        for h = 1:emme
            pow=pow-2^c(h+1);
            rhs = rhs - C{h}*UD{pow+1}(rj,rj); % Recall that UD{k+1}=U^k
        end
        if (mod(p,2)==0) emmep=emme; else emmep=emme-1; end
        for h = 0:emmep
            G=[];
            G=rootpm_download(G,[0,c(h+1),0]); % c(h+1)=c_h
            siz=size(G,1);
            clear SUM;
            SUM=zeros(s(i),s(j));
            for h2 = 1:siz
                row = G(h2,1:3);
                erre=row(1);esse=row(2);ti=row(3);
                SUM=SUM+UD{erre+1}(ri,ri)*B{esse}*UD{ti+1}(rj,rj);
            end
            shif=0;
            for y=h+1:emme
                shif=shif+2^c(y+1);
            end
            ind1=p-2^c(h+1)-shif;ind2=shif;

            rhs = rhs - UD{ind1+1}(ri,ri)*SUM*UD{ind2+1}(rj,rj);
        end
        % Compute the coefficient matrix
        coeff = 0;
        for k = 0:p-1
            coeff = coeff + kron( UD{p-k}(rj,rj)', UD{k+1}(ri,ri) );
        end
        y = coeff\rhs(:);
        rhs(:) = y;                  % `Un-vec' the solution.
        U(ri,rj) = rhs;

        % Compute the new values of V and W for the next steps
        V{1}(ri,rj)=U(ri,rj);
        % Coputhe Vk's
        for k = 1:c0
            V{k+1}(ri,rj)=B{k}+V{k}(ri,ri)*V{k}(ri,rj)+V{k}(ri,rj)*V{k}(rj,rj);
        end
        % Compute Wk's
        W{1}(ri,rj)=V{c0+1}(ri,rj);
        for h = 1:emme-1
            W{h+1}(ri,rj)=C{h}+W{h}(ri,ri)*V{c(h+1)+1}(ri,rj)+W{h}(ri,rj)*V{c(h+1)+1}(rj,rj);
        end
   end
end

X = Q*U*Q';

%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function X = rootpm_root_block(R,p)
%ROOT_BLOCK  Pth root of a real 2x2 matrix with complex conjugate eigenvalues.

if norm(imag(R)) ~= 0 | any(size(R) - [2,2])
   error('Matrix must be real, of dimension 2.')
end

r11 = R(1,1); r12 = R(1,2);
r21 = R(2,1); r22 = R(2,2);

theta = (r11 + r22) / 2;
musq = (-(r11 - r22)^2 - 4*r21*r12) / 4;
mu = sqrt(musq);

if musq <= 0
   error('Matrix must have non-real complex conjugate eigenvalues.')
end

r = sqrt(theta^2+musq);
phi = angle(complex(theta,mu));
rootp = r^(1/p)*exp(i*phi/p);

alpha = real(rootp);
beta = imag(rootp);

X = alpha*eye(2) + (beta/mu)*(R - theta*eye(2));

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [b,c] = root_binary_struct(p)
%Compute b and c from the binary structure of p

b=dec2bin(p);
c0=size(b,2);

c(1)=c0-1;
b=bin2dec(b(:))';

h=1;
for k=2:c0
    b(k);
    if (b(k)==1)
        c(h+1)=c0-k;
        h=h+1;
    end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function G=rootpm_download(G,g)
% Compute the graph A_c

sc=0;
if (g(2)==1)
    G=[G;g];
else
    G=[G;g];
    c=g(2)-1;
    g1=[g(1)+2^c,c,g(3)];
    G=rootpm_download(G,g1);
    g2=[g(1),c,g(3)+2^c];
    G=rootpm_download(G,g2);    
end
