FR_choice      = 1;
ABKG_choice    = 1;
a_start        = 86;       % initial scattering length (a_0)
a_end          = 85;       % final scattering length (a_0)
T              = 0.030;    % ramp duration (s)
Nt             = 1000;     % number of time points
ResonanceRange = [1.2, 2.6];

% Options for ramp generation
options = struct( ...
    'smoothingMethod', 'sgolay', ... % 'sgolay', 'lowpass', or 'none'
    'sgolayOrder', 3, ...
    'sgolayFrameLength', 51, ...
    'maxRampRate', 5, ...
    'Bmin', 0.5, ...
    'Bmax', 3.5, ...
    'rampShape', 'linear' ...   % Choose: 'linear', 'exponential', 'sigmoid'
);

[t, B_ramp, a_check] = generateSmoothBRamp(FR_choice, ABKG_choice, a_start, a_end, ResonanceRange, T, Nt, options);

% ----- Plot results -----
figure(1); clf

subplot(2,1,1);
plot(t, B_ramp, 'b-', 'LineWidth', 2);
xlabel('Time (s)');
ylabel('Magnetic field B(t) (G)');
title('Generated magnetic field ramp B(t)');
grid on;
set(gca, 'FontSize', 14);

subplot(2,1,2);
plot(t, a_check, 'r-', 'LineWidth', 2);
xlabel('Time (s)');
ylabel('Scattering length a_s(t) (a_0)');
legend('Resulting a_s(t)');
title('Scattering length ramp');
grid on;
set(gca, 'FontSize', 14);

% Visualize full resonance curve and selected window
[B_full, a_full]   = extractBetweenResonances(FR_choice, ABKG_choice, ResonanceRange);
[B_curve, a_curve] = fullResonanceCurve(FR_choice, ABKG_choice, ResonanceRange);
[B_all, a_all]     = getFullFeschbachSpectrum(FR_choice, ABKG_choice);

figure(2);
clf
plot(B_all, a_all , 'g-', 'LineWidth', 1); hold on;
plot(B_curve, a_curve, 'k-', 'LineWidth', 1); 
% Plot all unfiltered resonances in black
plot(B_full, a_full, 'b-', 'LineWidth', 2);               % zoomed-in region
plot(B_ramp, a_check, 'r-', 'LineWidth', 2);              % actual ramp
plot(B_ramp([1 end]), a_check([1 end]), 'ro', 'MarkerSize', 8); % endpoints
xline(min(B_ramp), '--b', 'B_{min}');
xline(max(B_ramp), '--b', 'B_{max}');
yline(min(a_check), '--r', 'a_{min}');
yline(max(a_check), '--r', 'a_{max}');
xlabel('B field (G)');
ylabel('Scattering length a_s (a_0)');
title('Feshbach spectrum with selected B and a_s range', 'Interpreter','tex');
legend('Full a(B)', 'Modified a(B)', 'Zoomed Region', 'Ramp a_s(t)', 'Ramp Endpoints', 'Location', 'NorthWest');
xlim([1.15 2.7])
ylim([0 150])
set(gca, 'FontSize', 14)
grid on;

%% Isolate from spectrum manually
FR_choice      = 1;
ABKG_choice    = 1;
a_start        = 124;       % initial scattering length (a_0)
a_end          = 117;       % final scattering length (a_0)
T              = 0.010;    % ramp duration (s)
Nt             = 15000;     % number of time points
ResonanceRange = [1.2, 2.6];

% Get the full spectrum
[B, a_s] = getFullFeschbachSpectrum(1, 2); % Using new FR params and middle a_bkg

% Define the plotting range
x_limits = [1.15, 2.7];
y_limits = [0, 150];

% Find indices within our x-range
mask = (B >= x_limits(1)) & (B <= x_limits(2));
B_plot = B(mask);
a_s_plot = a_s(mask);

