%% Neuromechanic - A neuromechanic model of button-pressing
%
% Uses: bayesopt, Simulink, Simscape
% 
% Authors: Antti Oulasvirta (1), Byungjoo Lee (2), Sunjun Kim (1)
% (1) Aalto University; (2) KAIST
%  
% Version: September 19 2017
% 
% 
% This is the version reported in the Proceedings of CHI 2018
% (Oulasvirta, Kim, & Lee, 2018). Read full paper at
% http://doi.org/10.1145/3173574.3174082
%
% This work is licensed under the Creative Commons Attribution 4.0 International License. 
% To view a copy of this license, visit http://creativecommons.org/licenses/by/4.0/ or 
% send a letter to Creative Commons, PO Box 1866, Mountain View, CA 94042, USA.
%

warning('off','all');   % All warnings off
close all;              % Clears workspace
addpath(genpath(pwd));  % Adds working folder to path

%% Simulink and Simscape initialization

CFR_HomeDir = pwd;
CFL_libname = 'Libraries/Contact_Forces_Lib';
load_system(CFL_libname);
load_system('CMFB');

% "hws" means "human workspace", used for storing variables in the human model
hws = get_param('CMFB', 'modelworkspace');
hws.DataSource = 'MAT-File';
hws.FileName = 'params';
hws.assignin('peak_int', 0.5);  % Set peak of integrated p-center at 500 ms (this will be udpated by the BO with experience)
hws.saveToSource;
hws.reload;

%% Bayesian Optimizer setup
% See Matlab documentation for reference
max_Time = 1200;                                          % Optimization budget (in seconds)
BOexplorationratio = 0.15;                              % Exploration rate (larger value -> more exploration)
traintrials = 8;                                        % Number of repeated attempts of a button press 
testtrials = 100;                                       % Number of test trials (after model-learning) 

%% Motor, finger and sensor feedback model parameters
% See Table 1 in paper for explanation
Motornoise = readtable('i_motornoise.csv');             % Muscle noise parameters
Sensory = readtable('i_sensorynoise.csv');              % Sensory feedback noise parameters
Finger = readtable('i_finger.csv');                     % Physical properties of finger 
Finger_Stiffness = readtable('i_finger_stiffness.csv'); % Finger pulp model (stiffness per displacement)
Finger.flength = 0.06;                                  % Length of finger bone (unit: meters)

% Hill-type finger muscle model parameters
Finger.Lmag = Finger.L0ag * 0.3;                        % Minimum length at which the muscle can produce force (agonist)                  
Finger.Lman =Finger.L0an * 0.3;                         % Minimum length at which the muscle can produce force (antagonist)
Finger.Ks = 0.8 * Finger.fcsa;                          % Stiffness coefficient of passive muscle element
Finger.Kd = 0.1 * Finger.Ks;                            % Damping coefficient of passive muscle element
Finger.vmag = Finger.L0ag * 8;                          % Maximum contraction velocity under no load (agonist)
Finger.vman = Finger.L0an * 8;                          % Maximum contraction velocity under no load (antagonist)

%% Button model parameters

Button = readtable('i_button.csv');             % Muscle noise parameters
Feedback=readtable('i_feedback.csv');                   % Physical properties - intensity, loudness - of of light and audio feedback from button-press, respectively
Button_Stiffness_temp = readtable('FDdatabase/Mechanical/Samsung SKM-1000UB (2015-05, Cherry MX Blue).csv'); % Tactile button type
%Button_Stiffness_temp = readtable('o_linear_fd.csv');  % Linear button
d = designfilt('lowpassfir', ...
    'PassbandFrequency',0.15,'StopbandFrequency',0.2, ...
    'PassbandRipple',1,'StopbandAttenuation',60, ...
    'DesignMethod','equiripple');
