folderpath = 'C:\Users\sioe\Documents\High_Speed_Measurement_2024\baudrate_sweep_b2b\';
experiment_name = 'PAMX_b2b_baudrate';
currentTime = datetime('now', 'Format', 'yyyyMMdd_HHmmss');
timeStr = char(currentTime);
experiment_name = [experiment_name, timeStr];

ffe_only            = 0;
postfilter_approach = 1;
db_channel_approach = 0;
db_coding_approach  = 0;

db_precode = 0;%db_coding_approach || db_channel_approach;

%%% SIR Sweep for MPI Experiment %%%
params = struct;

params.fsym = [100:6:200].*1e9; %2.67; % PAM6=2.3V %PAM8=2.68V
params.rop_atten = 0:0.5:7;
params.M = [8,6,4];

wh = DataStorage(params);

wh.addStorage("ber_ffe");
wh.addStorage("ber_mlse");
wh.addStorage("ber_db");
wh.addStorage("pd_in");
wh.addStorage("rop");
wh.addStorage("M");
wh.addStorage("signals");
wh.addStorage("v_bias");
wh.addStorage("awg_vpp");
wh.addStorage("precomp_amp_max")

precomp_path = "C:\Users\sioe\Documents\High_Speed_Measurement_2024\precomp\";
precomp_fn = "lab_high_speed";
precomp_mode = 2; %0=do nothing ; 1= measure; 2=precomp active

precomp_amp_max = 5;
awg_vpp         = 2.7;
random_key      = 2;
pd_in_set       = 7;

looptotal = prod(wh.dim);

disp(['Start Measurement of ',num2str(looptotal),' loops...'])
iterationTimes = zeros(looptotal, 1); % Preallocate for speed
if ~exist('hWaitbar', 'var') || ~isvalid(hWaitbar)
    hWaitbar = waitbar(0, sprintf('Starting %d measurements',looptotal), 'Name', 'Processing Progress');
else
    waitbar(0, hWaitbar, sprintf('Starting %d measurements',looptotal));
end

loopcnt = 0;
estimatedTimeRemaining = 0;
estimatedTotalTime = 0;

