% this is using Trust-Region algorithm to solve 4-dimensional optimizations

clear all
clc

%%%%% Start to construct MIMO radar receive signals model %%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%

Mt=3;             % number of transmitters
Mr=3;             % number of receivers
N=20;             % number of samples per signal
gamma=50;         % RCS
delta=100;        % Von Mises distribution parameter
K=300;            % number of independent trials
noise_var_db=-60:10:60;     % noise power level
noise_var=10.^(noise_var_db./10);
sigma_n=1./noise_var;
L_n=length(sigma_n);

Cy=zeros(Mt*Mr);

wait=waitbar(0,'Please wait...');

for xx=1:L_n
    xx
    
    for kk=1:K
       
        %%%%% complex reflectivity %%%%%
        Rh=ones(Mt*Mr,Mt*Mr);
        a(1,kk)=sqrt(gamma/2)*(randn(1,1)+j*randn(1,1));
        h=a(1,kk)*ones(Mt*Mr,1);
        H=reshape(h,Mt,Mr);

        %%%%% generate transmitted signals %%%%%
        S=1/sqrt(2)*(randn(N*Mt,1)+j*randn(N*Mt,1));     
        S=reshape(S,N,Mt);
    
        %%%%% reduced QR factorization %%%%%
        vc=zeros(N,Mt);
        rc=zeros(Mt,Mt);
        
        vc(:,1)=S(:,1);
        rc(1,1)=norm(vc(:,1));
        qc(:,1)=vc(:,1)./rc(1,1);

        for ii=2:Mt
            vc(:,ii)=S(:,ii);
            for jj=1:ii-1
                rc(jj,ii)=qc(:,jj)'*S(:,ii);
                vc(:,ii)=vc(:,ii)-rc(jj,ii)*qc(:,jj);        
            end
            rc(ii,ii)=norm(vc(:,ii));
            qc(:,ii)=vc(:,ii)./rc(ii,ii);
        end

        Q=qc;          % orthonormal transmit signals
        
        %%%%% CDF of Von Mises distribution %%%%%
        thetavec=pi*(-999.5:999.5)/1000;   
        pmf=exp(delta*cos(thetavec));   
        pmf=pmf/sum(pmf);
        cmf=zeros(1,2000);   
        cmf(1)=pmf(1);   
        for ii=2:2000       
            cmf(ii)=cmf(ii-1)+pmf(ii);   
        end

        %%%%% generate phase mismatches %%%%%
        for ii=1:Mt+Mr-2
            nn=2000;      % 2000 samples of Von Mises distribution
            u=(rand(1,1)<cmf);       
            q=u-[0 u(1:nn-1)];       
            [qmax,tt]=max(q);              
            theta(ii,1)=thetavec(tt);
        end
        
        theta_t=[0;theta(1);theta(2)];    % true transmitter's phases, phi_t-1=0
        theta_r=[0 theta(3) theta(4)];    % true receiver's phases, phi_r-1=0

        phi_t=diag(exp(-j*theta_t));
        phi_r=diag(exp(-j*theta_r));
        
        t_t2(1,kk)=theta(1);
        t_t3(1,kk)=theta(2);
        t_r2(1,kk)=theta(3);
        t_r3(1,kk)=theta(4);

        %%%%% generate noise %%%%%
        w=sqrt(sigma_n(1,xx)/2)*(randn(N*Mr,1)+j*randn(N*Mr,1));
        w=reshape(w,N,Mr);
        
        %%%%% data model %%%%%
        Z=Q*phi_t*H*phi_r+w;
        Y=Q'*Z;          % pre-process 
        y=reshape(Y,Mt*Mr,1);
        
        data(:,kk)=y;
        
    end
    
