%% FORCE MEASUREMENT
%  Laboratory Course
% =========================================================================
%  Script 04: Continuous Move 
%  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);

v_limit = 8;

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

% Variables to save the data
DATA_disp = [];
DATA_force = [];

curr_reading = inputSingleScan(s);

% Start long move
span_move = contact_end_pos - start_pos;
fprintf('Span Move = %f mm\n',span_move);
PI.PI_C863.MoveRelative('X',span_move); % start the move...
curr_pos = start_pos;
%% Measurement

fprintf('Current position mm; Current voltage V\n');
% Termination condition: reaching max indentation or max voltage
while( abs(curr_pos) < contact_end_pos - 0.0001 )
    
    WaitMillisecond(wait_time*1000);

    %% Modify the code to get stage position and reading the force values

    curr_pos = ?;
    curr_reading = ?;

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

curr_pos = PI.PI_C863.GetPosition('X');
fprintf('Max indent 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_ContinuousMove_', material), 'DATA_disp', 'DATA_force');
plot(DATA_disp, DATA_force, '.')

xlabel("Displacement (mm)")
ylabel("Voltage (V)")
title(strcat('Continuous measurement',{' '}, material))