clear
close all
%% perform better fitting for 0 to 30dB
% load data
dat0 = load([pwd,'\NEP_0_sesn2mV_5000msTC_40acgain_4umstep_v1']);
dat10 = load([pwd,'\NEP_10_sesn500uV_5000msTC_40acgain_4umstep_v1']);
dat20 = load([pwd,'\NEP_20_sesn200uV_5000msTC_40acgain_4umstep_v1']);
dat30 = load([pwd,'\NEP_30_sesn20uV_5000msTC_40acgain_4umstep_v1']);
% dat40 = load([pwd,'\NEP_1pc_sesn10uV_5000msTC_40acgain_4umstep_v1']);

datall = [dat0 dat10 dat20 dat30];
attendb = [0 10 20 30];

fielddb = attendb/2;

%%lock a, free c
% initial vals
L0 = 0.48;
f0 = 3.4e12;
lambda = 3e8/f0;
C = 1;
alpha = -1.92;
V0 = 0;
dL = 0;
[Cstore2, alphastore2, betastore2, V0store2, dLstore2, fftamp2] = deal(zeros(length(attendb),1));
[plotstack2,figstack2] = createStackPlot(size(datall,2)/2);

j = 1;
for x = 1:size(datall,2)/2
    % load current data
    clear curdat
    curdat = datall(:,x*2-1:x*2);
    % scale to um and mV (AC coupled)
    curdat(:,1) = (curdat(:,1)-42.2)*1000;
    curdat(:,2) = (curdat(:,2)-mean(curdat(:,2)))*1000;
    % calculate initial params
    beta = (max(curdat(:,2))-min(curdat(:,2)))/2;
    data = [curdat(:,1)*1e-6 curdat(:,2)];
    curfft = abs(fft(curdat(:,2)));
    fftamp2(x) = max(curfft(4:6))/2;
    % set C and alpha for second data onwards
    if x ~= 1
        C = Cstore2(x);
        alpha = alphastore2(x);
    end
    %% perform intial dL & lambda fits
    params = dL;
    vars = [beta V0 C alpha L0 lambda];
    options = optimset('TolX',1e-6,'MaxFunEvals',100,'MaxIter',100);
    dL = fminsearch('SM_fit_dL',params,options,data,vars);

    params = lambda;
    vars = [beta V0 C alpha L0 dL];
    options = optimset('TolX',1e-6,'MaxFunEvals',100,'MaxIter',100);
    lambda = fminsearch('SM_fit_lambda',params,options,data,vars);
    
    %% Perform full fit
    if x==1
        % perform fit to 0dB atten
        params = [C alpha dL beta];
        vars = [V0 L0 lambda];
        options = optimset('TolX',1e-7,'MaxFunEvals',10000,'MaxIter',10000);
        [params,~] = fminsearch('SM_fit_4params',params,options,data,vars);
        
        % Store params
        betastore2(1) = params(4);
        Cstore2(1) = params(1);
        alphastore2(1) = params(2);
        dLstore2(1) = params(3);
        V0store2(1) = V0;
        % store alpha for higher attens
        for i = 2:size(datall,2)/2
            alphastore2(i) = alphastore2(1);
        end
    else
        params = [C dL beta];
        vars = [V0 L0 lambda alpha];
        options = optimset('TolX',1e-7,'MaxFunEvals',10000,'MaxIter',10000);
        [params,~] = fminsearch('SM_fit_3params',params,options,data,vars);
        betastore2(x) = params(3);
        Cstore2(x) = params(1);
        dLstore2(x) = params(2);
        V0store2(x) = V0;
    end
end
% perform linear fit to C vs beta data, for use as initial values in 2nd
% fit
fitdatx = [0;Cstore2];
fitdaty = [0;betastore2];
[fitresult, ~] = fit( fitdatx, fitdaty, 'poly1');
m = fitresult.p1;