%Button_Stiffness_temp.Var2 = filtfilt(d,Button_Stiffness_temp.Var2); %Smoothing the empirical stiffness of buttons; Comment this out for Samsung tactile button
maximum_travel_index=find(Button_Stiffness_temp.Var1==max(Button_Stiffness_temp.Var1));
x_displacement=[Button_Stiffness_temp.Var1(1:maximum_travel_index)/1000000;(max(Button_Stiffness_temp.Var1)/1000000+0.0001)];
stiffness=[0;diff(Button_Stiffness_temp.Var2(1:maximum_travel_index)/100)./diff(Button_Stiffness_temp.Var1(1:maximum_travel_index)/1000000);10000];
activation_d=zeros(size(stiffness));
activation_d(1)=-0.002;                                 % Activation point of the physical button
activation_midair=zeros(size(stiffness));
activation_midair(1)=-100000;                           % Disable the activation of mid-air button
activation_p=zeros(size(stiffness));
activation_p(1)=100000;                                 % Disable the activation of touch button
damping=zeros(size(stiffness));
damping(1)=0.1;                                         % Damping of the button spring
Button_Stiffness=table(x_displacement,stiffness,activation_p,activation_d,activation_midair,damping);
Finger.fstart_height = Finger.fradius+Button.bdepth/2;  % Starting height of finger 
Finger.fstart_height_std = 0.001;                       % Standard deviation (uncontrollable component) of finger starting height

%% OptimizableVariables for Bayesian Optimization
% These variables define the decision task of the BO, ranges in square brackets
% Motor command:
% $\theta = \{ \mu_{A+}, \tau_{A+}, \sigma_{A+}\}$ %
% Expected p-center:
% $ pc_e $ %
% The current setup assumes a 3.0 second total time window for the
% simulation

onag = optimizableVariable('onag', [0.0, 1.5]);         % Onset of agonist muscle (s)
amag = optimizableVariable('amag', [0.5, 1.0]);         % Amplitude of agonist 
pwag = optimizableVariable('pwag', [1, 5]);             % Pulse width of agonist
exp_ap = optimizableVariable('exp_ap', [0, (3-0.1)]);   % Expected p-center

%% Training phase
% Obtains an optimized motor program over repeated button presses
% Uses Expected Improvement as the Acquisition function of BO
% See MATLAB's documentation for description of BO's parameters

disp('Training');
testing = false;
training = @(x)perceptualcontrol(x,traintrials,Motornoise,Finger,Feedback, Button,Sensory, testing);
results = bayesopt(training, [amag,pwag,onag,exp_ap],'MaxTime',max_Time, 'Verbose', 1, 'PlotFcn',[], ...
    'ExplorationRatio', BOexplorationratio, 'NumSeedPoints', 5,'IsObjectiveDeterministic',false, ....
    'MaxObjectiveEvaluations', inf, ...
    'AcquisitionFunctionName', 'expected-improvement'); 

%% Testing phase
% Uses the optimized motor program over repeated trials for estimates of
% performance 
% Results are written in /results
writetable (results.XAtMinObjective,'results/trace');   % Stores winner motor program
disp('Testing')
testing = @(x,testtrials)perceptualcontrol(x,testtrials,Motornoise,Finger,Feedback,Button,Sensory, true); % Testing = true
objvalue = testing(results.XAtMinObjective,testtrials);  