% Identify resonances to mask (looking at the spectrum, we'll mask around 2.174G and 2.336G)
resonances_to_mask = [2.174, 2.336];
mask_width = 0.1; % Width to mask around each resonance (in G)

% Create a mask for the regions to keep (1 = keep, 0 = mask)
keep_mask = true(size(B_plot));
for res = resonances_to_mask
    keep_mask = keep_mask & (B_plot < (res - mask_width) | B_plot > (res + mask_width));
end

% Create masked version
B_masked             = B_plot(keep_mask);
a_s_masked           = a_s_plot(keep_mask);

% Interpolate over the masked regions
a_s_interp           = interp1(B_masked, a_s_masked, B_plot, 'pchip');

% --- Find the remaining resonances (peaks) in the interpolated data ---
% Use findpeaks to detect peaks (adjust MinPeakProminence as needed)
[peaks, locs]        = findpeaks(abs(a_s_interp), 'MinPeakProminence', 20); % Tune this!
remaining_resonances = B_plot(locs); % Positions of remaining resonances

% --- Select the two resonances of interest (1.3G and 2.59G) ---
% If automatic detection fails, manually define them:
target_resonances    = [1.3, 2.59]; % Manually specified (adjust as needed)
% OR use the closest detected resonances:
% target_resonances = remaining_resonances(abs(remaining_resonances - 1.3) < 0.2 & abs(remaining_resonances - 2.59) < 0.2);

% --- Extract the curve BETWEEN these two resonances ---
res1                 = min(target_resonances); % Lower resonance (1.3G)
res2                 = max(target_resonances); % Upper resonance (2.59G)
between_mask         = (B_plot > res1) & (B_plot < res2); % No masking here, since resonances are already removed
B_between            = B_plot(between_mask);
a_s_between          = a_s_interp(between_mask);

% Plotting
figure(3);
hold on;

% Plot the original spectrum
plot(B_plot, a_s_plot, 'b-', 'DisplayName', 'Original spectrum');

% Plot the masked regions in red
for res = resonances_to_mask
    mask_region = (B_plot >= (res - mask_width)) & (B_plot <= (res + mask_width));
    plot(B_plot(mask_region), a_s_plot(mask_region), 'r-', 'LineWidth', 2, 'DisplayName', 'Unwanted Resonance');
end

% Plot the interpolated curve
plot(B_plot, a_s_interp, 'g--', 'LineWidth', 1.5, 'DisplayName', 'Interpolated');

% Formatting
xlim(x_limits);
ylim(y_limits);
xlabel('Magnetic Field (G)');
ylabel('Scattering Length (a_0)');
title('Dy-164 Feshbach Spectrum with Masked Resonances');
legend('Location', 'best');
grid on;
hold off;

% --- Plotting ---
figure(4);
hold on;

% Plot the interpolated spectrum (without 2.174G and 2.336G)
plot(B_plot, a_s_interp, 'k-', 'LineWidth', 1, 'DisplayName', 'Interpolated spectrum');

% Highlight the region between 1.3G and 2.59G
plot(B_between, a_s_between, 'm-', 'LineWidth', 2, 'DisplayName', 'Between 1.3G and 2.59G');

% Formatting
xlim(x_limits);
ylim([0, 150]);
xlabel('Magnetic Field (G)');
ylabel('Scattering Length (a_0)');
title('Interpolated Spectrum: Region Between 1.3G and 2.59G');
legend('Location', 'best');
grid on;
hold off;

% Generate the ramp
[t, B_ramp, a_check] = generateLinearBRamp(B_between, a_s_between, a_start, a_end, T, Nt);

% Plot results
figure;
subplot(2,1,1);
plot(t, B_ramp, 'b');
xlabel('Time (s)');
ylabel('B Field (G)');
title('Generated B-Field Ramp');
grid on;

subplot(2,1,2);
plot(t, a_check, 'r');
xlabel('Time (s)');
ylabel('a_s (a_0)');
title('Achieved Scattering Length');
grid on;

%% Helper functions
function [t, B_ramp, a_check] = generateSmoothBRamp(FR_choice, ABKG_choice, a_start, a_end, selectedResRange, T, Nt, opts)
    % Time array
    t = linspace(0, T, Nt);

    if strcmp(opts.rampShape, 'linear')
        % Directly call helper for linear LUT ramp generation
        [t, B_ramp, a_check] = generateLinearBRampUsingLUT(FR_choice, ABKG_choice, a_start, a_end, selectedResRange, T, Nt);
        return
    end

    % Get target a_s(t) based on rampShape choice
    a_target = getTargetScatteringLength(t, T, a_start, a_end, opts.rampShape);

    % --- a(B) interpolation ---
    [B_between, a_between] = extractBetweenResonances(FR_choice, ABKG_choice, selectedResRange);
    valid_idx = a_between > 0 & a_between < 150;
    [a_sorted, sort_idx] = sort(a_between(valid_idx));
    B_sorted = B_between(valid_idx);
    B_sorted = B_sorted(sort_idx);
    B_of_a = @(a) interp1(a_sorted, B_sorted, a, 'linear', 'extrap');
    B_raw = B_of_a(a_target);

    % --- Smoothing ---
    switch opts.smoothingMethod
        case 'sgolay'
            B_smooth = sgolayfilt(B_raw, opts.sgolayOrder, opts.sgolayFrameLength);
        case 'lowpass'
            dt = T / (Nt - 1); Fs = 1 / dt;
            B_smooth = lowpass(B_raw, Fs / 20, Fs);
        case 'none'
            B_smooth = B_raw;
        otherwise
            error('Unknown smoothing method');
    end

    % --- Bound the ramp ---
    B_smooth = min(max(B_smooth, opts.Bmin), opts.Bmax);

    % --- Enforce max dB/dt ---
    dt = T / (Nt - 1);
    for i = 2:Nt
        delta = B_smooth(i) - B_smooth(i-1);
        if abs(delta/dt) > opts.maxRampRate
            delta = sign(delta) * opts.maxRampRate * dt;
            B_smooth(i) = B_smooth(i-1) + delta;
        end
    end
    B_ramp = B_smooth;

    % --- Verify a_s(t) from B_ramp ---
    [a_bkg, resonanceB, resonancewB] = getResonanceParams(FR_choice, ABKG_choice, selectedResRange);
    a_of_B = @(B) arrayfun(@(b) ...
        a_bkg * prod(1 - resonancewB ./ (b - resonanceB)), B);
    a_check = a_of_B(B_ramp);
end

function a_target = getTargetScatteringLength(t, T, a_start, a_end, rampShape)
    % If rampShape is a function handle, use it directly (for flexibility)
    if isa(rampShape, 'function_handle')
        a_target = rampShape(t);
        return
    end

    switch lower(rampShape)
        case 'exponential'
            tau = T / 3;
            base = (1 - exp(-t / tau)) / (1 - exp(-T / tau));
            a_target = a_start + (a_end - a_start) * base;

        case 'sigmoid'
            s = 10 / T; center = T / 2;
            sigmoid = @(x) 1 ./ (1 + exp(-s * (x - center)));
            base = (sigmoid(t) - sigmoid(t(1))) / (sigmoid(t(end)) - sigmoid(t(1)));
            a_target = a_start + (a_end - a_start) * base;

        otherwise
            error('Unknown ramp shape: %s', rampShape);
    end
end

function [B_between, a_between] = extractBetweenResonances(FR_choice, ABKG_choice, selectedRange)
    [a_bkg, resonanceB, resonancewB] = getResonanceParams(FR_choice, ABKG_choice, selectedRange);
    [~, idx] = sort(resonancewB, 'descend');
    B1 = resonanceB(idx(1)); B2 = resonanceB(idx(2));
    w1 = resonancewB(idx(1)); w2 = resonancewB(idx(2));
    Bvis = linspace(min(B1, B2) - 20*min(w1,w2), max(B1, B2) + 20*min(w1,w2), 2000);

    a_of_B = @(B) arrayfun(@(b) ...
        a_bkg * prod(1 - resonancewB ./ (b - resonanceB)), B);
    avis = a_of_B(Bvis);
    between_idx = Bvis >= min(B1,B2) & Bvis <= max(B1,B2);
    B_between = Bvis(between_idx);
    a_between = avis(between_idx);
end

function [B_range, a_values] = fullResonanceCurve(FR_choice, ABKG_choice, selectedRange)
    [a_bkg, resonanceB, resonancewB] = getResonanceParams(FR_choice, ABKG_choice, selectedRange);
    B_range = linspace(min(resonanceB)-0.2, max(resonanceB)+0.2, 3000);
    a_of_B = @(B) arrayfun(@(b) ...
        a_bkg * prod(1 - resonancewB ./ (b - resonanceB)), B);
    a_values = a_of_B(B_range);
end

function [a_bkg, resonanceB, resonancewB] = getResonanceParams(FR_choice, ABKG_choice, selectedRange)
    if FR_choice == 1
        a_bkg_list  = [85.5, 93.5, 77.5];
        resonanceB  = [1.295, 1.306, 2.174, 2.336, 2.591, 2.740, 2.803, ...
             2.780, 3.357, 4.949, 5.083, 7.172, 7.204, 7.134, 76.9];
        resonancewB = [0.009, 0.010, 0.0005, 0.0005, 0.001, 0.0005, 0.021, ...
             0.015, 0.043, 0.0005, 0.130, 0.024, 0.0005, 0.036, 3.1];
    else
        a_bkg_list = [87.2, 95.2, 79.2];
        resonanceB = [1.298, 2.802, 3.370, 5.092, 7.154, 2.592, 2.338, 2.177];
        resonancewB = [0.018, 0.047, 0.048, 0.145, 0.020, 0.008, 0.001, 0.001];
    end
    a_bkg = a_bkg_list(ABKG_choice);

    % --- Filter resonanceB and resonancewB if selectedRange is provided ---
    if nargin >= 3 && ~isempty(selectedRange)
        minB = min(selectedRange); maxB = max(selectedRange);
        keep_idx = (resonanceB >= minB) & (resonanceB <= maxB);
        
        % Keep only the lowest and highest resonance in the selected range
        if sum(keep_idx) >= 2
            B_sub = resonanceB(keep_idx);
            w_sub = resonancewB(keep_idx);
            [~, idx_lo] = min(B_sub);
            [~, idx_hi] = max(B_sub);

            resonanceB = [B_sub(idx_lo), B_sub(idx_hi)];
            resonancewB = [w_sub(idx_lo), w_sub(idx_hi)];
        else
            error('Selected resonance range does not include at least two resonances.');
        end
    end
end

function [t, B_ramp, a_check] = generateLinearBRampUsingLUT(FR_choice, ABKG_choice, a_start, a_end, selectedResRange, T, Nt)

    % Time vector
    t = linspace(0, T, Nt);

    % 1) Generate LUT of B and a_s(B)
    [B_between, a_between] = extractBetweenResonances(FR_choice, ABKG_choice, selectedResRange);
    
    % Restrict to physically meaningful range (optional)
    valid_idx = a_between > 0 & a_between < 150;
    B_lut = B_between(valid_idx);
    a_lut = a_between(valid_idx);

    % 2) Generate linear a_s ramp in time
    a_target = linspace(a_start, a_end, Nt);

    % 3) Interpolate B(t) from LUT a_s -> B
    % Make sure a_lut is sorted ascending
    [a_lut_sorted, idx_sort] = sort(a_lut);
    B_lut_sorted = B_lut(idx_sort);

    B_ramp = interp1(a_lut_sorted, B_lut_sorted, a_target, 'linear', 'extrap');

    % 4) Compute resulting a_s(t) for verification
    [a_bkg, resonanceB, resonancewB] = getResonanceParams(FR_choice, ABKG_choice, selectedResRange);
    a_of_B = @(B) arrayfun(@(b) ...
        a_bkg * prod(1 - resonancewB ./ (b - resonanceB)), B);
    a_check = a_of_B(B_ramp);

