%
% Developers note: In order to ensure good compatability, this program
% should be developed in Matlab R2012a.
%
function tank_sim(frame_time)
if nargin < 1
    frame_time = 0.1;
end

% Create figure
fig = figure(...
    'Visible','off',...
    'CloseRequestFcn',@stop_gui);

[ui_elements, tank_plt, data_plt] = setup_figure(fig);

set(fig,'Visible','on');

% Initialize simulation
sim = setup_simulation();

% Create timer that periodically update the gui
frame_time = sim.dt*ceil(frame_time/sim.dt); % Round to multiple of sample time
update_timer = timer(...
    'ExecutionMode', 'fixedRate', ...
    'Period', frame_time,...
    'BusyMode', 'queue',...
    'UserData',sim,...
    'TimerFcn', @update);
start(update_timer);

    % Timer function that periodically is called and updates the gui and
    % simulation
    function update(timerobj,~)
        % Read ui states
        params = get_ui_params(ui_elements);
        
        if params.pause
            return % Do nothing more if we have paused
        end
        
        % Get simulation data and settings
        sim = get(timerobj,'UserData'); 
        
        frame_time = get(timerobj,'Period');
        multiplier = 1*( params.speed_x2*2 + ~params.speed_x2*1 )*(params.speed_x3*3 + ~params.speed_x3*1);
        buf = create_SOA(sim);
        % Run simulation (multiple times if fast forwarding)
        for i = 1:multiplier
            sim = update_simulation(sim,frame_time,params);
            buf = add_to_SOA(buf,sim);
        end
        
        % Store simulation state for next timer call
        set(timerobj,'UserData',sim);
      
        % Update plots
        update_data_plot(data_plt,buf.t,buf.y,params.r*ones(1,multiplier),buf.u,buf.u_p,buf.u_i,buf.u_d);
        update_tank_plot(tank_plt,sim.x,sim.u,params.bv1,params.av3,params.av4,params.pour);
    end

    % Callback for window close
    function stop_gui(~,~)
        % Stop and delete timer and kill window
        stop(update_timer)
        delete(update_timer)
        delete(gcf)
    end
end



%
% Simulation
%
function [gamma,delta,sigma] = get_tank_params()
% Params from the manual, they are a bit slow and the pump power seem very
% big. Therefore it is reduced...
A = 4.9*10^(-4)/1; % m^2
a = 3.1*10^(-6); % m^2
alpha = 2.1*10^(-5)/2; % m^3s^-1
beta = 6.25; % m^-1
g = 9.82; % ms^-2

% params after variable change
gamma = a/A*sqrt(2*g*beta);
delta = alpha*beta/A;

sigma = 0.002; % Measurement noise variance
end

function sim = setup_simulation()
sim = struct();

% Time and sampling
sim.dt = 0.01; % Discretization time
sim.t = 0; % Time elapsed

% Process state
sim.x = zeros(2,1); % Tank heights
sim.y = 0; % Measurement
sim.y_prev = 0; % Old measurment for derivative estimate
sim.tank = 'upper';

% Regulator state
sim.u = 0; % Used control signal
sim.u_p = 0; % Proportional part of pid
sim.u_i = 0; % Intergral part
sim.u_d = 0; % Derivative part
end

function sim = update_simulation(sim, time_step, params)
% Tank selection
if ~strcmp(params.tank,sim.tank)
    sim.tank = params.tank;
    % Tank changed, reset measurement memory and controller state
    if strcmp(sim.tank,'upper')
        sim.y = sim.x(1);
        sim.y_prev = sim.x(1);
    else
        sim.y = sim.x(2);
        sim.y_prev = sim.x(2);
    end
    sim.u_p = 0;
    sim.u_i = 0;
    sim.u_d = 0;
end
upper_tank = strcmp(sim.tank,'upper');

% Control method selection
pid_enabled = strcmp(params.loop,'closed');
if ~pid_enabled
    % Open loop, reset controller state and set fixed control signal
    sim.u = params.u;
    sim.u_p = 0;
    sim.u_i = 0;
    sim.u_d = 0;
end

