Nostradamus

src/main/java/telraam/App.java

@@ -1,11 +1,11 @@
 package telraam;
 
 import io.dropwizard.core.Application;
+import io.dropwizard.core.setup.Bootstrap;
+import io.dropwizard.core.setup.Environment;
 import io.dropwizard.jdbi3.JdbiFactory;
 import io.dropwizard.jdbi3.bundles.JdbiExceptionsBundle;
 import io.dropwizard.jersey.setup.JerseyEnvironment;
-import io.dropwizard.core.setup.Bootstrap;
-import io.dropwizard.core.setup.Environment;
 import io.federecio.dropwizard.swagger.SwaggerBundle;
 import io.federecio.dropwizard.swagger.SwaggerBundleConfiguration;
 import jakarta.servlet.DispatcherType;
@@ -24,13 +24,11 @@
 import telraam.logic.lapper.robust.RobustLapper;
 import telraam.logic.lapper.slapper.Slapper;
 import telraam.logic.positioner.Positioner;
-import telraam.logic.positioner.simple.SimplePositioner;
+import telraam.logic.positioner.nostradamus.Nostradamus;
 import telraam.station.FetcherFactory;
-import telraam.station.websocket.WebsocketFetcher;
 import telraam.util.AcceptedLapsUtil;
 import telraam.websocket.WebSocketConnection;
 
-import java.io.IOException;
 import java.util.EnumSet;
 import java.util.HashSet;
 import java.util.Set;