end

function [B, a_s] = getFullFeschbachSpectrum(FR_choice, ABKG_choice)
    % plotScatteringLength(FR_choice, ABKG_choice)
    % Plots the Dy-164 scattering length vs B field based on PhysRevX.9.021012 Suppl. Fig. S5
    % 
    % Inputs:
    %   FR_choice: 1 = new resonance parameters, 2 = old
    %   ABKG_choice: 1 = lower a_bkg, 2 = mid, 3 = upper
    %
    % Example:
    %   plotScatteringLength(1, 1);

    if nargin < 1, FR_choice = 1; end
    if nargin < 2, ABKG_choice = 1; end

    % Choose background scattering length
    if FR_choice == 1  % New values
        switch ABKG_choice
            case 1, a_bkg = 85.5;
            case 2, a_bkg = 93.5;
            case 3, a_bkg = 77.5;
        end

        % Resonance positions (G) and widths (G)
        resonanceB = [1.295, 1.306, 2.174, 2.336, 2.591, 2.740, 2.803, 2.780, ...
                      3.357, 4.949, 5.083, 7.172, 7.204, 7.134, 76.9];
        resonancewB = [0.009, 0.010, 0.0005, 0.0005, 0.0010, 0.0005, 0.021, ...
                       0.015, 0.043, 0.0005, 0.130, 0.024, 0.0005, 0.036, 3.1];

    else  % Old values
        switch ABKG_choice
            case 1, a_bkg = 87.2;
            case 2, a_bkg = 95.2;
            case 3, a_bkg = 79.2;
        end

        % Resonance positions (G) and widths (G)
        resonanceB = [1.298, 2.802, 3.370, 5.092, 7.154, 2.592, 2.338, 2.177];
        resonancewB = [0.018, 0.047, 0.048, 0.145, 0.020, 0.008, 0.001, 0.001];
    end

    % Magnetic field range for plotting (G)
    B = linspace(0.5, 8, 2000);

    % Compute scattering length using product formula
    a_s = a_bkg * ones(size(B));
    for j = 1:length(resonanceB)
        a_s = a_s .* (1 - resonancewB(j) ./ (B - resonanceB(j)));
    end