% Calculate new time and number of simulation steps to get there
n_sim_step = round(time_step/sim.dt); % This should always be a whole number but still...
sim.t = sim.t + time_step;

[~,~,sigma] = get_tank_params();

% Simulate from t to t_new by basic explicit euler
for i = 1:n_sim_step
    % Update measurement
    sim.y_prev = sim.y;
    if upper_tank
        sim.y = sim.x(1);
    else
        sim.y = sim.x(2);
    end
    sim.y = sim.y + sigma*randn();

    % Calculate control signal if closed loop
    if pid_enabled
        [sim.u,sim.u_p,sim.u_i,sim.u_d] = pid_update(sim,params);
    end
    % Update state
    dx_dt = tank_derivatives(sim,params);
    sim.x = sim.x + sim.dt.*dx_dt;
end

end

function [u,u_p,u_i,u_d] = pid_update(sim,params)
K = params.K;
Ti = params.Ti;
Td = params.Td;
r = params.r;

% Calculate the different parts
u_p = 0;
if params.K_enabled
    u_p = K*(r-sim.y);
end

u_i = 0;
if params.Ti_enabled && Ti > 0
    u_i = sim.u_i + K*sim.dt/Ti*(r-sim.y);
end

N = 5;
u_d = 0;
if params.Td_enabled
    u_d = Td/(Td + N*sim.dt) * (sim.u_d - K*N*(sim.y - sim.y_prev));
end

% Add together, check for saturation
u = u_p + u_i + u_d;

saturate = false;
if u > 1
    u = 1;
    saturate = true;
elseif u < 0
    u = 0;
    saturate = true;
end

% Don't update integral part if saturated to avoid windup
if saturate
    u_i = sim.u_i;
end
end

function dx_dt = tank_derivatives(sim,params)
% x(1) upper tank, x(2) lower tank

[gamma,delta,~] = get_tank_params();

% Adjust for valve settings and disturbance
gamma_1 = gamma*(~params.av3); % block outlet of first tank
gamma_2 = gamma*(~params.av4); % block outlet of second tank
true_input = delta*(1-params.bv1*0.5)*sim.u + delta*params.pour; % Divert 50% of flow from pump and/or add pour water

% Calculate derivative
sqrt_x = sqrt(max(sim.x,0)); % Ensure positivity when x is numerically 0
dx_dt = [
    -gamma_1*sqrt_x(1)+true_input
    gamma_1*sqrt_x(1)-gamma_2*sqrt_x(2)
    ];

% Check if overflowing
if sim.x(1) >= 1 && dx_dt(1) > 0
    dx_dt(1) = 0;
end
if sim.x(2) >= 1 && dx_dt(2) > 0
    dx_dt(2) = 0;
end
end


%
% Figure setup
%
function [ui_elements, tank_plt, data_plt] = setup_figure(fig)
set(fig,'outerposition',[160 90 1280 720],'color',[.8 .8 .8])

tank_axes = axes('Parent',fig,...
    'Position',[0.05 0.05 0.25 0.9]);
ctrl_axes = axes('Parent',fig,...
    'Position',[0.35 0.08 0.43 0.36]);
meas_axes = axes('Parent',fig,...
    'Position',[0.35 0.59 0.43 0.36]);
ctrl_panel = uipanel('Parent',fig,...
    'BorderType','none',...
    'BackgroundColor',[.8 .8 .8],...
    'Position',[0.805,0.025,0.17,.95]);

ui_elements = create_ui_elements(ctrl_panel);
tank_plt = setup_tank_plot(tank_axes);
data_plt = setup_data_plot(meas_axes, ctrl_axes);

end


%
% Tank and water plotting and animation
%
function plt = setup_tank_plot(ax)
% General setup
set(ax,...
    'NextPlot','add',...
    'ylim',[0,1],...
    'xlim',[0,1],...
    'YTick',[],...
    'XTick',[],...
    'YTickMode','manual',...
    'XTickMode','manual',...
    'Box','on');
axes(ax) % Set the axes as current for drawing

