%Mike Novey
%University of Maryland Baltimore County
%2/26/09    From "The Complex Generalized Gaussian Distribution---"
%                  Characterization, Generation, and Estimation
%%%Function to estimate the shape parameter, c=1 Gaussian, and Covariance
%Of the bivariate GGD distribution
%X = 2 x N augmented data, i.e., X = [z, z*]^T
%shape parameter c = 1 is Gausssian
%Converged is a 1 if converged and 0 if not
%C is augmented covariance, i.e., E{ X X^H}   

function [c,R] = estimateGGDCovShape(X)

mom=1;
converged = 1;
N = size(X,2);
maxCount = 50;
count = 0;
R = cov(X'); %Consistant estimator but not MLE
bestC = max(.5,.1*randn+1); %start at Gaussian
%Moment estimator to get c in ball park prior to MLE
%Maximize using iterations over c
if mom == 1
    %Moment estimator for c to get in ball park
    bestVal = 1e10;
    x = real(X(1,:));y = imag(X(1,:));
    mm = mean(x.^4)/mean(x.^2)^2 + mean(y.^4)/mean(y.^2)^2;
    for cc = [[.1:.1:.45] [.5:.5:1.5] [2:4]]
        temp = 3*gamma(1/cc)*gamma(3/cc)/gamma(2/cc)^2 -mm; %root
        if abs(temp) < bestVal
            bestVal = abs(temp);
            bestC = cc;
        end
    end
end
c = bestC;
Rold = zeros(2,2);
cold = 0;
delt = 10;
toll = .001;
while count < maxCount && delt > toll
    %R = [[2 .25]' [.25 3]'];
    %Trace runs slower
    %     xRxC = real(trace((X'*inv(R)*X).^c));
    %     dirXRX = real(trace( log(X'*inv(R)*X).* ((X'*inv(R)*X).^c)));
    %     dirXRX2 = real(trace( (log(X'*inv(R)*X).^2).* ((X'*inv(R)*X).^c)));
    xRxC = 0;
    dirXRX = 0;
    dirXRX2 = 0;
    for n = 1:N
        temp = (X(:,n)'*inv(R)* X(:,n));
        xRxC = xRxC +   real(temp^(c));
        dirXRX = dirXRX + real(log(temp)*temp^(c));
        dirXRX2 = dirXRX2 + real(log(temp)^2*temp^(c));
    end
    %Test
    %      c = .4;
    %      xRxC = 1.426^c;
    %      dirXRX = log(1.426)*1.426^c
    %      dirXRX2 = log(1.426)^2 *1.426^c;
    %      N = 1;
    %%%%

    c2 = gamma(2/c)/(2*gamma(1/c));
    c2p = log(c2) - inv(c)*(2*psi(2/c)-psi(1/c));
    gc = N*(inv(c) - inv(c^2)*2*psi(2/c)+inv(c^2)*2*psi(1/c))-(c2^c)*(c2p*xRxC + dirXRX);

    %%Second dir
    A = N*((4*psi(2/c)/c^3) + (4*psi(1,2/c)/c^4)-(1/c^2) - (4*psi(1/c)/c^3) - (2*psi(1,1/c)/c^4));
    %Dir c2^c
    dc2C = log(c2)*(c2^c) - c*(c2^(c-1))*(c2*2*psi(2/c)/c^2 - c2*psi(1/c)/c^2);
    dc2p= -((psi(1/c) - 2*psi(2/c))/c^2) - ((psi(1,1/c) - 4*psi(1,2/c))/c^3) - ...
        ((2*psi(2/c)/c^2) - psi(1/c)/c^2);
    B = dc2C*c2p *xRxC + c2^c*(dc2p*xRxC + c2p*dirXRX);
    C = dc2C*dirXRX + c2^c*(dirXRX2);
    ggc = A - B - C; %Testted with MAthacd
    cold = c;
    cn = c-inv(ggc)*gc;
    c = min(4,max(.05,cn)); %Newton update with no negatives
    % delt = norm(Rold-R);
    delt = abs(c-cold);
    % disp([num2str(count) ' ' num2str(c)]);
    count=count+1;
end %while

%%Do one last fixed point on cov
%Fixed point only contraction mapping with c < 1.55 (empirical)

delt = 10;
count = 0;
if c < 1
    while count < maxCount && delt > toll
        Rn = zeros(2,2);
        for n = 1:N
            temp = 2*(c2^c)*c*((X(:,n)'*inv(R)* X(:,n))^(c-1))*ones(2,2);
            Rn = Rn + temp.*(X(:,n)*X(:,n)');
        end
        Rold = R;
        R = (Rn/N);
        %Test for convergence
        if det(R) <= 0 || isnan(sum(sum(R))) || isinf(sum(sum(R))) %Did not converge
            R = cov(X');
            disp(['Did not converge with c = ' num2str(c)]);
            converged = 0;
            break;
        end
        delt = norm(Rold-R);
        count = count + 1;
    end
end