%% llbn_feedback_demo.m
% Demonstration of LLBN feedback:
% m(t) = ∫ [SC(t) - e_dot(t)] dt
% and equivalence m(t) = Δe_d - Δe(t), *if* the SC burst area = Δe_d

clear; clc; close all;


%% Simulation parameters
Fs   = 1000;            % sampling frequency (Hz)
dt   = 1/Fs;            % time step (s)
Tend = 0.5;             % total simulation time (s)
t    = 0:dt:Tend;       % time vector

%% Desired saccade (from SC)
DeltaE_d = 40;          % desired eye displacement (deg)

% [SC,SC_amp] = SCburst(Fs,Tend,DeltaE_d);

[SC,SC_amp] = skewedSCburst(Fs,Tend,DeltaE_d);

%% Burst nonlinearity parameters (EBN/IBN)
Vmax = 200;            % max eye velocity (deg/s)
m0   = 5;              % saturation constant for motor error (deg)
% Nonlinear mapping: e_dot = Vmax * (1 - exp(-m/m0))

%% Preallocate variables
N     = numel(t);
m     = zeros(1, N);   % motor error from LLBN integrator
e_dot = zeros(1, N);   % eye velocity
e     = zeros(1, N);   % eye position

% nlfun = @(x,beta)(Vmax * (1 - exp(-m_pos / m0)));

nlfun = @(x,beta)(beta(1) * (1 - exp(-x / beta(2))));

%% Main simulation loop
for k = 1:N-1
    % 1. LLBN integrator: dm/dt = SC - e_dot
    dm        = SC(k) - e_dot(k);
    m(k+1)    = m(k) + dt * dm;
    
    % 2. Burst generator (EBN/IBN): nonlinear mapping from m to e_dot
	% m_pos = m(k+1);
    m_pos        = max(m(k+1), 0);   % no negative motor error in this simple model
    % e_dot(k+1)   = Vmax * (1 - exp(-m_pos / m0));
	e_dot(k+1)   = nlfun(m_pos,[Vmax m0]);
    % Try uncommenting the next line for a linear burst:
    % e_dot(k+1) = Vmax*m_pos/m0;
    
    % 3. Eye position: integrate velocity
    e(k+1) = e(k) + dt * e_dot(k+1);
end

%% Compute displacement and theoretical motor error
DeltaE = e - e(1);                 % accumulated eye displacement
m_alt  = DeltaE_d - DeltaE;        % geometric motor error

fprintf('Final eye displacement ≈ %.2f deg (desired = %.2f deg)\n', ...
    DeltaE(end), DeltaE_d);

%% Plot results

figure(1)
clf;
subplot(321)
plot(t, SC, 'k-', 'LineWidth', 1.5);
hold on
nicegraph;
axis normal;
axis off;
ylim([-0.1* max(SC) 1.1*max(SC)])

subplot(323)
plot(t, e_dot, 'k-', 'LineWidth', 1.5);
nicegraph;
axis normal;
axis off;
ylim([-0.1* max(e_dot) 1.1*max(e_dot)])

subplot(322)
plot(t, DeltaE, 'k-', 'LineWidth', 1.5);
nicegraph;
axis normal;
axis off;
ylim([-0.1* max(DeltaE) 1.1*max(DeltaE)])

subplot(324)
plot(t, m, 'k-', 'LineWidth', 1.5);
nicegraph;
axis normal;
axis off;
ylim([-0.1* max(m) 1.1*max(m)])

subplot(326)
plot(t, -e, 'k-', 'LineWidth', 1.5);
nicegraph;
axis normal;
axis off;
ylim([1.1*min(-e) -0.1* min(-e)])

figure(2);
clf

subplot(211)
plot(t,m,'k-','LineWidth',2);
nicegraph;
axis normal;

hold on

subplot(212)
plot(m,e_dot,'k-','LineWidth',2);
hold on

nicegraph;
axis normal;
cnt = 0;
for ii = 180:40:380
	cnt = cnt+1;
	subplot(212)
	plot(m(ii),e_dot(ii),'o','MarkerFaceColor','w','MarkerSize',24,'LineWidth',1.5)
		text(m(ii),e_dot(ii),num2str(cnt),'FontSize',21,'HorizontalAlignment','center','VerticalAlignment','middle');

			subplot(211)
	plot(t(ii),m(ii),'o','MarkerFaceColor','w','MarkerSize',24,'LineWidth',1.5)
		text(t(ii),m(ii),num2str(cnt),'FontSize',21,'HorizontalAlignment','center','VerticalAlignment','middle');