% Tank geometry data
left_wall = 0.5; % Horizontal position of left wall of tanks
right_wall = 0.9; % Horizontal position of right wall of tanks
upper_bottom = 0.5; % Vertical position of bottom of upper tank
lower_bottom = 0.1; % Vertical position of bottom of lower tank
height = 0.3; % Tank height
centerline = (left_wall + right_wall)/2; % The centerline of the tanks

padding = 0.07; % A small padding measurement when drawin the tray, pump line etc.
flow_radius = 0.05; % Half the width of maximally flowing water
pump_line_left = 0.1; % The horisontal position of the left vertical pump pipe
pump_line_top = 0.95; % The vertical position of the upper horizontal pump pipe


% OBS! Draw all water first so it is visually "behind" all other lines.

% Draw all dynamic water objects and place in return struct for future animation
plt = struct();
plt.upper_water = draw_tank_water(upper_bottom,left_wall,right_wall,height);
plt.lower_water = draw_tank_water(lower_bottom,left_wall,right_wall,height);
plt.pump_flow = draw_flowing_water(centerline,flow_radius,upper_bottom+height+padding,upper_bottom);
plt.upper_outflow = draw_flowing_water(centerline,flow_radius,upper_bottom,lower_bottom);
plt.lower_outflow = draw_flowing_water(centerline,flow_radius,lower_bottom,lower_bottom-padding);
plt.bv1_spillflow = draw_flowing_water((pump_line_left+left_wall)/2,flow_radius/2,pump_line_top,-0.1);
plt.pour_flow = draw_flowing_water((centerline+flow_radius + right_wall)/2, flow_radius, 1.1, upper_bottom);
plt.overflow = draw_tank_water(-0.01,-0.01,1.01,1.02);

% Draw tank walls
draw_tank_walls(ax,upper_bottom,left_wall,right_wall,height)
draw_tank_walls(ax,lower_bottom,left_wall,right_wall,height)

% Draw catch tray
fill(...
    [left_wall-padding left_wall-padding right_wall+padding right_wall+padding],...
    [lower_bottom-padding/2 lower_bottom-padding lower_bottom-padding lower_bottom-padding/2],...
    'b',...
    'LineStyle','none');
plot(ax,...
    [left_wall-padding left_wall-padding right_wall+padding right_wall+padding],...
    [lower_bottom lower_bottom-padding lower_bottom-padding lower_bottom],...
    'k',...
    'LineWidth',5);

% Draw Pump Tube
plot(ax,...
    [left_wall-padding pump_line_left pump_line_left centerline centerline],...
    [lower_bottom-padding+0.01 lower_bottom-padding+0.01 pump_line_top pump_line_top upper_bottom+height+padding],...
    'k',...
    'LineWidth',10);
plot(ax,...
    [centerline-flow_radius centerline+flow_radius],...
    [upper_bottom+height+padding upper_bottom+height+padding],...
    'k',...
    'LineWidth',10);
end

function update_tank_plot(plt,x,u,bv1,av3,av4,pour)
[gamma,delta,~] = get_tank_params();
update_tank_water(plt.upper_water,x(1));
update_tank_water(plt.lower_water,x(2));
update_flowing_water(plt.pump_flow,u*(1-0.5*bv1));
update_flowing_water(plt.bv1_spillflow,u*bv1);
update_flowing_water(plt.upper_outflow, ~av3*sqrt(max(x(1),0))*gamma/delta); % Ensure positivity when x is numerically 0
update_flowing_water(plt.lower_outflow, ~av4*sqrt(max(x(2),0))*gamma/delta); % Ensure positivity when x is numerically 0
update_flowing_water(plt.pour_flow, pour);

if (x(1) >= 0.995) && (x(2) >= 0.995) && (pour || u > 0)
    increment_tank_water(plt.overflow,0.1/15);
else
    increment_tank_water(plt.overflow,-0.1/5);
end
end

function draw_tank_walls(ax,bot,left,right,height)
plot(ax,...
    [left left right right],...
    [bot+height bot bot bot+height],...
    'k',...
    'LineWidth',5);
end

function water = draw_tank_water(bot,left,right,height)
water = struct();
water.height = height;
water.drawing = fill(...
    [left right right left],...
    [bot bot bot bot],...
    'b',...
    'LineStyle','none');
end

