%% FORCE MEASUREMENT
%  Laboratory Course
% =========================================================================
%  Script 06: Simulator
%  Requirement:
%  - Data from Script 04
% -------------------------------------------------------------------------
%  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 Motion Simulation section where further instruction is given. 

clear all 
close all 
clc

% Ask user for sample material
prompt = "Enter material to simulate(EcoFlex, PDMS or EVA): ";
material = input(prompt,'s');
load(strcat('./Data/SingleRead_ContinuousMove_', material, ".mat"))

% Use different velocities for continuous move measurement
velocity_continuous_EcoFlex = 0.1;
velocity_continuous_PDMS = 0.01; 
velocity_continuous_EVA = 0.01; 
velocity_continuous_default = 0.01;

if strcmp(material, 'EcoFlex')
    velocity = velocity_continuous_EcoFlex;
elseif strcmp(material, 'PDMS')
    velocity = velocity_continuous_PDMS;
elseif strcmp(material, 'EVA')
    velocity = velocity_continuous_EVA;
else
    velocity = velocity_continuous_default;
end

% Interpolate real data points to obtain a continuous function for simulation
x_q = linspace(DATA_disp(1), DATA_disp(end),length(DATA_disp)*10);
vq1 = interp1(DATA_disp,DATA_force,x_q);

sim_contact_start = x_q(1);
sim_contact_end = x_q(length(x_q));     

steps = 50;
movement_range = sim_contact_end - sim_contact_start;
t_step = movement_range / velocity / steps;

% Set position to contact start position
curr_pos = sim_contact_start;

% Vectors for recording values
sim_disp = sim_contact_start;
sim_voltage = [vq1(1)];

%% Motion Simulation

%% Deploy a timer and try to estimates the stage absolute position given the velocity and curr_position

while(curr_pos < sim_contact_end)





    


    % Do not Modify further down
    % Interpolate to obtain the voltage corresponding to curr_pos

    voltage = interp1(x_q,vq1,curr_pos);

    if isnan(voltage)
        break
    end

    sim_disp = [sim_disp, curr_pos];
    sim_voltage = [sim_voltage, voltage];
    fprintf('Pos: %f mm; Voltage: %f V\n',curr_pos, voltage);
end

% Values from sensor characterization with 18V power, 1.5 mV/V amp gain
sensor_gain = 6.0744;   % g/V
bias = 0.0102;          % g
sim_force = sim_voltage * sensor_gain + bias;

plot(sim_disp,sim_force, "o",'LineWidth',2.5)
xlabel("Displacement (mm)",'FontSize',22)
ylabel("Force (gram)",'FontSize',22)
title(strcat('Continuous measurement',{' '}, material))
set(gca,'XTickLabel',get(gca,'XTick'),'FontSize',18)
set(gca,'YTickLabel',get(gca,'YTick'),'FontSize',18)