function RESULTS = main()
% example braitenburg vehicle simulator
% Sawyer B. Fuller, 2016.10.03 

global DT
TFINAL = 2; % time duration of simulation
PLOT = 1; % whether to plot trajectory results
UPDATE_RATE_HZ = 50; 
ARENA_SIZE = [-1.5, 1.5, -1.5, 1.5]; % m

% vehicle parameters
p.baseline_velocity = .75; % m/s  
p.length = .2; % vehicle length, m
p.width = .1; % vehicle width
p.light_source_x = 1; % position of light source, m
p.light_source_y = 1; 

% visualizing position
diagnl = 1/2*sqrt(p.length^2+p.width^2);
angl = atan(p.length/p.width);
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_HZ; 
t = 0:DT:TFINAL; 

%state vector: x, y, theta (m, m, rad)
q0 = [0, -1, .2]; % initial state
u0 = 0; % initial input
% initialize state 
RESULTS.q = zeros(length(t), length(q0)); 
RESULTS.u = zeros(length(t), 2);
q = q0; 
    
figure(1)
clf()

for idx = 1:length(t)
    u = compute_light_response(t(idx), q, p); 
    q = q + DT * robot_dynamics(t(idx), q, u, p);
    RESULTS.q(idx,:) = q; 
    RESULTS.u(idx,:) = u; 

    % animate
    plotbox(q(1), q(2), -q(3), angl, diagnl);
    hold on
    plot(p.light_source_x, p.light_source_y, 'r.')
    hold off
    axis equal
    title(['t=' num2str(t(idx),'%1.3f')])
    axis(ARENA_SIZE) 
    drawnow
end

if PLOT
    figure(1)
    plot(RESULTS.q(:,1), RESULTS.q(:,2))
    axis equal
    hold on
    for idx = 1:10:length(t)
        plotbox(RESULTS.q(idx, 1), RESULTS.q(idx, 2), -RESULTS.q(idx, 3), angl, diagnl);
    end
    plot(p.light_source_x, p.light_source_y, 'r.')
    xlabel('x position (m)')
    ylabel('y position (m)')
    hold off
    axis(ARENA_SIZE)
end
end

function u = compute_light_response(t, q, p)
    x = q(1);
    y = q(2); 
    theta = q(3);
    gain = 3;
    
    % fill this in
    % wheel_velocity = gain * ...
    % note that the x and y position of the light source are
    % p.light_source_x and p.light_source_y. 
    
    u = wheel_velocity; 
end

function qdot = robot_dynamics(t, q, u, p)
    % extract state and inputs
    theta = q(3); 
    wheel_velocity = u; 
    
    % fill this in
    % xdot = ...
    % ydot = ...
    % thetadot = ...

    qdot = [xdot, ydot, thetadot]; 
end
