clear all;
% close all;
% run('../../../data_file_cleanup.m');
% run('../../../load_data.m');
% load('../n_simul_res_940.mat');
% lambda=852;
% [r_0,F]=MM_eff_par_imp(n1_fit_adj,z1_fit_adj,L1,lambda);
a(11)=load('../../../../20141113Window9MHz20mVDC/SR_filtered_data.mat');
a(11).ligain=10e3; a(11).dc=0.020;a(11).m=10;
% a(6)=load('../../../../20141117D69MHz20mVDC/SR_filtered_data.mat');
% a(6).ligain=10e3;a(6).dc=0.020;a(6).m=10;
global loop;
fit=zeros(10,5);
residue=zeros(500,2,10);
for i=11:11;

% % Rename data and smoothen
f1=(a(i).angular_freqaxis_ref-a(i).pks_real(1,3))/(2*pi);
f=(a(i).angular_freqaxis_ref-a(i).pks_real(1,3))/(2*pi);

LockX=a(i).LockX/a(i).m/a(i).ligain;
LockY=a(i).LockY/a(i).m/a(i).ligain;
h=a(i).h;
% Fstop=h.Fstop;Fpass=h.Fpass;Astop=h.Astop;Apass=h.Apass;
Astop=h.Astop;Apass=h.Apass;
Fstop=0.6e-3;Fpass=5e-3;
clear h;
LockXsmooth=smooth(f,LockX,95,'rloess');
LockYsmooth=smooth(f,LockY,95,'rloess');

% % Trim to integer multiple
stepsize=1;remainder=mod(length(f),stepsize);
f=f(1:stepsize:end-remainder);
LockXtrim=LockXsmooth(1:stepsize:end-remainder);
LockYtrim=LockYsmooth(1:stepsize:end-remainder);


% % Resample filter and design
h=fdesign.highpass(Fstop*stepsize,Fpass*stepsize,Astop,Apass);
d=design(h,'butter');

% % Apply filter
LockXfilt=filtfilt(d.sosMatrix,d.ScaleValues,LockXtrim);
LockYfilt=filtfilt(d.sosMatrix,d.ScaleValues,LockYtrim);

% % Trim
x_range_minus=100;
x_range_plus=100;
[~,f45]=min(abs(f));
fmin=f45-round(x_range_minus/abs(f(2)-f(1)));
fmax=f45+round(x_range_plus/abs(f(2)-f(1)));
f=f(fmin:fmax);
LockXfilt=LockXfilt(fmin:fmax);
LockYfilt=LockYfilt(fmin:fmax);

% % Generatle theory curve


I0=1e-6;
R_0=[0.884 0.876 0.707 0.685 0.513 0.303 0.366 0.702 0.897 0.972 0.077];
% P=[10, 0.2,real(F(i)),imag(F(i))];
Amp=1;
x0=-1.0795;

angle_rot=(deg2rad(206.5));
angle_vary=deg2rad(0);
angle_step=deg2rad(2);
index_step=2*angle_vary/angle_step;
index_total=index_step+1;
% [ret,mesg,~]=mkdir(directoryname);
P=[10,1,0,-1];
P_upper=[20, 20, pi,10];
P_lower=[5.2, 0,-pi,-10];
% directoryname=['D' num2str(i) '_F940nm_F_free_physics_test'];mkdir(directoryname);

for ii=1:index_total
% % Data rotation
% angle_phase=angle_rot+(ii-(index_total+1)/2)*angle_step;
% angle_phase=deg2rad(23);
loop=0;
tic
[x_data,y_data]=rotate_scale(angle_rot,1,LockXfilt,LockYfilt);
fh=@(P)(min_diff_coupled(P(1:4),I0,R_0(i),d,x_data,y_data,f));
options=optimset('TolFun',1e-20,'TolX',1e-20, 'MaxFunEvals', 1e3); 
P=lsqnonlin(fh,P,P_lower,P_upper,options);
fit(ii,1:length(P))=P
toc
residue(1:length(f),:,ii)=fh(P);
G=P(1);
F=P(2)*exp(1i*P(3));
x0=P(4)
Amp_0=I0.*R_0(i);

[X,Y]=coupledFM(Amp_0,G,abs(F),x0,angle(F),...
                                       f');
[LockXfiltrotate,LockYfiltrotate]=...
     rotate_scale(angle_rot,1,LockXfilt,LockYfilt);

SR_x_theoryfilt=filtfilt(d.sosMatrix,d.ScaleValues,X);
SR_y_theoryfilt=filtfilt(d.sosMatrix,d.ScaleValues,Y);

% figure;
% plot(LockXfiltrotate,LockYfiltrotate);hold on;
% l=-0.06:0.03:0.06;
% plot(l,l,'r');

figure;
subplot(2,1,1)
plot(f,LockXfiltrotate,'b');hold on;plot(f,LockYfiltrotate,'r')
plot(f,SR_x_theoryfilt,'--b');
plot(f,SR_y_theoryfilt,'--r');
xlabel('Frequency (MHz)');title(['D ' num2str(i) '. F rotate ' num2str(rad2deg((ii-(index_total+1)/2)*angle_step)) '°']);
legend('X data','Y data', 'X fit', 'Y fit');
subplot(2,1,2)
plot(f,residue(1:length(f),1,ii),'k',f,residue(1:length(f),2,ii),'m');
xlim([-150 150]);
ylim([-0.00000002 0.00000002]);
legend('X residue', 'Y residue');
% filename=['./' directoryname '/Angle_rot_' num2str(rad2deg((ii-(index_total+1)/2)*angle_step))];
% print('-dpdf',filename)
% print('-dtiff',filename)    
% print('-depsc2',filename)
% save(['./' directoryname '/D' num2str(i)],'F_new','r_0','angle_phase','f','residue','fit',...
% 'LockXfiltrotate','LockYfiltrotate','SR_x_theoryfilt','SR_y_theoryfilt');

end
fit;
sum(sum(abs(residue(:,:,:))));
end
save('window.mat','fit','SR_x_theoryfilt','SR_y_theoryfilt','LockXfiltrotate','LockYfiltrotate','angle_rot','residue','f')