function fly_simulator()
% 1-dimensional robofly simulator and renderer 
% Sawyer B. Fuller, 2016.09.28 
% 
% states are q = [z, zdot]
%
% simulation parameters: 
% * TFINAL: time duration of simulation  
% * PLOT: whether to plot trajectory results
% * set the controller under 'controllers' section. 

% reset persistent variables in functions
clear all

global dt

% simulation parameters
TFINAL = 1; % time duration of simulation
PLOT = 1; % whether to plot trajectory results
UPDATE_RATE = 500; % steps/second

p.l = .013; % length of body
p.h = .0025; % thickness of body
p.m = 80e-6; %mass of robofly
p.g = 9.81; 
p.max_f_l = 1.5 * p.m * p.g; 

% rendering
diagnl = 1/2*sqrt(p.l^2+p.h^2);
angl = atan(p.l/p.h);
plotbox = inline('plot([x+diag*cos(th-angl) x+diag*cos(th+angl) x+diag*cos(th-angl+pi) x+diag*cos(th+angl+pi) x+diag*cos(th-angl)], [y+diag*sin(th-angl) y+diag*sin(th+angl) y+diag*sin(th-angl+pi) y+diag*sin(th+angl+pi) y+diag*sin(th-angl)])','x','y','th','angl','diag');

dt = 1/UPDATE_RATE; 
t = 0:dt:TFINAL; 

% q (the state) = z, zdot
q0 = [p.l/2, 0]; % initial state
qd = [.05, 0]; % desired state

% initialize record
q_log = zeros(length(t), length(q0)); 
u_log = zeros(length(t), 1);
q = q0; 

% controller selection
%thrust_controller = @open_loop_thrust;
thrust_controller = @proportional_thrust;
%thrust_controller = @PD_thrust;

figure(1) ;
clf()
for i = 1:length(t)
    % compute control signal and resulting dynamics
    f_l = thrust_controller(q, qd, p); 
    f_l = clip(f_l, p.max_f_l); 
    qdot = fly_altitude_dynamics(q, f_l, p); 
    
    % update state based on dynamics
    q = q + dt * qdot; 
    
    % enter into log
    q_log(i,:) = q'; 
    u_log(i) = f_l; 

    % animate
    if mod(i, 3) == 0 % don't update every frame for speed
        plot([-.05, .05], [0, 0]) % show where the ground is
        hold on
        plotbox(0, q(1), 0, angl, diagnl); % draw robot
        axis equal
        plot(.03, qd(1), '+') % setpoint
        hold off
        title(['t=' num2str(t(i),'%1.3f')])
        xlim([-.06, .06])
        drawnow
    end
end

if PLOT
    figure(1)
    % floor
    plot([-.05, .05], [0, 0])
    hold on
    %    trajectory of cm
    plot(0, q_log(:,1), 'k')
    for i = 1:10:length(t)
        plotbox(0, q_log(i, 1), 0, angl, diagnl); % draw robot
    end
    axis equal
    % setpoint location
    plot(.03, qd(1), '+')
    axis([-.06, .06, -.01, qd(1)+.01])
    xlabel('x position (m)')
    ylabel('y position (m)')
    hold off
    figure(2)
    subplot(2,1,1)
    plot(t, q_log(:,1), '-', [t(1), t(end)], [qd(1), qd(1)], '--')
    ylabel('z position (m)')
    subplot(2,1,2)
    plot(t, u_log, '-')
    ylabel('f_l (N)')
    xlabel('time (s)')
    
end

end

% controller functions ---------------------

function f_l = open_loop_thrust(q, qd, p)
f_l = p.m * p.g * 1.1; % feedforward gravity cancellation
end

function f_l = proportional_thrust(q, qd, p)
error = qd(1) - q(1);
k = 1e-2;
f_l = p.m * p.g + k * error; % feedforward gravity cancellation
end

function f_l = PD_thrust(q, qd, p)
% compute
% q = [z, zdot]
error = qd(1) - q(1);
error_dot = qd(2) - q(2);
k = 1e-2; % proportional gain
kd = 1e-3; % derivative gain
f_l = k * error + kd * error_dot; 
f_l = f_l + p.m * p.g; % feedforward gravity cancellation
last_error = error; % save for next function call
end


function qdot = fly_altitude_dynamics(q, u, p)
% given state and inputs u from controller, calculate derivatives

% extract state
z = q(1);
zdot = q(2); 

% extract inputs
f_l = u; 

vzdot = 1/p.m * f_l - p.g; % body coordinates

% output vector
qdot = [zdot, vzdot];
end

function clipped = clip(x, absmax)
    clipped = max(min(x, absmax), -absmax);
end
