%% load data
my_data = load('angle_sqz_oaf_short.txt');

%my_angle = 1.098.*my_data(:,1);
my_angle = my_data(:,1);

%my_sqz = 20*log10(my_data(:,2));
my_sqz = my_data(:,2);

my_oaf = my_data(:,3);

%% after sign flip
my_data_signflip = load('angle_sqz_oaf_signflip_short.txt'); %CLF sign flipped to +

%my_angle_signflip = 1.098.*my_data_signflip(:,1)-180;
my_angle_signflip = my_data_signflip(:,1)-180;

%my_sqz_signflip = 20*log10(my_data_signflip(:,2));
my_sqz_signflip = my_data_signflip(:,2);

my_oaf_signflip = my_data_signflip(:,3);

my_data_sort = load('angle_sqz_sorted.txt');
allangle = my_data_sort(:,1);
allsqz = my_data_sort(:,2);

%allangle = vertcat(my_angle,my_angle_signflip);
%allsqz = vertcat(my_sqz, my_sqz_signflip);

%% model

%Fractional Power to thershold
%x_ran = linspace(0,1,10000); 
%Classical parametric gain
%nlg = ((1+sqrt(x_ran))./(1-x_ran)).^2; %use this to figure out x
%nlg = ((1+x_ran)./(1-x_ran)).^2; %use this to figure out x
nlg = 2.17;
x = 1-sqrt(1/nlg);

%nlg_deamp = ((1-sqrt(x))./(1-x)).^2;

%x = 0.1916;
%x = 0.32;

%eff = linspace(.4,.7);
%theta = linspace(0,0.2); %rad

%Sqz/ anti-sqz (output variance)
%Sheila's thesis 2.72
asqz = @(var) 1 + ((var*4*x)./((1-x).^2)) ; %V+
sqz = @(var) 1 - ((var*4*x)./((1+x).^2)); %V_

%Sheila's thesis eq 2.71
V_theta = @(clf_theta, var) (asqz(var).*sin(clf_theta/2).^2) + (sqz(var).*cos(clf_theta/2).^2);
%V_theta = asqz.*sin(clf_deg./2).^2 + sqz.*cos(clf_deg./2).^2;


%Sheila's thesis eq 4.5
V_tot = @(clf_theta, var) sqrt( (V_theta(clf_theta+var(3), var(1)).*cos(var(2)).^2) + (V_theta( clf_theta+(pi)+var(3), var(1)) .*sin( var(2) ).^2)); 

%clf_deg = linspace(-500,200,500);
clf_deg = linspace(-300,200,500);
clf_rad = degtorad(clf_deg);
%r = 1.7;
allangle_rad = deg2rad(allangle);
real_sqz_angle = @(var) 2*acos( cos( rem((allangle_rad/2)+var(3),pi) )   ./ sqrt( (cos( rem((allangle_rad/2)+var(3),pi)  ).^2) + (var(4)^2.*sin( rem( (allangle_rad/2)+var(3),pi) ).^2)    ))+var(5);

func = @(var) (V_tot(real_sqz_angle(var), var)-allsqz); 

%efficiency, phase noise (rad), offset1 (rad), CLF angle offset (rad), SQZ angle offset (rad) 
lb = [.3,0, 0, 0, -2*pi];
ub = [.7,.5, 2*pi, 10, 2*pi];
x0 = [.5,.01, 3.1, 1.7, 2];
fit = lsqnonlin(func, x0, lb, ub)
%fit = [0.5186    0.1577    3.7890    1.7252   -1.0304]
%fit1 = [0.5186    0.1557   3.789    1.7252   -1.0953]

