% this is using fminsearch 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;
gamma=50;
delta=100;
K=300;    % Number of independent trials
noise_var_db=-60:10:60;    % noise power level in decibel
noise_var=10.^(noise_var_db./10);
sigma_n=1./noise_var;
L_n=length(sigma_n);
Cy=zeros(Mt*Mr);

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);

        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;
        
        %%%%% 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(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;
        y=reshape(Y,Mt*Mr,1);
        
        data(:,kk)=y;
        
    end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Use fminsearch to solve optimization problems

    %%%%% MAP estimate of phase and complex reflectivity %%%%%
    tic
    for tt=1:K
        dd=data(:,tt);
        
        A=gamma/(Mt*Mr*sigma_n(xx)*(K*gamma+sigma_n(xx)));
            
        W=@(x) -A*real(([1 exp(j*x(1)) exp(j*x(2)) exp(j*x(3)) exp(j*(x(1)+x(3))) exp(j*(x(2)+x(3))) ...
            exp(j*x(4)) exp(j*(x(1)+x(4))) exp(j*(x(2)+x(4)))]*dd)*([1 exp(j*x(1)) exp(j*x(2)) ...
            exp(j*x(3)) exp(j*(x(1)+x(3))) exp(j*(x(2)+x(3))) exp(j*x(4)) exp(j*(x(1)+x(4))) exp(j*(x(2)+x(4)))]*dd)')...
            -delta*(cos(x(1))+cos(x(2))+cos(x(3))+cos(x(4)));
            
        options=optimset('TolX',1e-6);    
        [x fval exitflag]=fminsearch(W,[0;0;0;0],options);  % Use fminsearch
                
        MAP_theta_t2(1,tt)=x(1);
        MAP_theta_t3(1,tt)=x(2);
        MAP_theta_r2(1,tt)=x(3);
        MAP_theta_r3(1,tt)=x(4);
        
        %% if you want to observe estimates, make K=1, and use below and
        %% compare:
        %% MAP_theta_t2 with t_t2;
        %% MAP_theta_t3 with t_t3;
        %% MAP_theta_r2 with t_r2;
        %% MAP_theta_r3 with t_r3; 
        
    end
    
    timing(1,xx)=toc;   % timing measure
    
    % Mean squared error of thetas
    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

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