%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Use Trust-Region algorithm to solve optimization problems

    %%%%% MAP estimate of phase and complex reflectivity %%%%%
    tic;
       
    for tt=1:K
        
        waitbar(tt/K)
        
        dd=data(:,tt);
        
        A=gamma/(Mt*Mr*sigma_n(1,xx)*(K*gamma+sigma_n(1,xx)));
        
    %%%%% use trust-region algorithm %%%%%
    
        syms pt2 pt3 pr2 pr3
        
        P1=[1 exp(j*pt2) exp(j*pt3) exp(j*pr2) exp(j*(pt2+pr2)) exp(j*(pt3+pr2)) ...
            exp(j*pr3) exp(j*(pt2+pr3)) exp(j*(pt3+pr3))];
        P2=[1;exp(-j*pt2);exp(-j*pt3);exp(-j*pr2);exp(-j*(pt2+pr2));exp(-j*(pt3+pr2));...
            exp(-j*pr3);exp(-j*(pt2+pr3));exp(-j*(pt3+pr3))];
            
        f=-A*(P1*dd)*(dd'*P2)...
            -delta*(cos(pt2)+cos(pt3)+cos(pr2)+cos(pr3));
            
        
        df=[diff(f,pt2);diff(f,pt3);diff(f,pr2);diff(f,pr3)];
        
        ddf=[diff(df(1,1),pt2),diff(df(1,1),pt3),diff(df(1,1),pr2),diff(df(1,1),pr3);diff(df(2,1),pt2),diff(df(2,1),pt3),diff(df(2,1),pr2),diff(df(2,1),pr3);diff(df(3,1),pt2),diff(df(3,1),pt3),diff(df(3,1),pr2),diff(df(3,1),pr3);diff(df(4,1),pt2),diff(df(4,1),pt3),diff(df(4,1),pr2),diff(df(4,1),pr3)];
        
        x0=0;
        y0=0;
        z0=0;
        t0=0;
        
        X0=[x0;y0;z0;t0];    % initial point
            
        radius_0=1;        % initial radius
        radius_hat=1.5;
        eta=1/8;
        Tol=1e-6;          % tolerent for terminating Newton iteration
        nMax=100;          % maximum iterates 
        n=1;               % count iterate step

        pt2=X0(1,1);
        pt3=X0(2,1);
        pr2=X0(3,1);
        pr3=X0(4,1);

        Xk=X0;             % xk
        fk=eval(f);        % fk
        gk=eval(df);       % delta fk
        radius_k=radius_0; % initial trust region radius

        Bk_old=eval(ddf);     % initial Bk
        Bk=Bk_old;
        Hk=inv(Bk);        % initial Hk

        % choose different methods (in this project I use DFP+Dogleg or DFP+2-dimensional minimization)
        pk=Dogleg(radius_k,df,Bk,Hk,Xk);

        while (norm(pk)>Tol&&n<=nMax)
            
            [Bk,Hk]=DFP(ddf,Xk,df,f,Bk_old);

            Bk_old=Bk;

            pk=Dogleg(radius_k,df,Bk,Hk,Xk);
%             pk=Two_dimension(radius_k,df,Bk,Hk,Xk);
    
            rou_k=solve_rou_k(Xk,f,df,pk,Bk);               % to solve rou
    
            if rou_k<1/4
                radius_k=radius_k/4;
            else if (rou_k>3/4 && norm(pk)==radius_k)
                    radius_k=min(radius_hat,2*radius_k);
                else radius_k=radius_k;
                end
            end
    
            if rou_k>eta
                Xk=Xk+pk;
            else Xk=Xk;
            end

%%%%%%%%%%%%%% 
            g_drawing(1,n)=norm(eval(df));
%%%%%%%%%%%%%%
    
            n=n+1;
        end
          
        MAP_theta_t2(1,tt)=real(Xk(1,1));
        MAP_theta_t3(1,tt)=real(Xk(2,1));
        MAP_theta_r2(1,tt)=real(Xk(3,1));
        MAP_theta_r3(1,tt)=real(Xk(4,1));
        
    end
    
    timing(1,xx)=toc;

% if you want to observe and compare the estimates with true value, let K=1
% and use below:
%     t_t2
%     MAP_theta_t2
%     t_t3
%     MAP_theta_t3
%     t_r2
%     MAP_theta_r2
%     t_r3
%     MAP_theta_r3
%     a
%     MAP_a

    % mean squared error of theta     
    MSE_theta(1,xx)=((MAP_theta_t2-t_t2)*(MAP_theta_t2-t_t2)'/K+(MAP_theta_t3-t_t3)*(MAP_theta_t3-t_t3)'/K...
        +(MAP_theta_r2-t_r2)*(MAP_theta_r2-t_r2)'/K+(MAP_theta_r3-t_r3)*(MAP_theta_r3-t_r3)'/K)/4;
    
end

close(wait);

ii=1:L_n;
figure(1)
plot(noise_var_db(ii),10*log10(MSE_theta(ii)))
hold on