%RMSE = sqrt( abs(20*log10(V_tot( real_sqz_angle(fit), fit )).^2 - 20*log10(allsqz).^2)  );
%RMSE1 = sqrt( abs(20*log10(V_tot( real_sqz_angle(fit1), fit1 )).^2 - 20*log10(allsqz).^2) );
%meanRMSE=mean(RMSE)
%meanRMSE1=mean(RMSE1) %need to be 0.2161
%fill([allangle_rad;flipud(allangle_rad)],[20*log10(V_tot( real_sqz_angle(fit), fit ))-RMSE;flipud(20*log10(V_tot( real_sqz_angle(fit), fit ))+RMSE)],[.9 .9 .9],'linestyle','none');
%line(allangle_rad,20*log10(V_tot( real_sqz_angle(fit), fit )))
RMSE = ones(size(allangle_rad))*sqrt( mean( (20*log10(V_tot( real_sqz_angle(fit), fit )) - 20*log10(allsqz)).^2 )  );
%RMSE1 = ones(size(allangle_rad))*sqrt( mean((20*log10(V_tot( real_sqz_angle(fit1), fit1 )) - 20*log10(allsqz)).^2) );
meanRMSE=mean(RMSE)
%meanRMSE1=mean(RMSE1) %need to be 0.2161
%% plot shit

figure(101)
%subplot(2,1,1)

set(gcf,'color',[0.5, 0.5, 0.5]) 
fig = gcf;
fig.InvertHardcopy = 'off';

plot(allangle_rad, 20*log10(allsqz), '.', 'LineWidth', 1.5);
xlabel('CLF phase, deg'), ylabel('SQZ/ASQZ dB');
grid on
hold on
plot(allangle_rad, 20*log10(V_tot( real_sqz_angle(fit), fit )),'LineWidth', 1.5)
%plot(allangle_rad, 20*log10(V_tot( real_sqz_angle(fit1), fit1 )),'LineWidth', 1.5)

%plot(allangle, 20*log10(func(fit)), 'LineWidth', 1.5)
f = fill([allangle_rad;flipud(allangle_rad)],[20*log10(V_tot( real_sqz_angle(fit), fit ))-RMSE;flipud(20*log10(V_tot( real_sqz_angle(fit), fit ))+RMSE)],[0.8500, 0.3250, 0.0980]);
% Choose a number between 0 (invisible) and 1 (opaque) for facealpha.  
set(f,'facealpha',.4)
%f1 = fill([allangle_rad;flipud(allangle_rad)],[20*log10(V_tot( real_sqz_angle(fit1), fit1 ))-RMSE1;flipud(20*log10(V_tot( real_sqz_angle(fit1), fit1 ))+RMSE1)],[0.9290, 0.6940, 0.1250]);
% Choose a number between 0 (invisible) and 1 (opaque) for facealpha.  
%set(f1,'facealpha',.2)

%p1 = plot(allangle_rad, 20*log10(V_tot( real_sqz_angle(fit), fit )))

set(gca,'FontSize',18, 'Color', 'k');
hTitle = title('CLF phase (deg) vs. sqz BLRMs 2');
ylim([-3,5])
xlim([-3.5,5])

ax = gca;
ax.XColor = 'k'; 
ax.YColor = 'k'; 
ax.MinorGridColor = 'w';
ax.GridColor = 'w';
%legend({'\color{white} SQZ BLRMS CLF sign - (nominal)', '\color{white} SQZ BLRMS CLF sign + (CLF RF6 deg-180)'},'FontSize',14, 'Location', 'Northeast')
legend({'\color{white} measured','\color{white} fit (RMSE = 0.327)'},...
    'FontSize',14, 'Location', 'Northeast')
hold off
% 
% subplot(2,1,2)
% plot(angle, oaf, '.', 'LineWidth', 1.5)
% grid on
% hold on
% plot(angle_signflip, oaf_signflip, '.', 'LineWidth', 1.5)
% ylim([0,0.2])
% hold off
% 
% 

figure(102)

set(gcf,'color',[0.5, 0.5, 0.5]) 
fig = gcf;
fig.InvertHardcopy = 'off';

plot(allangle_rad, real_sqz_angle(fit), '.', 'LineWidth', 1.5);

xlabel('CLF rad'), ylabel('real rad');

set(gca,'FontSize',18, 'Color', 'k');
%hTitle = title('x vs nlg');
xlim([-3.5,5])
ax = gca;
ax.XColor = 'k'; 
ax.YColor = 'k'; 
ax.MinorGridColor = 'w';
ax.GridColor = 'w';
%legend({'\color{white} nlg'},'FontSize',14, 'Location', 'Northeast')
