clc;
clear;
addpath('./tensor_toolbox_3.1')

% Parameters 
load('.\CSI_file\gain.mat');
load('.\CSI_file\theta_1.mat');
load('.\CSI_file\theta_2.mat');
load('.\CSI_file\theta_3.mat');
load('.\CSI_file\L1.mat');
load('.\CSI_file\L2.mat');

global verbose
verbose = 5000;

Nt = 64; 
Nr = 1;
My = 16; 
Mz = 16;
M = My*Mz;
% d = lameda/2; 
P0 = 128;
P = 16;
T = 16;
Q = 16;
SNR = -20:5:40;

V = exp(1j*2*pi*rand(M,Q)); 
F = exp(1j*2*pi*rand(Nt,T)); 
I = 1:P0;
I = I(1:P);
S = eye(P0);
S = S(I,:);
                                                                                                                                                                                       
turn = 500;
t_error_theta_1=zeros(1,length(SNR));
t_error_theta_2=zeros(1,length(SNR));
t_error_theta_3=zeros(1,length(SNR));
t_error_gain=zeros(1,length(SNR));
t_error_H=zeros(1,length(SNR));

t_crb_theta_1=zeros(1,length(SNR));
t_crb_theta_2=zeros(1,length(SNR));
t_crb_theta_3=zeros(1,length(SNR));
t_crb_gain=zeros(1,length(SNR));

total = length(SNR)*turn;
count = 1;

for j = 1:length(SNR)
    total_error_theta_1 = zeros(1,turn);
    total_error_theta_2 = zeros(1,turn);
    total_error_theta_3 = zeros(1,turn);
    total_error_gain = zeros(1,turn);
    total_error_H = zeros(1,turn);
    
    total_crb_theta_1 = zeros(1,turn); 
    total_crb_theta_2 = zeros(1,turn); 
    total_crb_theta_3 = zeros(1,turn); 
    total_crb_gain= zeros(1,turn); 
    for i = 1:turn

        [ Y,H, sigma_2,A,B,C] = Observation(theta_1,theta_2,theta_3,gain,V,F,S,SNR(j),My,Mz );
        dim_ratio=200;rank1 = L1*L2;

        %% CRB
        CRB_value = CRB( theta_1,theta_2,theta_3,gain,V,F,S,sigma_2,My,Mz,Nt,P0,rank1 );
        CRB_value = diag(CRB_value );
        crb_theta_1=real(sum(CRB_value(1:2*rank1)));
        crb_theta_2=real(sum(CRB_value((2*rank1+1):3*rank1)));
        crb_theta_3=real(sum(CRB_value((3*rank1+1):4*rank1)));
        crb_gain=real(sum(CRB_value((4*rank1+1):5*rank1)));
        

    %% Structured_CP 
        [est_Y,est_A,est_B,est_C,zl] = structured_CP(Y,Q,T,P,rank1);
        [est_H,est_f_MS,est_f_BS,est_f_F,est_gain,est_theta_1,est_theta_2,est_theta_3] = Par_estimation(est_Y,My,Mz,est_A,est_B,est_C,V,F,S,dim_ratio,zl);% theta_1²»Õý³£
        [error_theta_1,error_theta_2,error_theta_3,error_gain,error_H]=parameter_result(est_H,est_gain,est_theta_1,est_theta_2,est_theta_3,H,gain,theta_1,theta_2,theta_3);
        
        total_error_theta_1(i) = error_theta_1; 
        total_error_theta_2(i)=error_theta_2; 
        total_error_theta_3(i)=error_theta_3; 
        total_error_gain(i)=error_gain; 
        total_error_H(i)=error_H; 
        
        total_crb_theta_1(i) = crb_theta_1; 
        total_crb_theta_2(i) = crb_theta_2; 
        total_crb_theta_3(i) = crb_theta_3; 
        total_crb_gain(i)= crb_gain; 
        
        clc;
        fprintf('Progress:%.2f%%\n',(count/total)*100)
        count = count+1;
         
    end
    
    [value,index] = find(total_error_theta_1<1);
    t_error_theta_1(j) = mean(total_error_theta_1(index));
    [value,index] = find(total_error_theta_2<1);
    t_error_theta_2(j) = mean(total_error_theta_2(index));
    [value,index] = find(total_error_theta_3<1);
    t_error_theta_3(j) = mean(total_error_theta_3(index));
    [value,index] = find(total_error_gain<1);
    t_error_gain(j) = mean(total_error_gain(index));
    t_error_H(j) = mean(total_error_H);

    [value,index] = find(total_crb_theta_1<1);
    t_crb_theta_1(j) = mean(total_crb_theta_1(index));
    [value,index] = find(total_crb_theta_2<1);
    t_crb_theta_2(j) = mean(total_crb_theta_2(index));
    [value,index] = find(total_crb_theta_3<1);
    t_crb_theta_3(j) = mean(total_crb_theta_3(index));
    [value,index] = find(total_crb_gain<1);
    t_crb_gain(j) = mean(total_crb_gain(index));