end

function [t, B_ramp, a_check] = generateLinearBRamp(B_between, a_s_between, a_start, a_end, T, Nt)
    % Generates a B-field ramp (B_ramp) to produce a linear a_s ramp from a_start to a_end.
    % Uses precomputed LUT: B_between (magnetic field) and a_s_between (scattering length).
    %
    % Inputs:
    %   B_between    - Magnetic field values (G) between resonances [vector]
    %   a_s_between  - Corresponding scattering lengths (a_0) [vector]
    %   a_start, a_end - Target scattering length range (a_0)
    %   T            - Total ramp time (s)
    %   Nt           - Number of time steps
    %
    % Outputs:
    %   t            - Time vector (s)
    %   B_ramp       - Generated B-field ramp (G)
    %   a_check      - Verified a_s(t) using interpolation (a_0)

    % --- 1. Time vector ---
    t                      = linspace(0, T, Nt);

    % --- 2. Ensure LUT is sorted and unique ---
    [a_s_sorted, sort_idx] = unique(a_s_between);  % Remove duplicates and sort
    B_sorted               = B_between(sort_idx);

    % --- 3. Generate target linear a_s ramp ---
    a_target               = linspace(a_start, a_end, Nt);

    % --- 4. Interpolate B(t) from a_s -> B ---
    B_ramp                 = interp1(a_s_sorted, B_sorted, a_target, 'pchip', 'extrap');

    % --- 5. Verify a_s(t) by re-interpolating ---
    a_check                = interp1(B_sorted, a_s_sorted, B_ramp, 'pchip');
end