%% FORCE MEASUREMENT
%  Laboratory Course
% =========================================================================
%  Script 03: Incremental Step
%  Requirement:
%  - Script 01
% -------------------------------------------------------------------------
%  GROUP NUMBER: (put your group number here)
%  GROUP MEMBERS:
%  - (list name of group's member here)
%  - (list name of group's member here)
%  - (list name of group's member here)
%  - (add extra line as necessary)
% -------------------------------------------------------------------------

%% PLEASE DO NOT modify anything, except measurement section where further instruction is given. 
%% Unexpected modifications may result in hardware damage!

%% Preparation
% -------------------------------------------------------------------------

% Determine correct settings for each sample

MyFolderInfo = dir('./Data/Limits/');
use_old_lim = 'n';

% Logical indexing to filter out "." and ".."
validEntries = ~ismember({MyFolderInfo.name}, {'.', '..'});
filteredDirInfo = MyFolderInfo(validEntries);

if length(filteredDirInfo) >= 1
    % Ask if user wants to use old limits
    fprintf('At least one file has been found:\n');

    for ii= 1:length(filteredDirInfo)
    fprintf('\n\t%s\n', filteredDirInfo(ii).name);
    end

    fprintf('\nWARNING: Using wrong limits can result in HARDWARE DAMAGE!\n');
    prompt = "Do you want to use existing limits (y/n): ";
    use_old_lim = input(prompt,'s');
else 
    fprintf('\n No file has been found!\n');
end

if strcmp(use_old_lim, 'y')
    % Ask user for selecting the Limit file
    prompt = "Enter file name: ";
    fileName = input(prompt,'s');
    load(strcat('./Data/Limits/', fileName));
else
    % Ask user for sample material
    prompt = "Enter sample material: ";
    material = input(prompt,'s');
    
    % Starting position for finding the limit
    % WARNING: Make sure the sensor tip is not touching the sample at
    % scan_start_pos
    scan_start_pos = 1;
    steps = 50;
    
    [contact_start_pos, contact_end_pos, step_size, velocity] = Script02_FindLimit(s, material, scan_start_pos, steps);
end

% offset backward by 0.1 mm to not contact
offsetback = 0.1; 
offset_velocity = 1;
PI.PI_C863.SetVelocity('X', offset_velocity);

curr_pos = PI.PI_C863.GetPosition('X');
PI.PI_C863.Move('X',contact_start_pos - offsetback); 
pause( (abs(curr_pos-(contact_start_pos - offsetback))/offset_velocity) + 0.2 );
start_pos = PI.PI_C863.GetPosition('X');
fprintf('Start at = %f mm\n', start_pos);

%% Measurement
% -------------------------------------------------------------------------

v_limit = 8;

PI.PI_C863.SetVelocity('X', velocity);
wait_time = step_size / velocity;

% Variables to save the data
DATA_disp = [];
DATA_force1 = []; % after 1st delay
DATA_force2 = []; % after 2nd delay
DATA_force3 = []; % after 3rd delay
DATA_force4 = []; % after 4th delay, if needed

curr_reading = inputSingleScan(s);
curr_pos = start_pos;

fprintf('Current position mm; Current voltage V\n');
while( abs(curr_pos) < (contact_end_pos) )
    
    PI.PI_C863.MoveRelative('X',step_size);
    WaitMillisecond(wait_time*1000);
    
    curr_pos = curr_pos + step_size;
    curr_reading = inputSingleScan(s);

    %% Modify the code to read the force values 4 times (with 1 second intervals) at each stege 
    
    DATA_force1 = [DATA_force1; curr_reading];

    ...





    DATA_disp = [DATA_disp; curr_pos];
    
    fprintf('Pos: %f mm; Vol: %f V\n',curr_pos,curr_reading);
    if curr_reading > v_limit
        break
    end
 
end

curr_pos = PI.PI_C863.GetPosition('X');
fprintf('Max bend at = %f mm\n', curr_pos);

PI.PI_C863.SetVelocity('X', 1);
PI.PI_C863.Move('X',0);

%% Save and plot the data
save(strcat('./Data/SingleRead_IncrementalStep_', material), 'DATA_disp', ...
    'DATA_force1', 'DATA_force2', 'DATA_force3', 'DATA_force4');

plot(DATA_disp, DATA_force1, '.')
hold on
plot(DATA_disp, DATA_force2, '+')
plot(DATA_disp, DATA_force3, '^')
plot(DATA_disp, DATA_force4, 's')
hold off

legend("1st","2nd","3rd","4th","Location","southeast")
xlabel("Displacement (mm)")
ylabel("Voltage (V)")
title(strcat('Step relaxation',{' '}, material))