function update_tank_water(water,h)
    ydata = get(water.drawing,'YData');
    ydata(3:4) = ydata(1)+h*water.height;
    set(water.drawing,'YData',ydata);
end

function increment_tank_water(water,inc)
    ydata = get(water.drawing,'YData');
    ydata(3:4) = min(ydata(1:2)+water.height,max(ydata(1:2),ydata(3:4) + inc*water.height));
    set(water.drawing,'YData',ydata);
end

function water = draw_flowing_water(cl,radius,start,stop)
water = struct(...
    'centerline',cl,...
    'start',start,...
    'stop',stop,...
    'radius',radius);
[xdata, ydata] = flowing_water_xydata(water,0);
water.drawing = fill(xdata,ydata,'b','LineStyle','none');
end

function update_flowing_water(water,flow)
[xdata,ydata] = flowing_water_xydata(water,flow);
set(water.drawing,'XData',xdata,'YData',ydata);
end

function [xdata, ydata] = flowing_water_xydata(water_geometry,flow)
cl = water_geometry.centerline;
start = water_geometry.start;
stop = water_geometry.stop;
radius = water_geometry.radius;

if flow < 0.01 % Don't draw anything for really small flows
    xdata = [cl cl cl cl];
    ydata = [start start start start];
else
    r_edge = cl-flow*radius;
    l_edge = cl+flow*radius;
    xdata = [r_edge r_edge l_edge l_edge];
    ydata = [start stop stop start];
end
end



%
% Measurements and control signal plotting and animation
%
function plt = setup_data_plot(meas_axis, ctrl_axis)
% Return struct with all needed plot data
plt = struct(...
    'time_window',80,...
    'meas_axis',meas_axis,...
    'ctrl_axis',ctrl_axis);

% General appearance
set(meas_axis,...
    'NextPlot','add',...
    'ylim',[-0.05,1.05],...
    'Box','on',...
    'YGrid','on',...
    'XGrid','on');
set(ctrl_axis,...
    'NextPlot','add',...
    'ylim',[-1.05,1.05],...
    'Box','on',...
    'YGrid','on',...
    'YLimMode','manual',...
    'XGrid','on');

% Generate some initial data
plt.r_series = plot(meas_axis, linspace(-plt.time_window,0,100),zeros(100,1),'r--');
plt.y_series = plot(meas_axis, linspace(-plt.time_window,0,100),zeros(100,1),'k');
plt.up_series = plot(ctrl_axis, linspace(-plt.time_window,0,100),zeros(100,1),'r--');
plt.ui_series = plot(ctrl_axis, linspace(-plt.time_window,0,100),zeros(100,1),'b:');
plt.ud_series = plot(ctrl_axis, linspace(-plt.time_window,0,100),zeros(100,1),'g-.');
plt.u_series = plot(ctrl_axis, linspace(-plt.time_window,0,100),zeros(100,1),'k');

% Set labels and titles
title(meas_axis,'Measurement (Water Level)','FontSize',14);
l = legend(meas_axis,'Reference','Water Level','Location','northwest');
set(l,'FontSize',13);
xlabel(meas_axis,'time [s]','FontSize',14);
title(ctrl_axis,'Control Signal (u = u_p + u_I + u_D)','FontSize',14);
l = legend(ctrl_axis,'u_P','u_I','u_D','u','Location','southwest');
set(l,'FontSize',13)
xlabel(ctrl_axis,'time [s]','FontSize',14);
end

function update_data_plot(plt,t,y,r,u,u_p,u_i,u_d)
set(plt.meas_axis,'xlim',[t(end)-plt.time_window,t(end)]); % For some reason it is a factor 10 faster to do this before the add_data_to_series function calls.
set(plt.ctrl_axis,'xlim',[t(end)-plt.time_window,t(end)]);

add_data_to_series(plt.y_series,t,y,plt.time_window)
add_data_to_series(plt.r_series,t,r,plt.time_window)
add_data_to_series(plt.u_series,t,u,plt.time_window)
add_data_to_series(plt.up_series,t,u_p,plt.time_window)
add_data_to_series(plt.ui_series,t,u_i,plt.time_window)
add_data_to_series(plt.ud_series,t,u_d,plt.time_window)
end

