function vehicles = init_vehicle_agents(scenario)
%INIT_VEHICLE_AGENTS Creates the three vehicle-agent structures.
%
% Each agent contains:
%   - physical state [x, y, psi, v]
%   - role: leader/follower
%   - formation offset
%   - local terrain data
%   - history/logs for plotting

    nVehicles = size(scenario.start, 1);
    vehicles = repmat(empty_vehicle_agent(), nVehicles, 1);

    for i = 1:nVehicles
        vehicles(i).id = i;
        vehicles(i).state.x = scenario.start(i, 1);
        vehicles(i).state.y = scenario.start(i, 2);
        vehicles(i).state.psi = scenario.start(i, 3);
        vehicles(i).state.v = scenario.start(i, 4);

        if i == 1
            vehicles(i).role = 'leader';
        else
            vehicles(i).role = 'follower';
        end

        vehicles(i).formation_offset_body = scenario.formation.offsets_body(i, :);
        vehicles(i).status = 'active';
        vehicles(i).target = [scenario.start(i, 1), scenario.start(i, 2)];
        vehicles(i).last_control.a_cmd = 0;
        vehicles(i).last_control.delta_cmd = 0;
        vehicles(i).local_terrain = get_local_terrain(scenario.map, vehicles(i).state.x, vehicles(i).state.y);

        vehicles(i).history.t = [];
        vehicles(i).history.x = [];
        vehicles(i).history.y = [];
        vehicles(i).history.psi = [];
        vehicles(i).history.v = [];
        vehicles(i).history.target_x = [];
        vehicles(i).history.target_y = [];
        vehicles(i).history.mu = [];
        vehicles(i).history.slope_deg = [];
        vehicles(i).history.a_cmd = [];
        vehicles(i).history.delta_cmd = [];
    end
end

function vehicle = empty_vehicle_agent()
    vehicle.id = [];
    vehicle.role = '';
    vehicle.status = '';
    vehicle.state = struct('x', [], 'y', [], 'psi', [], 'v', []);
    vehicle.formation_offset_body = [0, 0];
    vehicle.target = [0, 0];
    vehicle.local_terrain = [];
    vehicle.last_control = struct('a_cmd', [], 'delta_cmd', []);
    vehicle.history = struct();
end
