function [data, bpm_x_offs, x_offsets, bpm_y_offs, y_offsets] = energy_scan(Es)

setup_utilities()

% data_file_name = './data/05-28-2019/temp.mat';

% Es = 42; % 40:2:60
vmp = 'vm0@erp122:';  % empty string for real machine
vmp_rmat = 'vm1@erp122:';
data = {};
hscales = linspace(-2,2,5);
vscales = linspace(-2,2,5);
set_magnets = true;
min_magnet_pause = 1;
readout_delay = 0;

correct_rmat_vm = true;

save_data = false;
n_avg = 1;

if(~strcmp(vmp,''))
    
    names = get_name_from_area('all');
%     bpms = names.bpms;
    
    pvs = get_pvs_from_name(names.bpms);
    bpm_x_offs = cellfun(@(x)x.x_offset.name, pvs, 'UniformOutput', false);
    bpm_y_offs = cellfun(@(x)x.y_offset.name, pvs, 'UniformOutput', false);
    
%     nbpms = remove_multipass(bpms);
%     
%     bpm_x_offs = strcat(nbpms,'_x_offset');
%     bpm_y_offs = strcat(nbpms,'_y_offset');
    
    bpm_x_offs = strcat(vmp, bpm_x_offs);
    bpm_y_offs = strcat(vmp, bpm_y_offs);
    
    x_offsets = lcaGet(bpm_x_offs');
    y_offsets = lcaGet(bpm_y_offs');
    
else
    
    x_offsets = NaN;
    y_offsets = NaN;
    
end

disp('Putting VM cavities in default states.');
set_vm_cavities_default(vmp);
set_vm_cavities_default(vmp_rmat);

for ii = 1:length(Es)
    disp(['Setting response matrix virtual machine to '  num2str(Es(ii)) ' MeV.']);
        
    cav_1_voltage = (Es(ii) - 36)*1e6;
    lcaPut([vmp_rmat 'RD1CAV01_voltage'],cav_1_voltage);
    hold_up(vmp_rmat)
    lcaPut([vmp_rmat 'autoscale_dipoles'],1);
    hold_up(vmp_rmat)
    
    if (isempty(vmp))
        disp(['Please set energy to ' num2str(Es(ii)) ', and then press enter.']);
        pause();
    else
        lcaPut([vmp 'RD1CAV01_voltage'],cav_1_voltage);
        hold_up(vmp)
        
        lcaPut([vmp 'autoscale_dipoles'],1);
        hold_up(vmp)
    end
    
    fa_elements = get_name_from_area('fa');
    all_fa_bpms = strcat(vmp,fa_elements.bpms);
    s1_elements = get_name_from_area('s1');
    last_2_s1 = get_N_dudes([s1_elements.dipoles s1_elements.v_correctors],-4);
        
    fprintf(['\nRetrieving ' num2str(Es(ii)) ' MeV response matrix...'])
    r_mat = get_cbetav_response(strcat(vmp_rmat,last_2_s1), strcat(vmp_rmat,fa_elements.bpms),vmp_rmat);
    fprintf('...done.')
    
    pause(1)
    
    if(correct_rmat_vm)
        % Steer into FA section
        options = struct;
        options.svd_tol = 'auto';
        options.Navg = 50;
        options.bpm_delay = 0.0;
        options.Nmax = 20;
        options.step_fraction = 1.0;
        options.abs_residual_tol = 0.0001;
        options.rel_residual_tol = 0.0001;
        options.Nfig = 3;
        options.title = ['Finding FA Periodic Orbit (' vmp_rmat ')'];
        fprintf(['\nCorrecting the RMAT virtual machine (' vmp_rmat ')...\n'])
        iterate_orbit_correction(strcat(vmp_rmat,fa_elements.bpms), strcat(vmp_rmat,last_2_s1), r_mat, options);
        r_mat = get_cbetav_response(strcat(vmp_rmat,last_2_s1), strcat(vmp_rmat,fa_elements.bpms),vmp_rmat);
        fprintf('...done.\n')
    end
    
    
    fprintf('...done.\n')
    
    if (~isempty(vmp))
        fprintf('Setting up orbit...\n')
        % Scan S1 dipoles to make sure the beam gets into FA BPMs
        beam_through = scan_beam_around([vmp 'MS1DIP07'],[vmp 'MS1DIP08'], 10, [-2,2], all_fa_bpms(1:4),4);
        if(~beam_through)
            warning('Could not get beam into FA section, thread_a_dude out')
            return
        end
        
        fprintf('Steering orbit into FA...\n')
        % Steer into FA section
        options = struct;
        options.svd_tol = 'auto';
        options.Navg = 50;
        options.bpm_delay = 0.0;
        options.Nmax = 20;
        options.step_fraction = 1.0;
        options.abs_residual_tol = 0.0001;
        options.rel_residual_tol = 0.0001;
        options.Nfig = 2;
        options.title = 'Finding FA Periodic Orbit';
        
        %iterate_orbit_correction(strcat(vmp,s1_fa_bpms), strcat(vmp,s1_fa_mags), r_mat, options);
        iterate_orbit_correction(strcat(vmp,fa_elements.bpms), strcat(vmp,last_2_s1), r_mat, options);
        fprintf('done.\n')
    else
        disp('Please verify that steering into FA is as good as possible, and press enter.');
        pause();
    end

    %----------------------------------------------------------------------
    % Take the tune data
    %----------------------------------------------------------------------
    options = struct;
    options.Energy = Es(ii);
    options.save_data = save_data;
    options.vmp = vmp;
    options.bpms = fa_elements.bpms;
    
    fprintf('\nFinding betatron kick coefficients...\n') 
    [x_kick_1, x_kick_2, y_kick_1, y_kick_2, x_nu, y_nu] = get_your_kicks(r_mat);
    fprintf(['Predicted tunes [x,y] = ' num2str(x_nu) ', ' num2str(y_nu) '\n'])
    
    options.hpvs = r_mat.column_names(1:length(r_mat.column_names)/2);
    options.hscales = hscales;
    options.h1cs = x_kick_1;
    options.h2cs = x_kick_2;
    options.nux0 = x_nu;
    
    options.vpvs = r_mat.column_names(length(r_mat.column_names)/2+1:end);
    options.vscales = vscales;
    options.v1cs = y_kick_1;
    options.v2cs = y_kick_2;
    options.nuy0 = y_nu;
    
    options.set_magnets = set_magnets;
    options.min_magnet_pause  = min_magnet_pause;
    options.n_avg = n_avg;
    options.readout_delay = readout_delay;
    
    fprintf('\nPerforming kick measurments...\n')
    data{end+1} = tuna_dude(options);
    data{end}.vmp = vmp;
    fprintf('done.\n')
    
end

%save(data_file_name,'data', 'bpm_x_offs', 'x_offsets', 'bpm_y_offs', 'y_offsets')

end

function nbpms = remove_multipass(bpms)
nbpms = {};
for ii=1:length(bpms)
    bpm = bpms{ii};
    nbpms{end+1} = bpm(1:8);
end
end


function set_vm_cavities_default(vmp)

if ~isempty(vmp)
    voltage = 6000;
    phase = 0.0;
    names = get_name_from_area('d1', false, true);
    cavities = strcat(vmp, names.cavities);
    
    lcaPut([vmp, 'tracking_on'], 0);
    pause(0.1);
    
    for ii=1:length(cavities)
        cav = cavities{ii};
        set_cmd_from_name(cav, voltage*1e3, 'cmd_name', 'voltage_cmd', 'max_wait_time', 0.0); % this record likes eV
        set_save_from_name(cav, voltage*1e3, 'cmd_save_name', 'voltage_cmd_save', 'max_wait_time', 0.0); % this record likes eV
        set_cmd_from_name(cav, phase, 'cmd_name', 'phase_cmd', 'max_wait_time', 0.0);
        set_save_from_name(cav, phase, 'cmd_save_name', 'phase_cmd_save', 'max_wait_time', 0.0);
    end
    
    lcaPut([vmp, 'tracking_on'], 1);
    pause(0.1);
    
    hold_up(vmp);
end

end

function hold_up(vmp)
    if (~isempty(vmp))
        lcaPut(strcat(vmp,'machine_ready'), 0)
        while(lcaGet(strcat(vmp,'machine_ready'))~=1)
            pause(0.2);
        end
    end
end