function add_data_to_series(series,x,y,x_win)
xdata = get(series,'XData');
ydata = get(series,'YData');

idx_start = find(xdata >= x(end)-x_win,1);
xdata = xdata(idx_start:end);
ydata = ydata(idx_start:end);

xdata = [xdata x];
ydata = [ydata y];

set(series,'XData',xdata);
set(series,'YData',ydata);
end



%
% Input handling
%
% When a ui element is triggered, the corresponding callback will read the
% element state. It will then process and format it for our use and save
% this processed value in the 'UserData' field of the element. When the
% program later wish to read the state it simply loops through all elements
% and reads the 'UserData' field.
%
% We also mark each ui element with a 'tag'-string. This is used to
% construct a struct where each field is named after a tag and the field
% value is the corresponding 'UserData'.
%
function ui_elements = create_ui_elements(panel)
% Panels for control grouping
p_open_loop = uipanel('Parent',panel,...
    'Title','Manual Mode',...
    'FontSize',14,...
    'Position',[0 .915 1 .085]);

p_closed_loop = uipanel('Parent',panel,...
    'Title','Automatic Mode',...
    'FontSize',14,...
    'Position',[0 .665 1 .225]);

p_valves = uipanel('Parent',panel,...
    'Title','Valves',...
    'FontSize',14,...
    'Position',[0 .44 1 .2]);

p_options = uipanel('Parent',panel,...
    'Title','Process Options',...
    'FontSize',14,...
    'Position',[0 .025 1 .39]);


% Add the different controls
ui_elements = {};

pad = 0.02;

% manual control signal
ui_elements{end+1} = positive_numeric_input(p_open_loop, 0, 0, .6, 1, pad, 'um:', 'u', 1);

% PID params
ui_elements{end+1} = positive_numeric_input(p_closed_loop, 0, 3/4, .6, 1/4, pad, 'r:', 'r', 1);
ui_elements{end+1} = positive_numeric_input(p_closed_loop, 0, 2/4, .6, 1/4, pad, 'K:', 'K', 100);
ui_elements{end+1} = positive_numeric_input(p_closed_loop, 0, 1/4, .6, 1/4, pad, 'Ti:', 'Ti', 100);
ui_elements{end+1} = positive_numeric_input(p_closed_loop, 0, 0/4, .6, 1/4, pad, 'Td:', 'Td', 100);

% PID params enable/disable
ui_elements{end+1} = toggle_input(p_closed_loop, .6, 2/4, .4, 1/4, pad, 'Enable', 'K_enabled', true);
ui_elements{end+1} = toggle_input(p_closed_loop, .6, 1/4, .4, 1/4, pad, 'Enable', 'Ti_enabled', false);
ui_elements{end+1} = toggle_input(p_closed_loop, .6, 0/4, .4, 1/4, pad, 'Enable', 'Td_enabled', false);

% Valves
ui_elements{end+1} = toggle_input(p_valves, 0, 2/3, 1, 1/3, pad, 'BV1', 'bv1', false);
ui_elements{end+1} = toggle_input(p_valves, 0, 1/3, .5, 1/3, pad, 'AV3', 'av3', false);
ui_elements{end+1} = toggle_input(p_valves, .5, 1/3, .5, 1/3, pad, 'AV4', 'av4', false);
ui_elements{end+1} = toggle_input(p_valves, 0, 0, 1, 1/3, pad, 'Pour Water', 'pour', false);

% Process control
ui_elements{end+1} = two_alternative_input(p_options,0,2/3,1,1/3,pad,'Upper Tank', 'upper', 'Lower Tank', 'lower', 'tank');
ui_elements{end+1} = two_alternative_input(p_options,0,1/3,1,1/3,pad, 'Manual', 'open', 'Automatic', 'closed', 'loop');
ui_elements{end+1} = toggle_input(p_options, 0, 1/6, 1, 1/6, pad, 'Pause', 'pause', false);
ui_elements{end+1} = toggle_input(p_options, 0, 0/6, .5, 1/6, pad, '2x Speed', 'speed_x2', false);
ui_elements{end+1} = toggle_input(p_options, .5, 0/6, .5, 1/6, pad, '3x Speed', 'speed_x3', false);
end