%% lock a, calculate initial C from beta and gradient of first fit
% initial vals
lambda = 3e8/f0;
C = 1;
alpha = 0;
V0 = 0;
dL = 0;
[Cstore5, alphastore5, betastore5, V0store5, dLstore5, fftamp5] = deal(zeros(length(attendb),1));
[plotstack5,figstack5] = createStackPlot(size(datall,2)/2);
j = 1;
for x = 1:size(datall,2)/2
    clear curdat
    curdat = datall(:,x*2-1:x*2);
    curdat(:,1) = (curdat(:,1)-42.2)*1000;
    curdat(:,2) = (curdat(:,2)-mean(curdat(:,2)))*1000;
    % calc beta
    beta = (max(curdat(:,2))-min(curdat(:,2)))/2;
    original_beta(x) = beta;
    data = [curdat(:,1)*1e-6 curdat(:,2)];
    % fix noise point to help fit
    if x == 3
        data([40],:) = [];
    end
    curfft = abs(fft(curdat(:,2)));
    fftamp2(x) = max(curfft(4:6))/2;
    % calculate C from beta and gradient
    C = beta/m;
    original_C(x) = C;
    
    % apply more sensitive intial params
    if x ~= 1
        alpha = alphastore5(x);
    end
    if x == 2
        beta = betastore5(1);
        C = Cstore5(1);
        alpha = alphastore5(1);
        V0 = V0store5(1);
        dL = 4e-5;
    end
    if x == 3
        beta = m*Cstore5(2);
        C = 0;
        dL = 4.5e-5;
    end
    if x == 5
        dL = 4e-5;
    end
    
    plot(plotstack5(j),curdat(:,1),curdat(:,2),'b*');
    hold(plotstack5(j),'on');
    
    %% perform intial fits for dL and lambda
    params = dL; 
    vars = [beta V0 C alpha L0 lambda];
    options = optimset('TolX',1e-6,'MaxFunEvals',100,'MaxIter',100);
    dL = fminsearch('SM_fit_dL',params,options,data,vars);

    params = lambda; 
    vars = [beta V0 C alpha L0 dL];
    options = optimset('TolX',1e-6,'MaxFunEvals',100,'MaxIter',100);
    lambda = fminsearch('SM_fit_lambda',params,options,data,vars);
    
    %% perform full fit
    if x==1
        alpha = -1.92;
        params = [C alpha dL beta];
        vars = [V0 L0 lambda];
        options = optimset('TolX',1e-7,'MaxFunEvals',10000,'MaxIter',10000);
        [params,~] = fminsearch('SM_fit_4params',params,options,data,vars);
        first_fit_beta(x) = params(4);
        first_fit_C(x) = params(1);
        
        % set beta from fitted C and gradient
        beta = m*params(1);
        params = [C alpha dL]; 
        vars = [V0 L0 lambda beta];
        options = optimset('TolX',1e-7,'MaxFunEvals',10000,'MaxIter',10000);
        [params,~] = fminsearch('SM_fit_3params_C_alpha_dL',params,options,data,vars);
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % Store params
        betastore5(1) = beta;
        Cstore5(1) = params(1);
        alphastore5(1) = params(2);
        dLstore5(1) = params(3);
        V0store5(1) = V0;
        
        for i = 2:size(datall,2)/2
            alphastore5(i) = alphastore5(1);
        end
    else
        % use C and beta as free params
        params = [C beta];
        vars = [V0 L0 lambda alpha dL];
        options = optimset('TolX',1e-7,'MaxFunEvals',10000,'MaxIter',10000);
        [params,~] = fminsearch('SM_fit_2params_C_beta',params,options,data,vars);
        first_fit_beta(x) = params(2);
        first_fit_C(x) = params(1);
        
        % calc beta from fitted C and gradient
        beta = m*params(1);
        params = [C dL];
        vars = [V0 L0 lambda alpha beta];
        options = optimset('TolX',1e-7,'MaxFunEvals',10000,'MaxIter',10000);
        [params,~] = fminsearch('SM_fit_2params',params,options,data,vars);
        
        % store beta calced from C and grad
        betastore5(x) = beta;
        Cstore5(x) = params(1);
        dLstore5(x) = params(2);
        V0store5(x) = V0;
    end
    
    V_fit = zeros(1,length(curdat))';
    for loop = 1:length(curdat)
        z_inst = curdat(loop,1)*1e-6;
        L = L0 + dLstore5(x) + z_inst;
        V_fit(loop) = selmixpower(betastore5(x),Cstore5(x),L,lambda,alphastore5(x));
        V_fit(loop) = V_fit(loop) + V0store5(x);
    end
    plot(plotstack5(j),curdat(:,1),V_fit,'r');

    j=j+1;
end
figure(figstack5)
set(plotstack5(1:end-1),'xticklabel','');
set(get(plotstack5(end),'xlabel'),'string','Cavity Extension (\mum)','fontsize',12);
set(plotstack5(1:2),'ylim',[-1.2 1.2]);
set(plotstack5(3),'ylim',[-0.6 0.6]);
set(plotstack5(4),'ylim',[-0.2 0.2]);
[~,h]=suplabel('{\it{V}}_{SM} (mV)','y');
set(h,'fontsize',12);

% table(attendb',original_beta',first_fit_beta',betastore5,original_C',first_fit_C',Cstore5,'VariableNames',{'Attenuation';'beta_original';'beta_first_fit';'beta_end';'C_original';'C_first_fit';'C_end'})

figure
plot(Cstore5,betastore5,'*-');
fitdatx = [0;Cstore5];
fitdaty = [0;betastore5];
[fitresult, gof] = fit( fitdatx, fitdaty, 'poly1');
hold on
plot(fitresult);
m2 = fitresult.p1;
legend off
xlabel('C');
ylabel('\beta');

for x = 1:size(datall,2)/2
    [ecer(x),ecer_dB(x), reinjected_power(x)] = Sim__e_a__e_r(Cstore5(x),attendb(x));
end
table(attendb',Cstore5,ecer',ecer_dB',reinjected_power','VariableNames',{'Attenuation';'C';'EcEr';'EcEr_db';'Reinjected_Power'})