end

subplot(212)
xline(m0);
yline((1-exp(-1))*Vmax);
plot(m0,(1-exp(-1))*Vmax,'ko','MarkerFaceColor','w','MarkerSize',32,'LineWidth',2);
yline(Vmax,':','LineWidth',2);
ylim([-0.1 1.1]*Vmax)
%% Save

fname = fullfile('..','..','images','nonlinearburst.svg');
savegraph(fname,'svg');



function [SC,SC_amp] = skewedSCburst(Fs,Tend,DeltaE_d)

%% Parameters
% Fs   = 1000;            % sampling frequency (Hz)
dt   = 1/Fs;
% Tend = 0.5;             % total simulation time (s)
t    = 0:dt:Tend;

% DeltaE_d = 20;          % desired eye displacement (deg)

SC      = zeros(size(t));   % SC(t) burst command
SC_rt   = 0.10;             % 100 ms reaction time (burst onset)
t_rise  = 0.01;             % 10 ms rise time  (fast)
t_plate = 0.02;             % 20 ms plateau
t_fall  = 0.07;             % 70 ms linear fall (slow, skewed tail)

% Indices for segments
idx_on   = round(SC_rt/dt);
idx_rise = idx_on + (0:round(t_rise/dt));   % including onset
idx_plate= idx_rise(end) + (1:round(t_plate/dt));
idx_fall = idx_plate(end) + (1:round(t_fall/dt));

% Keep indices within bounds
idx_rise(idx_rise > numel(t))   = [];
idx_plate(idx_plate > numel(t)) = [];
idx_fall(idx_fall > numel(t))   = [];

% Shape with unit amplitude (we'll scale later)
shape = zeros(size(t));

% 1) Fast rise (you can choose ramp or step; here: simple step to 1)
shape(idx_rise) = 1;

% 2) Plateau at 1
shape(idx_plate) = 1;

% 3) Linear ramp-down from 1 → 0 over t_fall
if ~isempty(idx_fall)
    n_fall = numel(idx_fall);
    shape(idx_fall) = linspace(1, 0, n_fall);
end

% (Optionally) Ensure no overlap issues
shape(idx_fall(end)+1:end) = 0;

% Scale so that ∫ SC(t) dt = Δe_d
area_shape = trapz(t, shape);            % area for unit-amplitude shape
SC_amp     = DeltaE_d / area_shape;      % scaling factor
SC         = SC_amp * shape;

fprintf('Area under SC(t) ≈ %.2f deg (desired ΔE_d = %.2f deg)\n', ...
        trapz(t, SC), DeltaE_d);

%% Plot to inspect the skewed SC pulse
figure('Color','w');
plot(t, SC, 'k', 'LineWidth', 1.5); hold on;
plot(t, shape, 'r--', 'LineWidth', 1); % unit-amplitude shape for reference
xlabel('Time (s)');
ylabel('SC(t)');
legend('Scaled SC(t)', 'Unit-amplitude shape', 'Location','Best');
title('Skewed SC burst with linear ramp-down');
grid on;

end

function [SC,SC_amp] = SCburst(Fs,Tend,DeltaE_d)

%% Simulation parameters
% Fs   = 1000;            % sampling frequency (Hz)
dt   = 1/Fs;            % time step (s)
% Tend = 0.5;             % total simulation time (s)
t    = 0:dt:Tend;       % time vector

%% Desired saccade (from SC)
% DeltaE_d = 20;          % desired eye displacement (deg)

SC     = zeros(size(t));    % SC(t) burst command
SC_rt  = 0.10;              % 100 ms reaction time
SC_dur = 0.05;              % 50 ms SC burst duration

rt_idx  = round(SC_rt/dt);        % index of burst onset
dur_idx = round(SC_dur/dt);       % number of samples in burst

% IMPORTANT: scale amplitude so that area under SC(t) equals ΔE_d
% ∫ SC(t) dt ≈ SC_amp * SC_dur = ΔE_d  →  SC_amp = ΔE_d / SC_dur
SC_amp = DeltaE_d / SC_dur;       % units: deg/s (desired velocity-like signal)
SC(rt_idx:(rt_idx+dur_idx)) = SC_amp;

% Check: numerical area under SC(t) ≈ ΔE_d
DeltaE_from_SC = trapz(t, SC);    % should be ~ 20 deg
fprintf('Area under SC(t) ≈ %.2f deg\n', DeltaE_from_SC);

end
