% Kinematic simulator for a non-holonomic robot with car-like steering
function positions = simulator(bot_start, traj_start, traj_goal)
    delta_t = 0.1; % Simulation time step in seconds
    max_time = 30 / delta_t; % A simulation can run for no more than 30 seconds
    p = bot_start; % Robot position
    speed = 0; % Robot speed
    bearing = 0; % Robot bearing (heading)
    min_turning_radius = 0.5;
    max_speed = 5;
    positions = zeros(max_time, 2);
    
    for t = 1:max_time
        % Get control inputs from the [[controller]] function
        [throttle, steering] = verySimple_controller(p, bearing, traj_start, traj_goal);
        
        steering = min(max(-1, steering), 1);
        
        throttle = min(max(0, throttle), 1);
        
        d = (speed + 0.5 * throttle * delta_t) * delta_t;
        final_speed = speed + throttle * delta_t;
        
        speed = max(min(max_speed, final_speed), 0);
        
        if abs(steering) < 0.0001
            p = p+ rotate([d;0], bearing);
        else
            r = (1.0 / abs(steering)) * min_turning_radius;
            theta_change = d / r;
            offset = rotate([r;0], bearing + pi * 0.5);
            if steering < 0
                c = p + offset;
            else
                c = p - offset;
                theta_change = -theta_change;
            end
            bearing = bearing + theta_change;
            p = rotate(p - c, theta_change) + c;
        end
        
        positions(t,:) = p'; % Store the new position
        
        % Check if we've reached the goal
        if norm(p - traj_goal) < 1
            break;
        end
    end
    positions = positions(1:t,:); % Trim off any elements we don't need
        