% script to run braitenberg vehicle simulator
% Sawyer B. Fuller, 2018.10.17 

% simulation contsants
TFINAL = 3; % time duration of simulation
PLOT = 1; % whether to plot trajectory results
DT = .1; 
ARENA_SIZE = [-1.5, 1.5, -1.5, 1.5]; % m

% vehicle parameters
p.length = .2; % vehicle length, m
p.width = .1; % vehicle width
p.initial_x = -1; % m
p.initial_y = 0; 
p.initial_theta = -.1; % rad
% environment parameters
env.light_source_x = 0; % position of light source, m
env.light_source_y = 0; 

time = 0:DT:TFINAL; 

%state vector is [x, y, theta]' (m, m, rad)
q_history = [p.initial_x, p.initial_y, p.initial_theta]'; % initialize state
u_history = []; 

figure(1)
clf()

for idx = 1:length(time)
    q = q_history(:, end); 
    t = time(idx);
    distance_to_light = simulate_environment(env, q, p);
    u = light_response(distance_to_light, p); 
    q = q + DT * robot_dynamics(q, u, p);
    q_history = [q_history q];
    u_history = [u_history u];
    
    % animate
    plotbox(q(1), q(2), q(3), p);
    hold on
    plot(env.light_source_x, env.light_source_y, 'r.')
    hold off
    axis equal
    title(['t=' num2str(time(idx),'%1.3f')])
    axis(ARENA_SIZE) 
    drawnow
end

if PLOT
    figure(1)
    plot(q_history(1,:), q_history(2,:))
    axis equal
    hold on
    for idx = 1:3:length(time)
        plotbox(q_history(1,idx), q_history(2,idx), q_history(3,idx), p);
    end
    plot(env.light_source_x, env.light_source_y, 'r.')
    xlabel('x position (m)')
    ylabel('y position (m)')
    hold off
    axis(ARENA_SIZE)
end

function distance_to_light_sensor = simulate_environment(env, q, p)
    % extract state
    x = q(1);
    y = q(2); 
    theta = q(3);
   
    % fill this in
    % distance_to_light_sensor = 
end

function u = light_response(distance_to_light, p)
    u = 2 * distance_to_light; 
end

function qdot = robot_dynamics(q, u, p)
    % extract state and inputs
    theta = q(3); 
    wheel_velocity = u; 
    
    % fill this in
    % xdot = ...
    % ydot = ...
    % thetadot = ...
    
    qdot = [xdot; ydot; thetadot]; 
end
 
function plotbox(x, y, th, p)
    diag = 1/2*sqrt(p.length^2+p.width^2);
    angl = atan(p.width/p.length);
    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)], ...
        'b');
end