for M = wh.parameter.M.values
    %%%%% 1) SET Voltages for each modulation format once %%%%%%
    if M == 4
        v_bias = 2.1;
    elseif M == 6
        v_bias = 2.3;
    elseif M == 8
        v_bias = 2.67;
    end

    dcs = DC_supply("active",[1,0],"voltage",[v_bias, 0]);
    dcs.set("voltage",[v_bias, 0]);
    %%%%% SET Voltages %%%%%%

    if M ~= 8
        pause(30*60); %wait 30 minutes for stable bias
    end

    for fsym = wh.parameter.fsym.values
        
        %%%% 2) PREARE THE TX SIGNAL ONCE FOR EACH FSYM RATE %%%%

        %%%%% Construct AWG and Scope Modules %%%%%%
        fdac        = 256e9;
        fadc        = 256e9;
        SCP = ScopeKeysight("model","UXR1104B",'autoscale',1,"fadc","GSa_256","channel",[0,1,0,0],"recordLen",4000000,"removeDC",1);
        AWG = AwgKeysight("model","M8199B","fdac",fdac,"scaletodac",[1,1],"skews",[0,0],"voltages",[0,awg_vpp]);
        A2S = Awg2Scope(AWG,SCP,[0,2,0,0],"waitUntilClick",0); %

        %%%%% Symbol Generation %%%%%%
        Pform       = Pulseformer("fsym",fsym,"fdac",4*fsym,"pulse","rrc","pulselength",16,"rrcalpha",0.05);

        [Digi_sig,Symbols,Bits] = PAMsource(...
            "fsym",fsym,"M",M,"order",19,"useprbs",1,...
            "fs_out",fdac,...
            "applyclipping",0,"clipfactor",1.7,...
            "applypulseform",0,"pulseformer",Pform,...
            "randkey",random_key,...
            "db_precode",db_precode,"db_encode",db_coding_approach,...
            "mrds_code",0,"mrds_blocklength",512).process();

        %%%%% Precompensation Routine %%%%%%
        precomp_est = ChannelFreqResp("Nacq",1024,"Navg",64,"Ncp",63,'f_ref',Digi_sig.fs);
        Digi_sig = precomp_est.precomp(Digi_sig,'maxampdb',precomp_amp_max,'loadPath',precomp_path,'fileName',precomp_fn);

        %%%%% Resample to DAC rate %%%%%%
        Digi_sig = Digi_sig.resample("fs_out",AWG.fdac);

        %%%%% Plot and Save Routine 1 %%%%%%%%%%%%%%%%%%%%%%%%%
        %Digi_sig.spectrum("displayname","Normal Tx","fignum",10);
        loop_name = ['PAM_',num2str(M),'_fsym_',num2str(fsym.*1e-9)];
        save([folderpath,experiment_name,loop_name,'_bits'],"Bits");
        save([folderpath,experiment_name,loop_name,'_symbols'],"Symbols");
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

        for rop_atten = wh.parameter.rop_atten.values

            %%%%% Loop Preps
            iterationStartTime = tic;
            loopcnt = loopcnt+1;

            %%%%% SET Attenuator %%%%%%
            voa = OptAtten("active",[1,2,1,1],"value",[rop_atten,pd_in_set,0,0],"wavelength",[1310,1310,1310,1310]);
            voa.set('active',[1,2,1,1],'value',[rop_atten,pd_in_set,0,0]);

            %%% HERE SHOULD BE THE DATA PREPARATION WHICH IS NOW IN BETWEEN
            %%% THE LOOPS :-) %%%

            %%%%% AWG --> Scope %%%%%%
            [~,Scpe_sig_raw,~,D] = A2S.process("signal2",Digi_sig,"waitUntilClick",0);

            %%%%%% Sample to 2x fsym %%%%%%
            Scpe_sig_resampled = Scpe_sig_raw.resample("fs_in",fadc,"fs_out",2*fsym);

            voa.readvals();
            rop = voa.power_state(1);
            pd_in = voa.power_state(2);
            % disp(['ROP: ',num2str(rop),' dBm || PD in: ',num2str(pd_in), ' dBm']);

            %%%%%% Sync Rx signal with reference (S is a cell array with all occurences) %%%%%%
            [Scpe_sig_syncd,S,isFlipped] = Scpe_sig_resampled.tsynch("reference",Symbols,"fs_ref",fsym);

            %%%%% Plot and Save Routines: SAVE RECEIVED SIGNALS %%%%%%%%%%%%%%%%%%%%%%%%%
            loop_name = ['PAM_',num2str(M),'_fsym_',num2str(fsym.*1e-9),'_rop_',num2str(rop_atten)];
            loop_name = strrep(loop_name,'.','_'); 
            save([folderpath,experiment_name,loop_name,'_rx_signal'],"S");

            %%%%% EQUALIZE %%%%%%
            Eq = FFE("epochs_tr",5,"epochs_dd",5,"len_tr",4096*2,"mu_dd",1e-4,"mu_tr",0,"order",50,"sps",2,"decide",0);
            % Eq = VNLE("epochs_tr",5,"epochs_dd",5,"len_tr",4096*2,"mu_dd",[0.0004 0.0005 0.0006],"mu_tr",0,"order",[50,7,7],"sps",2,"decide",1);
            Eq = EQ("Ne",[50,7,7],"Nb",[0,0,0],"training_length",4096*2,"training_loops",5,"dd_loops",5,"K",2,"DCmu",0.05,"DDmu",[0.0004 0.0004 0.0004 0.0004 ],"DFEmu",0.005,"FFEmu",0,"plotfinal",0,"ideal_dfe",1);

            % set to minus one not zero not avoid confusion if BER is acutally zero
            ber_ffe = -1;
            ber_mlse = -1;
            ber_db = -1;

            if ffe_only %%%%%%%%%%%%%%%%%%%%%%%%%%%

                [EQ_sig] = Eq.process(Scpe_sig_syncd,Symbols);

                % EQ_sig.plot("fignum",50,"displayname",'After EQ');

                Rx_bits = PAMmapper(M,0).demap(EQ_sig);

                [~,errors_bm,ber_ffe,errors] = calc_ber(Rx_bits.signal,Bits.signal,"skip_front",100,"skip_end",150,"returnErrorLocation",1);

                disp(['FFE: ',sprintf('%.1E',ber_ffe),'| ROP: ',num2str(rop),' dB | PD_in: ',num2str(pd_in),' dBm']);

                if 0

                    EQ_sig.plot("fignum",50,"displayname",'After EQ','clear',1);


                    figure(56);
                    clf
                    title(sprintf('PAM %d ; BER: %1.2e',M, ber_ffe));
                    constellation = unique(Symbols.signal);
                    received = NaN(numel(constellation),length(Symbols));
                    for lvl = 1:numel(constellation)
                        %Separate the equalized signal into the
                        %respective levels based on the actually
                        %transmitted level!
                        received(lvl,Symbols.signal==constellation(lvl)) = EQ_sig.signal(Symbols.signal==constellation(lvl));
                        intermediate = received(lvl,:);
                        cnt(lvl) = numel(intermediate(~isnan(intermediate)));
                        hold on
                        histogram(received(lvl,:),1000,"EdgeAlpha",0,'DisplayName',['Lvl ',num2str(lvl),' | ',num2str(cnt(lvl)),' entries']);
                    end
                    legend
                end

            elseif postfilter_approach %%%%%%%%%%%%%%%%%%%%%%%%%%%

                [EQ_sig] = Eq.process(Scpe_sig_syncd,Symbols);

                % EQ_sig.plot("fignum",50,"displayname",'After EQ','clear',1);

                Noi = EQ_sig-Symbols;

                Rx_bits = PAMmapper(M,0).demap(EQ_sig);

                [~,num_errors,ber_ffe,pos_errors] = calc_ber(Rx_bits.signal,Bits.signal,"skip_front",100,"skip_end",150,"returnErrorLocation",1);

                nc = 2;
                burg_coeff = arburg(Noi.signal,nc);

                EQ_sig = EQ_sig.filter(burg_coeff,1);

                if 0
                    Noi.spectrum('displayname','Noise PSD','fignum',123)
                    [h,w] = freqz(1,burg_coeff,length(Noi),"whole",Noi.fs);
                    h = h/max(abs(h));
                    hold on
                    w_ = (w - Noi.fs/2);
                    plot(w_.*1e-9,20*log10(fftshift(h)),'DisplayName',['', num2str(nc), ' coefficients for burg alg.']);
                end

                EQ_sig = MLSE("DIR",burg_coeff,"duobinary_output",0,"M",M,"trellis_states",PAMmapper(M,0).levels).process(EQ_sig);

                Rx_bits = PAMmapper(M,0).demap(EQ_sig);

                [~,num_errors,ber_mlse,pos_errors] = calc_ber(Rx_bits.signal,Bits.signal,"skip_front",100,"skip_end",150,"returnErrorLocation",1);

                disp(['FFE: ',sprintf('%.1E',ber_ffe),' -> PF -> MLSE: ',sprintf('%.1E',ber_mlse),'  dB | PD_in: ',num2str(pd_in),' dBm']);

            elseif db_channel_approach %%%%%%%%%%%%%%%%%%%%%%%%%%%
                
                if db_precode

                    [EQ_sig, Noi] = Eq.process(Scpe_sig_syncd,Duobinary().encode(Symbols));
    
                    EQ_sig.plot("fignum",50,"displayname",'After EQ','clear',1);
    
                    EQ_sig = MLSE("DIR",[1,1],"duobinary_output",1,"M",M,"trellis_states",PAMmapper(M,0).levels).process(EQ_sig);
    
                    EQ_sig = Duobinary().decode(EQ_sig);
    
                    Rx_bits = PAMmapper(M,0).demap(EQ_sig);
                    [~,num_errors,ber_db,pos_errors] = calc_ber(Rx_bits.signal,Bits.signal,"skip_front",100,"skip_end",150,"returnErrorLocation",1);
    
                    disp([' DB Precode -> Channel -> FFE -> Decode/ Mod  ',sprintf('%.1E',ber_db),' | PD_in: ',num2str(pd_in),' dBm']);
                
                else
                    % Toms approach für precode emulation
                    [EQ_sig, Noi] = Eq.process(Scpe_sig_syncd,Duobinary().encode(Symbols));
                    
                    EQ_sig.spectrum("displayname","Signal Spectrum after Postfilter","fignum",1234,"normalizeToNyquist",0);

                    EQ_sig = MLSE("DIR",[1,1],"duobinary_output",1,"M",M,"trellis_states",PAMmapper(M,0).levels).process(EQ_sig);

                    % 2: Entschiedene Symbole codieren
                    %EQ_sig = Duobinary().encode(EQ_sig);

                    % 3. Entschiedene und codierte Symbole dekodieren
                    EQ_sig = Duobinary().decode(EQ_sig);
                    
                    % 4. Demap EQ'd symbols
                    Rx_bits = PAMmapper(M,0).demap(EQ_sig);

                    Symbols_db = Duobinary().precode(Symbols);
                    Bits_ = PAMmapper(M,0).demap(Symbols_db);

                    [~,num_errors,ber_db,pos_errors] = calc_ber(Rx_bits.signal,Bits_.signal,"skip_front",100,"skip_end",150,"returnErrorLocation",1);
    
                    disp([' DB Precode -> Channel -> FFE -> Decode/ Mod  ',sprintf('%.1E',ber_db),' | PD_in: ',num2str(pd_in),' dBm']);


                end
            elseif db_coding_approach %%%%%%%%%%%%%%%%%%%%%%%%%%%

                [EQ_sig, Noi] = Eq.process(Scpe_sig_syncd,Symbols);
                EQ_sig = MLSE("DIR",[1,1],"duobinary_output",1,"M",M,"trellis_states",PAMmapper(M,0).levels).process(EQ_sig);
                EQ_sig = Duobinary().decode(EQ_sig);

                Rx_bits = PAMmapper(M,0).demap(EQ_sig);
                [~,errors_bm,ber_db,errors] = calc_ber(Rx_bits.signal,Bits.signal,"skip_front",100,"skip_end",150,"returnErrorLocation",1);

                EQ_sig.plot("fignum",50,"displayname",'After EQ');

                disp([' DB Precode -> DB Code -> Channel -> FFE -> Decode/ Mod  ',sprintf('%.1E',ber_db),' | PD_in: ',num2str(pd_in),' dBm']);

            end

            %%%%% Store measurement into measurement "warehouse" %%%%%%
            wh.addValueToStorage(ber_ffe,'ber_ffe',fsym,rop_atten,M);
            wh.addValueToStorage(ber_mlse,'ber_mlse',fsym,rop_atten,M);
            wh.addValueToStorage(ber_db,'ber_db',fsym,rop_atten,M);

            wh.addValueToStorage(rop,'rop',fsym,rop_atten,M);
            wh.addValueToStorage(pd_in,'pd_in',fsym,rop_atten,M);
            % wh.addValueToStorage(Rx_bits,'signals',fsym,awg_vpp,precomp_amp_max,rop_atten);
            wh.addValueToStorage(M,'M',fsym,rop_atten,M);

            wh.addValueToStorage(v_bias,"v_bias",fsym,rop_atten,M);
            wh.addValueToStorage(awg_vpp,"awg_vpp",fsym,rop_atten,M);
            wh.addValueToStorage(precomp_amp_max,"precomp_amp_max",fsym,rop_atten,M);
            %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

            %%%%% Plot stuff into Table (feel free to add own values in -> 'name',value <- notation. Must be closed when table is changed)%%%%%%%%%%%%%%%%%%%%%%
            showCurrentMeasurement('FFE', ber_ffe,'MLSE',ber_mlse, 'Fsym',fsym.*1e-9, 'ROP', rop, 'PD in', pd_in, 'PAM',M, 'Vbias', v_bias, 'AWG Vpp', awg_vpp, 'Precomp MaxAmp',precomp_amp_max);

            %%%%% Arrange Figures %%%%%%%%%%%%%%%%%%%%%%
            % autoArrangeFigures(3,3,2);

            iterationTimes(loopcnt) = toc(iterationStartTime);
            averageTimePerIteration = mean(iterationTimes(1:loopcnt));
            estimatedTotalTime = averageTimePerIteration * looptotal;
            estimatedTimeRemaining = estimatedTotalTime - sum(iterationTimes(1:loopcnt));
            progressFraction = loopcnt / looptotal;
            waitbar(progressFraction, hWaitbar, ...
                sprintf('Loop: %d of %d \n  Runtime: %.1f min | %.1f sec per Loop |Time to go: %.1f min ', ...
                loopcnt, looptotal, sum(iterationTimes(1:loopcnt))/60, averageTimePerIteration, estimatedTimeRemaining/60 ));

            wh.save([folderpath,experiment_name,'_wh']);

            if rop_atten == 0
                figure(10)
                hold on
                
                col = linspecer(8);
                scatter(fsym.*1e-9,ber_ffe,30,'o','MarkerEdgeColor',col(M,:),'LineWidth',2);
                
                xlim([wh.parameter.fsym.values(1) wh.parameter.fsym.values(end)].*1e-9);
                yline(3.8e-3, 'DisplayName', 'HD-FEC', 'LineStyle', '--', 'HandleVisibility', 'off');
                xlabel('Received Optical Power (dBm)');
                ylabel('Bit Error Rate (BER)');
                title('Bit Error Rate vs. ROP');
                set(gca, 'yscale', 'log');
                set(gca, 'Box', 'on');
                grid on;
                grid minor;
                legend('Interpreter', 'none');
                
            end
        end
    end
end

close(hWaitbar);

wh.save([folderpath,experiment_name,'_wh_final']);

autoArrangeFigures(3,3,2)