@@ -92,11 +90,11 @@ public void run(AppConfiguration configuration, Environment environment) {
 
         // Register websocket endpoint
         JettyWebSocketServletContainerInitializer.configure(
-            environment.getApplicationContext(),
-            (servletContext, wsContainer) -> {
-                wsContainer.setMaxTextMessageSize(65535);
-                wsContainer.addMapping("/ws", (req, res) -> new WebSocketConnection());
-            }
+                environment.getApplicationContext(),
+                (servletContext, wsContainer) -> {
+                    wsContainer.setMaxTextMessageSize(65535);
+                    wsContainer.addMapping("/ws", (req, res) -> new WebSocketConnection());
+                }
         );
 
         // Add api resources
@@ -142,7 +140,7 @@ public void run(AppConfiguration configuration, Environment environment) {
             // Set up positioners
             Set<Positioner> positioners = new HashSet<>();
 
-            positioners.add(new SimplePositioner(this.database));
+            positioners.add(new Nostradamus(this.database));
 
             // Start fetch thread for each station
             FetcherFactory fetcherFactory = new FetcherFactory(this.database, lappers, positioners);

src/main/java/telraam/api/TeamResource.java

@@ -51,9 +51,6 @@ public Team update(Team team, Optional<Integer> id) {
         Team previousTeam = this.get(id);
         Team ret = super.update(team, id);
 
-        System.out.println(previousTeam.getBatonId());
-        System.out.println(team.getBatonId());
-
         if (!Objects.equals(previousTeam.getBatonId(), team.getBatonId())) {
             this.batonSwitchoverDAO.insert(new BatonSwitchover(
                     team.getId(),

src/main/java/telraam/database/models/Detection.java

@@ -31,4 +31,10 @@ public Detection(Integer batonId, Integer stationId, Integer rssi, Float battery
         this.timestamp = timestamp;
         this.timestampIngestion = timestampIngestion;
     }
+
+    public Detection(Integer id, Integer stationId, Integer rssi) {
+        this.id = id;
+        this.stationId = stationId;
+        this.rssi = rssi;
+    }
 }

src/main/java/telraam/database/models/Station.java

@@ -35,4 +35,8 @@ public Station(String name, boolean isBroken) {
         this.name = name;
         this.broken = isBroken;
     }
+
+    public Station(Integer id) {
+        this.id = id;
+    }
 }

src/main/java/telraam/logic/positioner/Position.java

@@ -1,19 +1,34 @@
 package telraam.logic.positioner;
 
+import com.fasterxml.jackson.databind.PropertyNamingStrategies;
+import com.fasterxml.jackson.databind.annotation.JsonNaming;
 import lombok.Getter;
 import lombok.Setter;
-import telraam.database.models.Team;
-import telraam.websocket.WebSocketMessageSingleton;
 
-@Getter @Setter
+@Getter @Setter @JsonNaming(PropertyNamingStrategies.SnakeCaseStrategy.class)
 public class Position {
-    private int teamId;
-    private float progress; // Progress of the lap. Between 0-1
-    private float speed; // Current speed. Progress / second
+    private final int teamId;
+    private double progress; // Progress of the lap. Between 0-1
+    private double speed; // Current speed. progress / millisecond
+    private long timestamp; // Timestamp in milliseconds
 
     public Position(int teamId) {
         this.teamId = teamId;
         this.progress = 0;
         this.speed = 0;
+        this.timestamp = System.currentTimeMillis();
+    }
+
+    public Position(int teamId, double progress) {
+        this.teamId = teamId;
+        this.progress = progress;
+        this.speed = 0;
+        this.timestamp = System.currentTimeMillis();
+    }
+
+    public void update(double progress, double speed, long timestamp) {
+        this.progress = progress;
+        this.speed = speed;
+        this.timestamp = timestamp;
     }
 }

src/main/java/telraam/logic/positioner/CircularQueue.java → src/main/java/telraam/logic/positioner/nostradamus/CircularQueue.java

@@ -1,7 +1,8 @@
-package telraam.logic.positioner;
+package telraam.logic.positioner.nostradamus;
 
 import java.util.LinkedList;
 
+// LinkedList with a maximum length
 public class CircularQueue<T> extends LinkedList<T> {
 
     private final int maxSize;

src/main/java/telraam/logic/positioner/nostradamus/DetectionList.java

@@ -0,0 +1,61 @@
+package telraam.logic.positioner.nostradamus;
+
+import lombok.Getter;
+import telraam.database.models.Detection;
+import telraam.database.models.Station;
+
+import java.sql.Timestamp;
+import java.util.ArrayList;
+import java.util.Comparator;
+import java.util.List;
+
+public class DetectionList extends ArrayList<Detection> {
+
+    private final int interval;
+    private final List<Integer> stations;
+    @Getter
+    private Detection currentPosition;
+    private Timestamp newestDetection;
+
+    public DetectionList(int interval, List<Station> stations) {
+        this.interval = interval;
+        this.stations = stations.stream().sorted(Comparator.comparing(Station::getDistanceFromStart)).map(Station::getId).toList();
+        this.currentPosition = new Detection(-1, 0, -100);
+        this.newestDetection = new Timestamp(0);
+    }
+
+    // Returns True if the added detection results in a new station
+    @Override
+    public boolean add(Detection e) {
+        super.add(e);
+
+        if (e.getTimestamp().after(newestDetection)) {
+            newestDetection = e.getTimestamp();
+        }
+
+        if (!e.getStationId().equals(currentPosition.getStationId()) && stationAfter(currentPosition.getStationId(), e.getStationId())) {
+            // Possible new position
+            if (e.getRssi() > currentPosition.getRssi() || !inInterval(currentPosition.getTimestamp(), newestDetection)) {
+                // Detection stored in currentPosition will change
+                int oldPosition = currentPosition.getStationId();
+                // Filter out old detections
+                removeIf(detection -> !inInterval(detection.getTimestamp(), newestDetection));
+
+                // Get new position
+                currentPosition = stream().max(Comparator.comparing(Detection::getRssi)).get();
+
+                return oldPosition != currentPosition.getStationId();
+            }
+        }
+
+        return false;
+    }
+
+    private boolean stationAfter(int oldStationId, int newStationId) {
+        return (stations.indexOf(newStationId) - stations.indexOf(oldStationId) + stations.size()) % stations.size() > 0;
+    }
+
+    private boolean inInterval(Timestamp oldest, Timestamp newest) {
+        return newest.getTime() - oldest.getTime() < interval;
+    }
+}

src/main/java/telraam/logic/positioner/nostradamus/Nostradamus.java

@@ -0,0 +1,154 @@
+package telraam.logic.positioner.nostradamus;
+
+import org.jdbi.v3.core.Jdbi;
+import telraam.database.daos.BatonSwitchoverDAO;
+import telraam.database.daos.StationDAO;
+import telraam.database.daos.TeamDAO;
+import telraam.database.models.BatonSwitchover;
+import telraam.database.models.Detection;
+import telraam.database.models.Station;
+import telraam.database.models.Team;
+import telraam.logic.positioner.Position;
+import telraam.logic.positioner.PositionSender;
+import telraam.logic.positioner.Positioner;
+
+import java.util.*;
+import java.util.concurrent.locks.Lock;
+import java.util.concurrent.locks.ReentrantLock;
+import java.util.logging.Logger;
+import java.util.stream.Collectors;
+
+public class Nostradamus implements Positioner {
+    private static final Logger logger = Logger.getLogger(Nostradamus.class.getName());
+    private final int INTERVAL_CALCULATE_MS = 500; // How often to handle new detections (in milliseconds)
+    private final int INTERVAL_FETCH_MS = 10000; // Interval between fetching baton switchovers (in milliseconds)
+    private final int INTERVAL_DETECTIONS_MS = 3000; // Amount of milliseconds to group detections by
+    private final int MAX_NO_DATA_MS = 30000; // Send a stationary position after receiving no station update for x amount of milliseconds
+    private final int MEDIAN_AMOUNT = 10; // Calculate the median running speed of the last x intervals
+    private final double AVERAGE_SPRINTING_SPEED_M_MS = 0.00684; // Average sprinting speed meters / milliseconds
+    private final int MIN_RSSI = -84; // Minimum rssi strength for a detection
+    private final int FINISH_OFFSET_M = 0; // Distance between the last station and the finish in meters
+    private final Jdbi jdbi;
+    private final List<Detection> newDetections; // Contains not yet handled detections
+    private Map<Integer, Integer> batonToTeam; // Baton ID to Team ID
+    private final Map<Integer, TeamData> teamData; // All team data
+    private final PositionSender positionSender;
+    private final Lock detectionLock;
+    private final Lock dataLock;
+
+    public Nostradamus(Jdbi jdbi) {
+        this.jdbi = jdbi;
+        this.newDetections = new ArrayList<>();
+        this.detectionLock = new ReentrantLock();
+        this.dataLock = new ReentrantLock();
+
+        // Will be filled by fetch
+        this.batonToTeam = new HashMap<>();
+        this.teamData = getTeamData();
+
+        this.positionSender = new PositionSender();
+
+        new Thread(this::fetch).start();
+        new Thread(this::calculatePosition).start();
+    }
+
+    // Initiate the team data map
+    private Map<Integer, TeamData> getTeamData() {
+        List<Station> stations = jdbi.onDemand(StationDAO.class).getAll();
+        stations.sort(Comparator.comparing(Station::getDistanceFromStart));
+        List<Team> teams = jdbi.onDemand(TeamDAO.class).getAll();
+
+        return teams.stream().collect(Collectors.toMap(
+                Team::getId,
+                team -> new TeamData(team.getId(), INTERVAL_DETECTIONS_MS, stations, MEDIAN_AMOUNT, AVERAGE_SPRINTING_SPEED_M_MS, FINISH_OFFSET_M)
+        ));
+    }
+
+    // Fetch all baton switchovers and replace the current one if there are any changes
+    private void fetch() {
+        List<BatonSwitchover> switchovers = jdbi.onDemand(BatonSwitchoverDAO.class).getAll();
+
+        Map<Integer, Integer> batonToTeam = switchovers.stream().sorted(
+                Comparator.comparing(BatonSwitchover::getTimestamp)
+        ).collect(Collectors.toMap(
+                BatonSwitchover::getNewBatonId,
+                BatonSwitchover::getTeamId,
+                (existing, replacement) -> replacement
+        ));
+
+        if (!this.batonToTeam.equals(batonToTeam)) {
+            dataLock.lock();
+            this.batonToTeam = batonToTeam;
+            dataLock.unlock();
+        }
+
+        // Sleep tight
+        try {
+            Thread.sleep(INTERVAL_FETCH_MS);
+        } catch (InterruptedException e) {
+            logger.severe(e.getMessage());
+        }
+    }
+
+    // handle all new detections and update positions accordingly
+    private void calculatePosition() {
+        Set<Integer> changedTeams = new HashSet<>(); // List of teams that have changed station
+        while (true) {
+            changedTeams.clear();
+            dataLock.lock();
+            detectionLock.lock();
+            for (Detection detection: newDetections) {
+                if (batonToTeam.containsKey(detection.getBatonId())) {
+                    Integer teamId = batonToTeam.get(detection.getBatonId());
+                    if (teamData.get(teamId).addDetection(detection)) {
+                        changedTeams.add(teamId);
+                    }
+                }
+            }
+            newDetections.clear();
+            detectionLock.unlock(); // Use lock as short as possible
+            dataLock.unlock();
+
+            if (!changedTeams.isEmpty()) {
+                // Update
+                for (Integer teamId: changedTeams) {
+                    teamData.get(teamId).updatePosition();
+                }
+
+                // Send new data to the websocket
+                positionSender.send(
+                        changedTeams.stream().map(team -> teamData.get(team).getPosition()).toList()
+                );
+            }
+
+            // Send a stationary position if no new station data was received recently
+            long now = System.currentTimeMillis();
+            for (Map.Entry<Integer, TeamData> entry: teamData.entrySet()) {
+                if (now - entry.getValue().getPreviousStationArrival() > MAX_NO_DATA_MS) {
+                    positionSender.send(
+                            Collections.singletonList(new Position(
+                                    entry.getKey(),
+                                    entry.getValue().getPosition().getProgress()
+                            ))
+                    );
+                }
+            }
+
+            // Goodnight
+            try {
+                Thread.sleep(INTERVAL_CALCULATE_MS);
+            } catch (InterruptedException e) {
+                logger.severe(e.getMessage());
+            }
+        }
+    }
+
+    @Override
+    public void handle(Detection detection) {
+        if (detection.getRssi() > MIN_RSSI) {
+            detectionLock.lock();
+            newDetections.add(detection);
+            detectionLock.unlock();
+        }
+    }
+}

src/main/java/telraam/logic/positioner/nostradamus/StationData.java

@@ -0,0 +1,38 @@
+package telraam.logic.positioner.nostradamus;
+
+import telraam.database.models.Station;
+
+import java.util.ArrayList;
+import java.util.List;
+
+// Record containing all data necessary for TeamData
+public record StationData(
+        Station station, // The station
+        Station nextStation, // The next station
+        List<Long> times, // List containing the times (in ms) that was needed to run from this station to the next one.
+        int index, // Index of this station when sorting a station list by distanceFromStart
+        float currentProgress, // The progress value of this station
+        float nextProgress // The progress value of the next station
+) {
+    public StationData() {
+        this(
+                new Station(-10),
+                new Station(-9),
+                new ArrayList<>(0),
+                -10,
+                0F,
+                0F
+        );
+    }
+
+    public StationData(List<Station> stations, int index, int averageAmount, int totalDistance) {
+        this(
+                stations.get(index),
+                stations.get((index + 1) % stations.size()),
+                new CircularQueue<>(averageAmount),
+                index,
+                (float) (stations.get(index).getDistanceFromStart() / totalDistance),
+                (float) (stations.get((index + 1) % stations.size()).getDistanceFromStart() / totalDistance)
+        );
+    }
+}