%% Perceptual control system linked to Simulink and Simscape
% This system runs the given motor program and returns objective value to BO
% See CMFB.slx for the finger + button models
%
function [objective] = perceptualcontrol(x,n,Neural,Finger,Feedback,Button,Sensory,testing)

    % Load the hws Simulink model, access variables in modelworkspace
    hws = get_param('CMFB', 'modelworkspace');
    hws.DataSource = 'MAT-File';
    hws.FileName = 'params';

    % Passive record-keeping of p-centers for final reporting
    peak_ps = [];
    peak_ds = [];
    peak_ls = [];
    peak_as = [];

    % Passive record-keeping of activation and other data for final
    % reporting
    activation_points = [];                                 % Button activations
    activations = [];                                       % Vector of activations (button not activated)
    ovs = [];                                               % Vector of objective values obtained in repeated trials
    deltas = [];                                            % Vector of deltas between expected and obtained perceptions
    percepts = [];                                          % Vector of percepts (after button presssing)
    displacements = [];
    velocities = [];
    muscleforces = [];

    % n Repeated button-pressing trials, over which the objective score is
    % computed
    for i = 1:n
        
        % Muscle model: Set trial-specific input parameters 

        % Noise model: additive Gaussian noise
        hws.assignin('amplitude_ag', abs(random('norm',x.amag,Neural.ag_am_noise)));    % Amplitude of agonist
        hws.assignin('pulsewidth_ag', abs(random('norm',x.pwag,Neural.ag_pw_noise)));   % Pulse width of agonist
        hws.assignin('amplitude_an', 0);        
        hws.assignin('pulsewidth_an', 10);                  % In the CHI2018 version, antagonist is passive (resists but not activated)
        ag_pd = min(3,random('norm',x.onag+Neural.ag_motor_delay,Neural.ag_motor_delay_noise));
        hws.assignin('phasedelay_ag', ag_pd);               % Onset time is subjected to noise and delay caused by to Treisman gating
        finger_start_height = random('norm',Finger.fstart_height,Finger.fstart_height_std); % Varying start position of finger
        hws.assignin('finger_start_height', finger_start_height);
        hws.assignin('window', 3);                          % Sets the total time window of the simulation (3 seconds in paper)
        hws.saveToSource;
        hws.reload;                                         % Save and refresh

        % Simulate
        sim('CMFB');

        
        % The rest of this function is used for computing objective score
        % and reporting variables on physical events in the simulation
        
        % Muscle force
        muscleforce = abs(min(push_muscle_force.data)); % amount of force
        muscleforces = [muscleforces muscleforce];
        
        % Activation point
        max_ap = max(keyevent.data);
        peak_ap = find(keyevent.data == max_ap);  % the first keyevent
        peak_ap = keyevent.Time(peak_ap(1));

        size_d = size(raw_displacement.data);
        size_v = size(velocity.data);
        
        % Computing p-centers
        % Pressure sensation in finger pulp
        % Note that sensory noises are applied in CMBF.slx
        max_p = max(pulpcontact.data);              % amplitude of peak
        peak_p = find(pulpcontact.data == max_p);   % location of first peak
        peak_index=peak_p(1);                       % find first peak
        peak_p = pulpcontact.Time(peak_index);      % take the value at that peak
        peak_p_shifted = peak_p + Sensory.p_delay;  % shift the peal per sensory delay
        peak_p_asdn = peak_p_shifted;               
        p_power=max_p;
        p_reliability=1/log2(p_power/Sensory.p_noise);
        peak_ps = [peak_ps peak_p_asdn];

        % Percept: Fingertip displacement
        min_d = min(displacement.data);
        peak_d = find(displacement.data == min_d);
        peak_index=peak_d(1);
        peak_d = displacement.Time(peak_index);
        peak_d_shifted = peak_d + Sensory.d_delay;
        peak_d_asdn = peak_d_shifted;
        peak_ds = [peak_ds peak_d_asdn];
        d_power=Finger.fstart_height-min_d;
        d_reliability=1/log2(d_power/Sensory.d_noise);

        % Percept: Light
        max_l = max(keyeventtime_l.data);
        peak_l = find(keyeventtime_l.data == max_l);  % the first light
        peak_l = keyeventtime_l.Time(peak_l(1));
        peak_l_shifted = peak_l +Sensory.l_delay;
        peak_l_asdn = peak_l_shifted;
        peak_ls = [peak_ls peak_l_asdn];
        l_power=Feedback.l_intensity*max_l;
        l_reliability=1/log2(l_power/Sensory.l_noise);

        % Percept: Auditory
        max_a = max(keyeventtime_a.data);
        peak_a = find(keyeventtime_a.data == max_a);  % the first beep
        peak_a = keyeventtime_a.Time(peak_a(1));
        peak_a_shifted = peak_a +Sensory.a_delay;
        peak_a_asdn = peak_a_shifted;
        peak_as = [peak_as peak_a_asdn];
        a_power=Feedback.a_loudness*max_a;
        a_reliability=1/log2(a_power/Sensory.a_noise);

        n_keyevents = sum(keyevent.data);

        % Objective function is set to 0 and increased based on success and
        % precision and effort
        objectivevalue = 0.0;

        raw_max_p = max(raw_pulpcontact.data);
        
        % Cue integration MLE
        % Because active sensory inputs are different per button type, we
        % separate some parts of computation 
        if(n_keyevents==1&&raw_max_p~=0) % Physical and touch button single activation
            sum_var = 1/p_reliability + 1/d_reliability + 1/l_reliability + 1/a_reliability;
            peak_int = (1 / p_reliability / sum_var) * peak_p_asdn + (1 / d_reliability / sum_var) * peak_d_asdn + (1 / l_reliability / sum_var) * peak_l_asdn + ((1 / a_reliability) / sum_var) * peak_a_asdn;
            activations = [activations 1];
            activation_points = [activation_points peak_ap];
        elseif (n_keyevents==0&&raw_max_p~=0) % Physical button contacted but not activated
            sum_var = 1/p_reliability + 1/d_reliability;
            peak_int = ((1 / p_reliability) / sum_var) * peak_p_asdn + ((1 / (d_reliability)) / sum_var) * peak_d_asdn;
            objectivevalue = objectivevalue + 1; % + 0.5 / muscleforce;
            % HACK: encourages the use of more force to activate the button
            activations = [activations 0];
        elseif (n_keyevents==1&&raw_max_p==0) % Midair button not contacted but activated
            sum_var = 1/d_reliability + 1/l_reliability + 1/a_reliability;
            peak_int = (1 / d_reliability / sum_var) * peak_d_asdn + (1 / l_reliability / sum_var) * peak_l_asdn + ((1 / a_reliability) / sum_var) * peak_a_asdn;
            activations = [activations 1];
            activation_points = [activation_points peak_ap];
        else % If not contacted and not activated
            sum_var = 1/d_reliability;
            peak_int = ((1 / (d_reliability)) / sum_var) * peak_d_asdn;
            objectivevalue = objectivevalue + 2; % + 0.5 / muscleforce;
            activations = [activations 0];            
        end

        percepts = [percepts peak_int];
        delta = abs(peak_int - x.exp_ap);
        deltas = [deltas delta];
        objectivevalue = objectivevalue + delta;
        ovs = [ovs objectivevalue];

    end

    %% Print result plots to folder /results
    
    if testing == true % Reporting
        
        delta_avg = mean(deltas);
        delta_std = std(deltas);
        success_avg = mean(activations);
        activation_point_std = std(activation_points);
        activation_point_avg = mean(activation_points);
        ovs_avg = mean(ovs);
        muscleforce_avg = mean(muscleforces);
        muscleforce_std = std(muscleforces);
        results = table(delta_avg,delta_std,success_avg,ovs_avg,activation_point_avg,activation_point_std,muscleforce_avg,muscleforce_std);
        disp(results);
        writetable(results,'results/results.txt');
        
        plot(pulpcontact.time, pulpcontact.data,'Color','black');
        title('Pulp contraction');
        saveas(gcf,'results/pulpcontraction.fig')
        saveas(gcf,'results/pulpcontraction.png')

        plot(displacement.time, displacement.data,'Color','black');
        title('Finger tip position');
        saveas(gcf,'results/tipposition.fig');
        saveas(gcf,'results/tipposition.png');

        plot(velocity.time, velocity.data,'Color','black');
        title('Finger tip velocity');
        saveas(gcf,'results/tipvelocity.fig');
        saveas(gcf,'results/tipvelocity.png');

        plot(push_muscle_force.time, push_muscle_force.data,'Color','black');
        title('Muscle force');
        saveas(gcf,'results/muscleforce.fig');
        saveas(gcf,'results/muscleforce.png');

        plot(push_muscle_force.time, abs(push_muscle_force.data),'Color','black');
        title('Muscle force');
        saveas(gcf,'results/muscleforce2.fig');
        saveas(gcf,'results/muscleforce2.png');

        plot(abs(buttonheight.data),abs(push_muscle_force.data),'Color','black');
        title('Button displacement vs. Muscle Force');
        saveas(gcf,'results/buttonheight-force.fig');
        saveas(gcf,'results/buttonheight-force.png');

        plot(abs(push_muscle_force.data), abs(buttonheight.data),'Color','black');
        title('Muscle force vs. Button height');
        saveas(gcf,'results/force-buttonheight.fig');
        saveas(gcf,'results/force-buttonheight.png');

        plot(acceleration.time, acceleration.data,'Color','black');
        title('Finger tip acceleration');
        saveas(gcf,'results/tipacceleration.fig');
        saveas(gcf,'results/tipacceleration.png');

        plot(buttonheight.time, buttonheight.data,'Color','black');
        title('Button height');
        saveas(gcf,'results/buttonheight.fig');
        saveas(gcf,'results/buttonheight.png');

        histogram(activation_points);
        title('Make signal points');
        saveas(gcf,'results/activationpoints.fig');
        saveas(gcf,'results/activationpoints.png');

        histogram(percepts);
        title('Integrated p-center');
        saveas(gcf,'results/pcenters.fig');
        saveas(gcf,'results/pcenters.png');

        histogram(peak_ps);
        title('Tactile: p-center');
        saveas(gcf,'results/pcenters-pulp.fig');
        saveas(gcf,'results/pcenters-pulp.png');

        histogram(peak_ds);
        title('Displacement p-center');
        saveas(gcf,'results/pcenters-displacement.fig');
        saveas(gcf,'results/pcenters-displacement.png');

        histogram(peak_ls);
        title('Light: p-center');
        saveas(gcf,'results/pcenters-light.fig');
        saveas(gcf,'results/pcenters-light.png');

        histogram(peak_as);
        title('Audition: p-center');
        saveas(gcf,'results/pcenters-audition.fig');
        saveas(gcf,'results/pcenters-audition.png');

        histogram(muscleforces);
        title('Muscle forces');
        saveas(gcf,'results/muscleforces.fig');
        saveas(gcf,'results/muscleforces.png');
               
        plot(raw_pulpcontact.time, raw_pulpcontact.data,'Color','black');
        title('Pulp contraction');
        saveas(gcf,'results/pulpcontraction-raw.fig');
        saveas(gcf,'results/pulpcontraction-raw.png');

        plot(raw_displacement.time, raw_displacement.data,'Color','black');
        title('Displacement of finger tip');
        saveas(gcf,'results/displacement-raw.fig');
        saveas(gcf,'results/displacement-raw.png');

        plot(Finger.fstart_height-raw_displacement.data, -velocity.data,'Color','black');
        title('Finger Displacement - Finger Velocity');
        saveas(gcf,'results/displacement-fingervelocity.fig');
        saveas(gcf,'results/displacement-fingervelocity.png');

        plot(displacements, velocities,'Color','black');
        title('Finger Displacement - Finger Velocity');
        saveas(gcf,'results/displacements-fingervelocities.fig');
        saveas(gcf,'results/displacements-fingervelocities.png');
        
        plot(-buttonheight.data, -velocity.data,'Color','black');
        title('Button Displacement - Finger Velocity');
        saveas(gcf,'results/buttonheight-fingervelocity.fig');
        saveas(gcf,'results/buttonheight-fingervelocity.png');

        plot(-buttonheight.data, raw_pulpcontact.data,'Color','black');
        title('Button Displacement - Pulp Contact');
        saveas(gcf,'results/buttonheight-contact.fig');
        saveas(gcf,'results/buttonheight-contact.png');

    end

% Compute objective function, weighing temporal precision (ovs) and muscle
% effort (muscleforces)
objective = mean(ovs) + mean(muscleforces) * 0.33; 

end

