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. 

% 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; 

% drawing
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
q = [p.l/2, 0]; % initialize state

figure(1)
clf()

for i = 1:length(t)
    % compute thrust
    f_l = 1.1 * p.m * p.g;
    % compute dynamics
    qdot = fly_altitude_dynamics(q, f_l, p); 
    
    % update state based on dynamics
    q = q + dt * qdot; 
    
    % 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
        hold off
        title(['t=' num2str(t(i),'%1.3f')])
        xlim([-.06, .06])
        drawnow
    end
end

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
