function robosim()

figure(1);
xlim([-50 50]);
ylim([-50 50]);

global events;
global vehicle;
global vehicle_state;
global vehicle_def;
global current_timestamp;

vehicle = [];

vehicle_def = [-1 -1 1 1; -2 4 4 -2];

events = [ 
    0 0 0;
    10 10 0;
    20 10 90;
    30 10 180;
    40 10 270;
    50 10 0;
    ];

% [X Y Omega V]
vehicle_state = [0 0 0 0];

update_vehicle([0 0 0]);

current_timestamp = 0;
t = timer('TimerFcn',@timer_expired, 'Period', .2, 'ExecutionMode', 'fixedRate');
start(t);

while 1
  string = input('S >> ', 's');
  
  fprintf(1,'\n');
  switch string
    case 'exit'
      break
    otherwise
      fprintf(1, 'Invalid command\n');
  end
end

stop(t);
delete(vehicle);

function update_vehicle(new_state)

global vehicle;
global vehicle_state;
global vehicle_def;

rad = new_state(3)/360*2*pi;

rot = [cos(rad) -sin(rad); sin(rad) cos(rad)];

vehicle_patch = (rot * vehicle_def);
vehicle_patch = vehicle_patch + new_state(1:2)'*ones(1,size(vehicle_patch,2));

if ( isempty(vehicle) ) 
  vehicle = patch(vehicle_patch(1,:), vehicle_patch(2,:), [1 1 1 1]);
else
  set(vehicle, 'XData', vehicle_patch(1,:), 'YData', vehicle_patch(2,:));
end

vehicle_state(1:3) = new_state;

refreshdata(1);

% TIMER
function timer_expired(obj, event)

global current_timestamp;
global vehicle_state;

current_timestamp = current_timestamp + obj.Period*50;

new_state = [cos(current_timestamp/360*2*pi)*10 sin(current_timestamp/360*2*pi)*10 current_timestamp 0];
update_vehicle(new_state(1:3));