end

figure(1)
subplot(2,2,1)
semilogy(SNR(5:end),t_error_theta_1(5:end),'ro-','linewidth',1.5);
hold on
semilogy(SNR(5:end),t_crb_theta_1(5:end),'b-','linewidth',1.5);
grid on
legend({'SCPD','CRB'},'Interpreter','latex','fontsize',10)
xlabel('$SNR(dB)$','Interpreter','latex','fontsize',13);
ylabel('MSE($\omega_{a},\omega_{e}$)','Interpreter','latex','fontsize',13);
axis([min(SNR(5:end)),max(SNR(5:end)),10^(-8),10^(-2)])

subplot(2,2,2);
semilogy(SNR(5:end),t_error_theta_2(5:end),'ro-','linewidth',1.5);
hold on
semilogy(SNR(5:end),t_crb_theta_2(5:end),'b-','linewidth',1.5);
grid on
legend({'SCPD','CRB'},'Interpreter','latex','fontsize',10)
xlabel('$SNR(dB)$','Interpreter','latex','fontsize',13);
ylabel('MSE($\phi$)','Interpreter','latex','fontsize',13);
axis([min(SNR(5:end)),max(SNR(5:end)),10^(-9),10^(-4)])

subplot(2,2,3);
semilogy(SNR(5:end),t_error_theta_3(5:end),'ro-','linewidth',1.5);
hold on
semilogy(SNR(5:end),t_crb_theta_3(5:end),'b-','linewidth',1.5);
grid on
legend({'SCPD','CRB'},'Interpreter','latex','fontsize',10)
xlabel('$SNR(dB)$','Interpreter','latex','fontsize',13);
ylabel('MSE($\iota$)','Interpreter','latex','fontsize',13);
axis([min(SNR(5:end)),max(SNR(5:end)),10^(-8),10^(-2)])

subplot(2,2,4);
semilogy(SNR(5:end),t_error_gain(5:end),'ro-','linewidth',1.5);
hold on
semilogy(SNR(5:end),t_crb_gain(5:end),'b-','linewidth',1.5);
grid on
legend({'SCPD','CRB'},'Interpreter','latex','fontsize',10)
xlabel('$SNR(dB)$','Interpreter','latex','fontsize',13);
ylabel('MSE($\beta$)','Interpreter','latex','fontsize',13);
axis([min(SNR(5:end)),max(SNR(5:end)),10^(-8),10^(-2)])


figure(2)
semilogy(SNR,smooth(t_error_H,3),'ro-','linewidth',1.5);
hold on
xlabel('$SNR(dB)$','Interpreter','latex');
ylabel('NMSE','Interpreter','latex');
legend('SCPD','OMP')
axis([min(SNR),max(SNR),10^(-5),10])
grid on
