feat: simulate wcs traffic control

javaTest/src/main/java/com/ft546/wcs/demo/model/DispatchQueueEntry.java

@@ -60,6 +60,33 @@ public final class DispatchQueueEntry {
         );
     }
 
+    public static DispatchQueueEntry avoidance(
+        String queueId,
+        String vehicleId,
+        String startSite,
+        String endSite
+    ) {
+        if (queueId == null || queueId.trim().isEmpty()) {
+            throw new IllegalArgumentException("queue id must not be blank");
+        }
+        if (vehicleId == null || vehicleId.trim().isEmpty()) {
+            throw new IllegalArgumentException("vehicle id must not be blank");
+        }
+        if (startSite == null || startSite.trim().isEmpty()
+            || endSite == null || endSite.trim().isEmpty()) {
+            throw new IllegalArgumentException("avoidance sites must not be blank");
+        }
+        return new DispatchQueueEntry(
+            queueId,
+            queueId,
+            vehicleId,
+            QueueType.AVOIDANCE,
+            startSite,
+            endSite,
+            9
+        );
+    }
+
     public void markIssued(List<String> issuedPath) {
         if (issued != 0) {
             throw new IllegalStateException("queue has already been issued");

javaTest/src/main/java/com/ft546/wcs/demo/traffic/GridMap.java

@@ -0,0 +1,81 @@
+package com.ft546.wcs.demo.traffic;
+
+import com.ft546.wcs.demo.model.Site;
+
+import java.util.ArrayList;
+import java.util.Collection;
+import java.util.Collections;
+import java.util.LinkedHashMap;
+import java.util.LinkedHashSet;
+import java.util.List;
+import java.util.Map;
+import java.util.Set;
+
+public final class GridMap {
+    private final Map<String, Site> sites = new LinkedHashMap<String, Site>();
+    private final Map<String, Set<String>> neighbors =
+        new LinkedHashMap<String, Set<String>>();
+    private final Set<String> blockedSites = new LinkedHashSet<String>();
+    private final Map<String, String> cacheSitesByChannel =
+        new LinkedHashMap<String, String>();
+
+    public void addSite(Site site) {
+        if (site == null) {
+            throw new IllegalArgumentException("site must not be null");
+        }
+        if (sites.containsKey(site.getId())) {
+            throw new IllegalArgumentException("duplicate site: " + site.getId());
+        }
+        sites.put(site.getId(), site);
+        neighbors.put(site.getId(), new LinkedHashSet<String>());
+    }
+
+    public void connect(String left, String right) {
+        requireSite(left);
+        requireSite(right);
+        neighbors.get(left).add(right);
+        neighbors.get(right).add(left);
+    }
+
+    public void block(String siteId) {
+        requireSite(siteId);
+        blockedSites.add(siteId);
+    }
+
+    public void setCacheSite(String channel, String siteId) {
+        if (channel == null || channel.trim().isEmpty()) {
+            throw new IllegalArgumentException("channel must not be blank");
+        }
+        requireSite(siteId);
+        cacheSitesByChannel.put(channel, siteId);
+    }
+
+    public Site getSite(String siteId) {
+        return sites.get(siteId);
+    }
+
+    public Map<String, Site> getSites() {
+        return Collections.unmodifiableMap(sites);
+    }
+
+    public List<String> getNeighbors(String siteId) {
+        requireSite(siteId);
+        List<String> result = new ArrayList<String>(neighbors.get(siteId));
+        Collections.sort(result);
+        return result;
+    }
+
+    public Set<String> getBlockedSites() {
+        return Collections.unmodifiableSet(blockedSites);
+    }
+
+    public String getCacheSite(String channel) {
+        return cacheSitesByChannel.get(channel);
+    }
+
+    private void requireSite(String siteId) {
+        if (!sites.containsKey(siteId)) {
+            throw new IllegalArgumentException("unknown site: " + siteId);
+        }
+    }
+}

javaTest/src/main/java/com/ft546/wcs/demo/traffic/GridPathPlanner.java

@@ -0,0 +1,66 @@
+package com.ft546.wcs.demo.traffic;
+
+import java.util.ArrayDeque;
+import java.util.ArrayList;
+import java.util.Collections;
+import java.util.Deque;
+import java.util.HashMap;
+import java.util.HashSet;
+import java.util.LinkedHashSet;
+import java.util.List;
+import java.util.Map;
+import java.util.Set;
+
+public final class GridPathPlanner {
+
+    public List<String> shortestPath(
+        GridMap map,
+        String start,
+        String end,
+        Set<String> dynamicBlocked
+    ) {
+        if (map == null || map.getSite(start) == null || map.getSite(end) == null) {
+            throw new IllegalArgumentException("map, start and end sites are required");
+        }
+
+        Set<String> blocked = new LinkedHashSet<String>(map.getBlockedSites());
+        if (dynamicBlocked != null) {
+            blocked.addAll(dynamicBlocked);
+        }
+        blocked.remove(start);
+        if (blocked.contains(end)) {
+            return Collections.emptyList();
+        }
+
+        Deque<String> open = new ArrayDeque<String>();
+        Set<String> visited = new HashSet<String>();
+        Map<String, String> parent = new HashMap<String, String>();
+        open.add(start);
+        visited.add(start);
+
+        while (!open.isEmpty()) {
+            String current = open.removeFirst();
+            if (current.equals(end)) {
+                return buildPath(parent, end);
+            }
+            for (String neighbor : map.getNeighbors(current)) {
+                if (!blocked.contains(neighbor) && visited.add(neighbor)) {
+                    parent.put(neighbor, current);
+                    open.addLast(neighbor);
+                }
+            }
+        }
+        return Collections.emptyList();
+    }
+
+    private List<String> buildPath(Map<String, String> parent, String end) {
+        List<String> reversed = new ArrayList<String>();
+        String current = end;
+        while (current != null) {
+            reversed.add(current);
+            current = parent.get(current);
+        }
+        Collections.reverse(reversed);
+        return Collections.unmodifiableList(reversed);
+    }
+}

javaTest/src/main/java/com/ft546/wcs/demo/traffic/TrafficController.java

@@ -0,0 +1,180 @@
+package com.ft546.wcs.demo.traffic;
+
+import com.ft546.wcs.demo.model.DecisionCode;
+import com.ft546.wcs.demo.model.DispatchQueueEntry;
+import com.ft546.wcs.demo.model.Site;
+import com.ft546.wcs.demo.model.VehicleSnapshot;
+
+import java.util.Collection;
+import java.util.LinkedHashSet;
+import java.util.List;
+import java.util.Set;
+
+public final class TrafficController {
+    private final GridMap map;
+    private final GridPathPlanner planner;
+
+    public TrafficController(GridMap map, GridPathPlanner planner) {
+        if (map == null || planner == null) {
+            throw new IllegalArgumentException("map and planner are required");
+        }
+        this.map = map;
+        this.planner = planner;
+    }
+
+    public TrafficDecision plan(
+        VehicleSnapshot vehicle,
+        DispatchQueueEntry queue,
+        Collection<VehicleSnapshot> vehicles,
+        Collection<DispatchQueueEntry> queues
+    ) {
+        requireInputs(vehicle, queue, vehicles, queues);
+        Site start = map.getSite(vehicle.getCurrentSite());
+        Site target = map.getSite(queue.getEndSite());
+        if (start == null || target == null) {
+            return TrafficDecision.hold(
+                DecisionCode.REJECT_INVALID_SITE,
+                "unknown start or target site"
+            );
+        }
+
+        VehicleSnapshot channelBlocker =
+            findChannelBlocker(vehicle, target, vehicles);
+        if (channelBlocker != null) {
+            if (hasActiveQueue(channelBlocker.getVehicleId(), queues)) {
+                return TrafficDecision.hold(
+                    DecisionCode.HOLD_CHANNEL_BLOCKED,
+                    "channel " + target.getChannel() + " is occupied by vehicle "
+                        + channelBlocker.getVehicleId()
+                );
+            }
+            String cacheSite = map.getCacheSite(target.getChannel());
+            if (cacheSite == null) {
+                return TrafficDecision.hold(
+                    DecisionCode.HOLD_CHANNEL_BLOCKED,
+                    "channel is blocked and has no configured cache site"
+                );
+            }
+            DispatchQueueEntry avoidance = DispatchQueueEntry.avoidance(
+                "AVOID-" + channelBlocker.getVehicleId() + "-FOR-" + queue.getQueueId(),
+                channelBlocker.getVehicleId(),
+                channelBlocker.getCurrentSite(),
+                cacheSite
+            );
+            return TrafficDecision.avoidance(
+                avoidance,
+                "move vehicle " + channelBlocker.getVehicleId()
+                    + " to cache site " + cacheSite
+            );
+        }
+
+        Set<String> blocked = buildBlockedSites(vehicle, vehicles, queues, queue);
+        List<String> path = planner.shortestPath(
+            map,
+            vehicle.getCurrentSite(),
+            queue.getEndSite(),
+            blocked
+        );
+        if (path.isEmpty()) {
+            return TrafficDecision.hold(
+                DecisionCode.HOLD_NO_PATH,
+                "no path from " + vehicle.getCurrentSite() + " to " + queue.getEndSite()
+            );
+        }
+        return TrafficDecision.path(path);
+    }
+
+    private Set<String> buildBlockedSites(
+        VehicleSnapshot vehicle,
+        Collection<VehicleSnapshot> vehicles,
+        Collection<DispatchQueueEntry> queues,
+        DispatchQueueEntry currentQueue
+    ) {
+        Set<String> blocked = new LinkedHashSet<String>();
+        for (VehicleSnapshot other : vehicles) {
+            if (!other.getVehicleId().equals(vehicle.getVehicleId())
+                && !other.isConnected()) {
+                blocked.add(other.getCurrentSite());
+            }
+        }
+        for (DispatchQueueEntry otherQueue : queues) {
+            if (otherQueue.getQueueId().equals(currentQueue.getQueueId())
+                || otherQueue.getVehicleId().equals(vehicle.getVehicleId())
+                || otherQueue.getPath().isEmpty()) {
+                continue;
+            }
+            VehicleSnapshot owner = findVehicle(otherQueue.getVehicleId(), vehicles);
+            int startIndex = owner == null
+                ? 0
+                : otherQueue.getPath().indexOf(owner.getCurrentSite());
+            if (startIndex < 0) {
+                startIndex = 0;
+            }
+            blocked.addAll(
+                otherQueue.getPath().subList(startIndex, otherQueue.getPath().size())
+            );
+        }
+        return blocked;
+    }
+
+    private VehicleSnapshot findChannelBlocker(
+        VehicleSnapshot vehicle,
+        Site target,
+        Collection<VehicleSnapshot> vehicles
+    ) {
+        for (VehicleSnapshot other : vehicles) {
+            if (other.getVehicleId().equals(vehicle.getVehicleId())
+                || !other.isConnected()
+                || other.getFloor() != target.getFloor()
+                || other.getWorkMode() != 5
+                || other.getStatus() != 1) {
+                continue;
+            }
+            Site otherSite = map.getSite(other.getCurrentSite());
+            if (otherSite != null
+                && target.getChannel().equals(otherSite.getChannel())
+                && !otherSite.getId().equals(vehicle.getCurrentSite())) {
+                return other;
+            }
+        }
+        return null;
+    }
+
+    private boolean hasActiveQueue(
+        String vehicleId,
+        Collection<DispatchQueueEntry> queues
+    ) {
+        for (DispatchQueueEntry queue : queues) {
+            if (vehicleId.equals(queue.getVehicleId())
+                && queue.getExecutionStatus() != 4) {
+                return true;
+            }
+        }
+        return false;
+    }
+
+    private VehicleSnapshot findVehicle(
+        String vehicleId,
+        Collection<VehicleSnapshot> vehicles
+    ) {
+        for (VehicleSnapshot vehicle : vehicles) {
+            if (vehicleId.equals(vehicle.getVehicleId())) {
+                return vehicle;
+            }
+        }
+        return null;
+    }
+
+    private void requireInputs(
+        VehicleSnapshot vehicle,
+        DispatchQueueEntry queue,
+        Collection<VehicleSnapshot> vehicles,
+        Collection<DispatchQueueEntry> queues
+    ) {
+        if (vehicle == null || queue == null || vehicles == null || queues == null) {
+            throw new IllegalArgumentException(
+                "vehicle, queue, vehicles and queues are required"
+            );
+        }
+    }
+}

javaTest/src/main/java/com/ft546/wcs/demo/traffic/TrafficDecision.java

@@ -0,0 +1,69 @@
+package com.ft546.wcs.demo.traffic;
+
+import com.ft546.wcs.demo.model.DecisionCode;
+import com.ft546.wcs.demo.model.DispatchQueueEntry;
+
+import java.util.ArrayList;
+import java.util.Collections;
+import java.util.List;
+import java.util.Optional;
+
+public final class TrafficDecision {
+    private final DecisionCode code;
+    private final List<String> path;
+    private final DispatchQueueEntry avoidanceQueue;
+    private final String reason;
+
+    private TrafficDecision(
+        DecisionCode code,
+        List<String> path,
+        DispatchQueueEntry avoidanceQueue,
+        String reason
+    ) {
+        this.code = code;
+        this.path = Collections.unmodifiableList(new ArrayList<String>(path));
+        this.avoidanceQueue = avoidanceQueue;
+        this.reason = reason;
+    }
+
+    public static TrafficDecision path(List<String> path) {
+        return new TrafficDecision(
+            DecisionCode.PATH_READY,
+            path,
+            null,
+            "path ready: " + path
+        );
+    }
+
+    public static TrafficDecision hold(DecisionCode code, String reason) {
+        return new TrafficDecision(code, Collections.<String>emptyList(), null, reason);
+    }
+
+    public static TrafficDecision avoidance(
+        DispatchQueueEntry avoidanceQueue,
+        String reason
+    ) {
+        return new TrafficDecision(
+            DecisionCode.AVOIDANCE_CREATED,
+            Collections.<String>emptyList(),
+            avoidanceQueue,
+            reason
+        );
+    }
+
+    public DecisionCode getCode() {
+        return code;
+    }
+
+    public List<String> getPath() {
+        return path;
+    }
+
+    public Optional<DispatchQueueEntry> getAvoidanceQueue() {
+        return Optional.ofNullable(avoidanceQueue);
+    }
+
+    public String getReason() {
+        return reason;
+    }
+}

javaTest/src/test/java/com/ft546/wcs/demo/TrafficControllerTest.java

@@ -0,0 +1,168 @@
+package com.ft546.wcs.demo;
+
+import com.ft546.wcs.demo.model.DecisionCode;
+import com.ft546.wcs.demo.model.DispatchQueueEntry;
+import com.ft546.wcs.demo.model.DispatchTask;
+import com.ft546.wcs.demo.model.Site;
+import com.ft546.wcs.demo.model.VehicleSnapshot;
+import com.ft546.wcs.demo.traffic.GridMap;
+import com.ft546.wcs.demo.traffic.GridPathPlanner;
+import com.ft546.wcs.demo.traffic.TrafficController;
+import com.ft546.wcs.demo.traffic.TrafficDecision;
+import org.junit.jupiter.api.Test;
+
+import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.Collections;
+import java.util.List;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+class TrafficControllerTest {
+
+    @Test
+    void plannerUsesDeterministicAlternateRouteAroundABlockedSite() {
+        GridMap map = diamondMap();
+        GridPathPlanner planner = new GridPathPlanner();
+
+        List<String> path = planner.shortestPath(
+            map,
+            "A",
+            "C",
+            Collections.singleton("B")
+        );
+
+        assertEquals(Arrays.asList("A", "D", "C"), path);
+    }
+
+    @Test
+    void otherQueueRemainingPathAndOfflineVehicleSitesAreLocked() {
+        GridMap map = diamondMap();
+        TrafficController controller = controller(map);
+        VehicleSnapshot car1 = vehicle("1", "A", true);
+        VehicleSnapshot car2 = vehicle("2", "B", true);
+        VehicleSnapshot offlineCar = vehicle("3", "D", false);
+        DispatchQueueEntry car1Queue = businessQueue("TASK-1", "1", "A", "C");
+        DispatchQueueEntry car2Queue = businessQueue("TASK-2", "2", "B", "C");
+        car2Queue.markIssued(Arrays.asList("B", "C"));
+
+        TrafficDecision decision = controller.plan(
+            car1,
+            car1Queue,
+            Arrays.asList(car1, car2, offlineCar),
+            Arrays.asList(car1Queue, car2Queue)
+        );
+
+        assertEquals(DecisionCode.HOLD_NO_PATH, decision.getCode());
+        assertTrue(decision.getReason().contains("no path"));
+    }
+
+    @Test
+    void sameChannelBlockerCreatesOnePriorityNineAvoidanceQueue() {
+        GridMap map = channelMap();
+        TrafficController controller = controller(map);
+        VehicleSnapshot car1 = vehicle("1", "A", true);
+        VehicleSnapshot blocker = vehicle("2", "B", true);
+        DispatchQueueEntry car1Queue = businessQueue("TASK-1", "1", "A", "TARGET");
+
+        TrafficDecision decision = controller.plan(
+            car1,
+            car1Queue,
+            Arrays.asList(car1, blocker),
+            Collections.singletonList(car1Queue)
+        );
+
+        assertEquals(DecisionCode.AVOIDANCE_CREATED, decision.getCode());
+        assertTrue(decision.getAvoidanceQueue().isPresent());
+        DispatchQueueEntry avoidance = decision.getAvoidanceQueue().get();
+        assertEquals(DispatchQueueEntry.QueueType.AVOIDANCE, avoidance.getQueueType());
+        assertEquals("2", avoidance.getVehicleId());
+        assertEquals("CACHE", avoidance.getEndSite());
+        assertEquals(9, avoidance.getPriority());
+    }
+
+    @Test
+    void existingAvoidanceIsNotDuplicatedAndMovingBlockerReleasesTheChannel() {
+        GridMap map = channelMap();
+        TrafficController controller = controller(map);
+        VehicleSnapshot car1 = vehicle("1", "A", true);
+        VehicleSnapshot blocker = vehicle("2", "B", true);
+        DispatchQueueEntry car1Queue = businessQueue("TASK-1", "1", "A", "TARGET");
+        DispatchQueueEntry avoidance =
+            DispatchQueueEntry.avoidance("AVOID-2", "2", "B", "CACHE");
+
+        TrafficDecision held = controller.plan(
+            car1,
+            car1Queue,
+            Arrays.asList(car1, blocker),
+            Arrays.asList(car1Queue, avoidance)
+        );
+
+        assertEquals(DecisionCode.HOLD_CHANNEL_BLOCKED, held.getCode());
+        assertFalse(held.getAvoidanceQueue().isPresent());
+
+        VehicleSnapshot movedBlocker = vehicle("2", "CACHE", true);
+        TrafficDecision released = controller.plan(
+            car1,
+            car1Queue,
+            Arrays.asList(car1, movedBlocker),
+            Collections.singletonList(car1Queue)
+        );
+
+        assertEquals(DecisionCode.PATH_READY, released.getCode());
+        assertEquals(Arrays.asList("A", "B", "TARGET"), released.getPath());
+    }
+
+    private TrafficController controller(GridMap map) {
+        return new TrafficController(map, new GridPathPlanner());
+    }
+
+    private GridMap diamondMap() {
+        GridMap map = new GridMap();
+        map.addSite(new Site("A", 1, 0, 0, "START"));
+        map.addSite(new Site("B", 1, 1, 0, "UPPER"));
+        map.addSite(new Site("C", 1, 2, 0, "TARGET"));
+        map.addSite(new Site("D", 1, 1, 1, "LOWER"));
+        map.connect("A", "B");
+        map.connect("B", "C");
+        map.connect("A", "D");
+        map.connect("D", "C");
+        return map;
+    }
+
+    private GridMap channelMap() {
+        GridMap map = new GridMap();
+        map.addSite(new Site("A", 1, 0, 0, "START"));
+        map.addSite(new Site("B", 1, 1, 0, "AISLE-1"));
+        map.addSite(new Site("TARGET", 1, 2, 0, "AISLE-1"));
+        map.addSite(new Site("CACHE", 1, 1, 1, "CACHE"));
+        map.connect("A", "B");
+        map.connect("B", "TARGET");
+        map.connect("B", "CACHE");
+        map.setCacheSite("AISLE-1", "CACHE");
+        return map;
+    }
+
+    private DispatchQueueEntry businessQueue(
+        String taskId,
+        String vehicleId,
+        String source,
+        String target
+    ) {
+        DispatchTask task = new DispatchTask(
+            taskId,
+            DispatchTask.TaskType.OUTBOUND,
+            source,
+            target,
+            1,
+            2
+        );
+        return DispatchQueueEntry.business(task, vehicleId);
+    }
+
+    private VehicleSnapshot vehicle(String id, String site, boolean connected) {
+        return new VehicleSnapshot(id, 1, site, 5, 1, 80, connected, true, false);
+    }
+}