function ui_elem = two_alternative_input(panel,hor,vert,width,height,pad,label_1,alt_1,label_2,alt_2,tag)
ui_elem = uibuttongroup(...
    'Parent',panel,...
    'Units','pixels',...
    'Tag',tag,...
    'UserData',true,...
    'Units','Normalized',...
    'Position',[hor+pad (1-pad)*vert+2*pad width-2*pad (1-pad)*height-pad],...
    'SelectionChangeFcn',@callback);
uicontrol('Parent',ui_elem,...
    'Style','radiobutton',...
    'String',label_1,...
    'FontSize',14,...
    'Units','Normalized',...
    'Position',[1/20 1/20+1/2 1-2/20 1/2-2/20],...
    'UserData',alt_1);
uicontrol('Parent',ui_elem,...
    'Style','radiobutton',...
    'String',label_2,...
    'FontSize',14,...
    'Units','Normalized',...
    'Position',[1/20 1/20 1-2/20 1/2-2/20],...
    'UserData',alt_2);

callback(ui_elem,'nothing'); % Set default value

    function callback(ui_element,~)
        button = get(ui_element,'SelectedObject');
        upper_tank = get(button,'UserData');
        set(ui_element,'UserData',upper_tank);
    end
end

function ui_elem = toggle_input(panel,hor,vert,width,height,pad,label,tag,default_value)
ui_elem = uicontrol(...
    'Parent', panel,...
    'Style','togglebutton',...
    'String',label,...
    'FontSize',14,...
    'Tag',tag,... % Used later to identify the parameter
    'Value',default_value,...
    'Units','Normalized',...
    'Position',[hor+pad (1-pad)*vert+2*pad width-2*pad (1-pad)*height-pad],...
    'Callback',@toggle_input_callback);
set(ui_elem,'UserData',get(ui_elem,'Value')) % Default value

    function toggle_input_callback(ui_element, ~)
        data = get(ui_element,'Value');
        set(ui_element,'UserData',data);
    end
end

function ui_elem = positive_numeric_input(panel,hor,vert,width,height,pad,label,tag,max_val)
uicontrol(...
    'Parent', panel,...
    'Style','text',...
    'FontSize',14,...
    'String',label,...
    'Units','Normalized',...
    'Position',[hor+pad (1-pad)*vert+pad+pad width/3-pad (1-pad)*height-pad]);
ui_elem = uicontrol(...
    'Parent', panel,...
    'Style','edit',...
    'String','0',...
    'Tag',tag,...
    'FontSize',14,...
    'Units','Normalized',...
    'Position',[hor+width/3 (1-pad)*vert+2*pad width*2/3-pad (1-pad)*height-pad],...
    'Callback',@numeric_input_callback);

set(ui_elem,'UserData',str2double(get(ui_elem,'String'))) % Default value

    function numeric_input_callback(ui_element, ~)
        data = get(ui_element,'String');
        val = str2double(data);
        if isnan(val) || val < 0
            val = 0;
            set(ui_element,'String','0');
        elseif val > max_val
            val = max_val;
            set(ui_element,'String',num2str(val));
        end
        set(ui_element,'UserData',val);
    end
end

function params = get_ui_params(ui_elements)
% Loop through all ui elements and extract the user data into a
% struct with fieldnames corresponding to the ui element tag.
params = struct();
for k=1:length(ui_elements)
    elem=ui_elements{k};
    
    data = get(elem,'UserData');
    tag = get(elem,'Tag');
    params.(tag) = data; % This is how you access a struct field through with name of the string in the variable tag
end
end


%
% Utility
%
function soa = create_SOA(soa)
% Creates an empty 'Struct of arrays'
fs = fieldnames(soa);
for k = 1:length(fs)
    f = fs{k};
    soa.(f) = [];
end
end

function soa = add_to_SOA(soa, st)
fs = fieldnames(soa);
for k = 1:length(fs)
    f = fs{k};
    soa.(f) = [soa.(f) st.(f)];
end
end