clear all
close all
load('./processed_data');

% vary=0.7:0.1:1.3;
structures=1:10;
vary=1;
s=length(vary);
sa=length(structures);
fit=zeros(sa,5,s);
fit1=zeros(sa,5,s);
plot_data=zeros(sa,500,11,s);
size_f=zeros(sa,s);
R_1=zeros(sa,2,2,s);
P_upper=[50,10,pi,10];
P_lower=[5.2,0,-pi,0];
options=optimset('TolFun',1e-18,'TolX',1e-18);
P=[10, 5,0,2.3];

I0=1e-6;
for jj=1:s

for ii=structures
freq=a(ii).f;
[x_data,y_data]=rotate_scale(angle_rot,1,a(ii).LockXfilt,a(ii).LockYfilt);
fh=@(P)min_diff_coupled(P,I0,R_0(ii),...
                               a(ii).d,...
                               x_data,y_data,freq);

% fh1=@(P)min_diffMM_for_process(P,I0(ii,jj),R1(ii),F_angle(ii,jj),...
%                                x0,a(ii).d,...
%                                x_data,y_data,freq);
tic
[P_fit,resnorm,res]=lsqnonlin(fh,P,P_lower,P_upper,options);
toc
% [P_fit1,resnorm1,res1]=lsqnonlin(fh1,P,P_lower,P_upper,options);
% toc
% P_fit1=P_fit;resnorm1=resnorm;res1=res;
SS_res=sqrt(sum(res.^2))./length(freq);
% SS_res1=sqrt(sum(res1.^2))./length(freq);

% R_1(ii,:,:,jj)=SS_res;
fit(ii,1:length(P),jj)=P_fit;
% fit(ii,4,jj)=F(ii);
% fit(ii,5,jj)=R_0(ii);
% x_theory=x_data-res(:,1);
% y_theory=y_data-res(:,2);
Amp_0=I0.*R_0(ii);G=P_fit(1);x0=P_fit(4);
F=P_fit(2)*exp(1i*P_fit(3));
[X,Y]=coupledFM(Amp_0,G,abs(F),x0,angle(F),freq);
x_theory=filtfilt(a(ii).d.sosMatrix,a(ii).d.ScaleValues,X);
y_theory=filtfilt(a(ii).d.sosMatrix,a(ii).d.ScaleValues,Y);

plot_data(ii,1:length(freq),1:7,jj)=[freq x_data y_data ...
    x_theory y_theory res ];
size_f(ii,jj)=length(freq);

figure
plot(freq,x_data,freq,x_theory,freq,y_data,freq,y_theory);
% plot(freq,res)

end
end
% lambda_p=b.lambda_p;
% lambda_u=b.lambda_u;
% filename=['plot_data_MM_process_coupledFM_positive_shift' datestr(now,'yyyymmddTHHMMSS') '.mat'];
% save(filename,'plot_data','fit','size_f','R_0','vary',...
%               'structures');




