From 19ee5c085e855941d67d6c07532b29c9ed1ebe19 Mon Sep 17 00:00:00 2001 From: NuttyShrimp Date: Wed, 20 Mar 2024 21:36:30 +0100 Subject: [PATCH 01/25] feat(station): send InitMessage on open connection --- src/main/java/telraam/station/Fetcher.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/telraam/station/Fetcher.java b/src/main/java/telraam/station/Fetcher.java index 753474a..626e26e 100644 --- a/src/main/java/telraam/station/Fetcher.java +++ b/src/main/java/telraam/station/Fetcher.java @@ -11,4 +11,5 @@ public interface Fetcher { int IDLE_TIMEOUT_MS = 4000; // Wait 4 seconds void fetch(); + } From 44832744e0d918dc7a2f561cac6391f20b2983e1 Mon Sep 17 00:00:00 2001 From: NuttyShrimp Date: Fri, 29 Mar 2024 17:50:21 +0100 Subject: [PATCH 02/25] refactor: Use jakarta websockets --- src/main/java/telraam/station/Fetcher.java | 1 - .../java/telraam/station/WebsocketClient.java | 58 +++++++++++++++++++ 2 files changed, 58 insertions(+), 1 deletion(-) create mode 100644 src/main/java/telraam/station/WebsocketClient.java diff --git a/src/main/java/telraam/station/Fetcher.java b/src/main/java/telraam/station/Fetcher.java index 626e26e..753474a 100644 --- a/src/main/java/telraam/station/Fetcher.java +++ b/src/main/java/telraam/station/Fetcher.java @@ -11,5 +11,4 @@ public interface Fetcher { int IDLE_TIMEOUT_MS = 4000; // Wait 4 seconds void fetch(); - } diff --git a/src/main/java/telraam/station/WebsocketClient.java b/src/main/java/telraam/station/WebsocketClient.java new file mode 100644 index 0000000..e57564a --- /dev/null +++ b/src/main/java/telraam/station/WebsocketClient.java @@ -0,0 +1,58 @@ +package telraam.station; + +import jakarta.websocket.*; + +import java.net.URI; + +@ClientEndpoint +public class WebsocketClient { + public interface MessageHandler { + void handleMessage(String message); + } + public interface OnOpenHandler { + void handleMsgOpen(); + } + + Session session = null; + private MessageHandler messageHandler; + private OnOpenHandler onOpenHandler; + + public WebsocketClient(URI endpointURI) { + try { + WebSocketContainer container = ContainerProvider.getWebSocketContainer(); + container.connectToServer(this, endpointURI); + } catch (Exception e) { + throw new RuntimeException(e); + } + } + + @OnOpen + public void onOpen(Session session) { + this.session = session; + } + + @OnClose + public void onClose(Session userSession, CloseReason reason) { + System.out.println("closing websocket"); + this.session = null; + } + + @OnMessage + public void onMessage(String message) { + if (this.messageHandler != null) { + this.messageHandler.handleMessage(message); + } + } + + public void addOnOpenHandler(OnOpenHandler openHandler) { + this.onOpenHandler = openHandler; + } + + public void addMessageHandler(MessageHandler msgHandler) { + this.messageHandler = msgHandler; + } + + public void sendMessage(String message) { + this.session.getAsyncRemote().sendText(message); + } +} From d35f66b0c8f4434422d0a6dfe55556571ba981d7 Mon Sep 17 00:00:00 2001 From: NuttyShrimp Date: Tue, 2 Apr 2024 01:13:00 +0200 Subject: [PATCH 03/25] feat: re-add http fetcher & move WS to own package --- .../java/telraam/station/WebsocketClient.java | 58 ------------------- .../station/websocket/WebsocketClient.java | 1 - .../station/websocket/WebsocketFetcher.java | 1 - 3 files changed, 60 deletions(-) delete mode 100644 src/main/java/telraam/station/WebsocketClient.java diff --git a/src/main/java/telraam/station/WebsocketClient.java b/src/main/java/telraam/station/WebsocketClient.java deleted file mode 100644 index e57564a..0000000 --- a/src/main/java/telraam/station/WebsocketClient.java +++ /dev/null @@ -1,58 +0,0 @@ -package telraam.station; - -import jakarta.websocket.*; - -import java.net.URI; - -@ClientEndpoint -public class WebsocketClient { - public interface MessageHandler { - void handleMessage(String message); - } - public interface OnOpenHandler { - void handleMsgOpen(); - } - - Session session = null; - private MessageHandler messageHandler; - private OnOpenHandler onOpenHandler; - - public WebsocketClient(URI endpointURI) { - try { - WebSocketContainer container = ContainerProvider.getWebSocketContainer(); - container.connectToServer(this, endpointURI); - } catch (Exception e) { - throw new RuntimeException(e); - } - } - - @OnOpen - public void onOpen(Session session) { - this.session = session; - } - - @OnClose - public void onClose(Session userSession, CloseReason reason) { - System.out.println("closing websocket"); - this.session = null; - } - - @OnMessage - public void onMessage(String message) { - if (this.messageHandler != null) { - this.messageHandler.handleMessage(message); - } - } - - public void addOnOpenHandler(OnOpenHandler openHandler) { - this.onOpenHandler = openHandler; - } - - public void addMessageHandler(MessageHandler msgHandler) { - this.messageHandler = msgHandler; - } - - public void sendMessage(String message) { - this.session.getAsyncRemote().sendText(message); - } -} diff --git a/src/main/java/telraam/station/websocket/WebsocketClient.java b/src/main/java/telraam/station/websocket/WebsocketClient.java index 25b7bb4..a1ea217 100644 --- a/src/main/java/telraam/station/websocket/WebsocketClient.java +++ b/src/main/java/telraam/station/websocket/WebsocketClient.java @@ -69,7 +69,6 @@ public void addMessageHandler(MessageHandler msgHandler) { } public void sendMessage(String message) { - this.session.getAsyncRemote().sendText(message); } } diff --git a/src/main/java/telraam/station/websocket/WebsocketFetcher.java b/src/main/java/telraam/station/websocket/WebsocketFetcher.java index 285ad15..3d49016 100644 --- a/src/main/java/telraam/station/websocket/WebsocketFetcher.java +++ b/src/main/java/telraam/station/websocket/WebsocketFetcher.java @@ -90,7 +90,6 @@ public void fetch() { return; } - WebsocketClient websocketClient = new WebsocketClient(url); websocketClient.addOnOpenHandler(() -> { websocketClient.sendMessage(wsMessageEncoded); From 607d73cf50ebd8eeabe5eab8d278bf142b0494bf Mon Sep 17 00:00:00 2001 From: NuttyShrimp Date: Tue, 2 Apr 2024 01:16:31 +0200 Subject: [PATCH 04/25] fix(ws-fetcher): catch client creation error --- .../station/websocket/WebsocketFetcher.java | 18 +++++++++++++++++- 1 file changed, 17 insertions(+), 1 deletion(-) diff --git a/src/main/java/telraam/station/websocket/WebsocketFetcher.java b/src/main/java/telraam/station/websocket/WebsocketFetcher.java index 3d49016..b7eaf28 100644 --- a/src/main/java/telraam/station/websocket/WebsocketFetcher.java +++ b/src/main/java/telraam/station/websocket/WebsocketFetcher.java @@ -90,7 +90,23 @@ public void fetch() { return; } - WebsocketClient websocketClient = new WebsocketClient(url); + + WebsocketClient websocketClient; + + try { + websocketClient = new WebsocketClient(url); + } catch (RuntimeException ex) { + this.logger.severe(ex.getMessage()); + try { + Thread.sleep(Fetcher.ERROR_TIMEOUT_MS); + } catch (InterruptedException e) { + logger.severe(e.getMessage()); + } + this.fetch(); + return; + } + + websocketClient.addOnOpenHandler(() -> { websocketClient.sendMessage(wsMessageEncoded); }); From de43e915d6e13afc5f1b72bafd25ce0b7140fda8 Mon Sep 17 00:00:00 2001 From: NuttyShrimp Date: Tue, 2 Apr 2024 18:20:33 +0200 Subject: [PATCH 05/25] feat(ws-fetcher): open WS after setting handlers --- .../station/websocket/WebsocketClient.java | 1 + .../station/websocket/WebsocketFetcher.java | 18 ++++++------------ 2 files changed, 7 insertions(+), 12 deletions(-) diff --git a/src/main/java/telraam/station/websocket/WebsocketClient.java b/src/main/java/telraam/station/websocket/WebsocketClient.java index a1ea217..25b7bb4 100644 --- a/src/main/java/telraam/station/websocket/WebsocketClient.java +++ b/src/main/java/telraam/station/websocket/WebsocketClient.java @@ -69,6 +69,7 @@ public void addMessageHandler(MessageHandler msgHandler) { } public void sendMessage(String message) { + this.session.getAsyncRemote().sendText(message); } } diff --git a/src/main/java/telraam/station/websocket/WebsocketFetcher.java b/src/main/java/telraam/station/websocket/WebsocketFetcher.java index b7eaf28..24c78b4 100644 --- a/src/main/java/telraam/station/websocket/WebsocketFetcher.java +++ b/src/main/java/telraam/station/websocket/WebsocketFetcher.java @@ -91,24 +91,18 @@ public void fetch() { } - WebsocketClient websocketClient; - - try { - websocketClient = new WebsocketClient(url); - } catch (RuntimeException ex) { - this.logger.severe(ex.getMessage()); + WebsocketClient websocketClient = new WebsocketClient(url); + websocketClient.addOnOpenHandler(() -> { + websocketClient.sendMessage(wsMessageEncoded); + }); + websocketClient.addOnCloseHandler(() -> { + this.logger.severe(String.format("Websocket for station %s got closed", station.getName())); try { Thread.sleep(Fetcher.ERROR_TIMEOUT_MS); } catch (InterruptedException e) { logger.severe(e.getMessage()); } this.fetch(); - return; - } - - - websocketClient.addOnOpenHandler(() -> { - websocketClient.sendMessage(wsMessageEncoded); }); websocketClient.addOnCloseHandler(() -> { this.logger.severe(String.format("Websocket for station %s got closed", station.getName())); From e8245c3452200cc7c936bec727038c6628dc34c4 Mon Sep 17 00:00:00 2001 From: NuttyShrimp Date: Tue, 2 Apr 2024 18:21:21 +0200 Subject: [PATCH 06/25] feat(ws-fetcher): retrieve missing values for detections from DB & try to fix simplePositioner cursedness with threading --- .../positioner/simple/SimplePositioner.java | 103 ------------------ 1 file changed, 103 deletions(-) delete mode 100644 src/main/java/telraam/logic/positioner/simple/SimplePositioner.java diff --git a/src/main/java/telraam/logic/positioner/simple/SimplePositioner.java b/src/main/java/telraam/logic/positioner/simple/SimplePositioner.java deleted file mode 100644 index 9958d1e..0000000 --- a/src/main/java/telraam/logic/positioner/simple/SimplePositioner.java +++ /dev/null @@ -1,103 +0,0 @@ -package telraam.logic.positioner.simple; - -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.CircularQueue; -import telraam.logic.positioner.Position; -import telraam.logic.positioner.PositionSender; -import telraam.logic.positioner.Positioner; - -import java.util.*; -import java.util.concurrent.Executors; -import java.util.concurrent.ScheduledExecutorService; -import java.util.concurrent.TimeUnit; -import java.util.concurrent.locks.Lock; -import java.util.concurrent.locks.ReentrantLock; -import java.util.logging.Level; -import java.util.logging.Logger; - -public class SimplePositioner implements Positioner { - private static final Logger logger = Logger.getLogger(SimplePositioner.class.getName()); - private final int QUEUE_SIZE = 50; - private final int MIN_RSSI = -85; - private final int DEBOUNCE_TIMEOUT = 1; - private boolean debounceScheduled; - private final ScheduledExecutorService scheduler; - private final PositionSender positionSender; - private final Map batonIdToTeam; - private final Map> teamDetections; - private final List stations; - private final Map teamPositions; - - public SimplePositioner(Jdbi jdbi) { - this.debounceScheduled = false; - this.scheduler = Executors.newScheduledThreadPool(1); - this.positionSender = new PositionSender(); - this.batonIdToTeam = new HashMap<>(); - this.teamDetections = new HashMap<>(); - this.teamPositions = new HashMap<>(); - - TeamDAO teamDAO = jdbi.onDemand(TeamDAO.class); - List teams = teamDAO.getAll(); - for (Team team : teams) { - teamDetections.put(team.getId(), new CircularQueue<>(QUEUE_SIZE)); - teamPositions.put(team.getId(), new Position(team.getId())); - } - List switchovers = jdbi.onDemand(BatonSwitchoverDAO.class).getAll(); - switchovers.sort(Comparator.comparing(BatonSwitchover::getTimestamp)); - - for (BatonSwitchover switchover : switchovers) { - batonIdToTeam.put(switchover.getNewBatonId(), teamDAO.getById(switchover.getTeamId()).get()); - } - - List stationList = jdbi.onDemand(StationDAO.class).getAll(); - stationList.sort(Comparator.comparing(Station::getDistanceFromStart)); - stations = stationList.stream().map(Station::getId).toList(); - } - - public void calculatePositions() { - logger.info("SimplePositioner: Calculating positions..."); - for (Map.Entry> entry : teamDetections.entrySet()) { - List detections = teamDetections.get(entry.getKey()); - detections.sort(Comparator.comparing(Detection::getTimestamp)); - - int currentStationRssi = MIN_RSSI; - int currentStationPosition = 0; - for (Detection detection : detections) { - if (detection.getRssi() > currentStationRssi) { - currentStationRssi = detection.getRssi(); - currentStationPosition = detection.getStationId(); - } - } - - float progress = ((float) 100 / stations.size()) * currentStationPosition; - teamPositions.get(entry.getKey()).setProgress(progress); - } - - positionSender.send(teamPositions.values().stream().toList()); - logger.info("SimplePositioner: Done calculating positions"); - } - - public synchronized void handle(Detection detection) { - Team team = batonIdToTeam.get(detection.getBatonId()); - teamDetections.get(team.getId()).add(detection); - - if (!debounceScheduled) { - debounceScheduled = true; - scheduler.schedule(() -> { - try { - calculatePositions(); - } catch (Exception e) { - logger.log(Level.SEVERE, e.getMessage(), e); - } - debounceScheduled = false; - }, DEBOUNCE_TIMEOUT, TimeUnit.SECONDS); - } - } -} From 8bcdffa5b0dac07277b00ae7fcb9e274a233fdd6 Mon Sep 17 00:00:00 2001 From: NuttyShrimp Date: Sat, 13 Apr 2024 14:40:53 +0200 Subject: [PATCH 07/25] fix(simplePositioner): make handle function synchronised --- .../positioner/simple/SimplePositioner.java | 103 ++++++++++++++++++ 1 file changed, 103 insertions(+) create mode 100644 src/main/java/telraam/logic/positioner/simple/SimplePositioner.java diff --git a/src/main/java/telraam/logic/positioner/simple/SimplePositioner.java b/src/main/java/telraam/logic/positioner/simple/SimplePositioner.java new file mode 100644 index 0000000..9958d1e --- /dev/null +++ b/src/main/java/telraam/logic/positioner/simple/SimplePositioner.java @@ -0,0 +1,103 @@ +package telraam.logic.positioner.simple; + +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.CircularQueue; +import telraam.logic.positioner.Position; +import telraam.logic.positioner.PositionSender; +import telraam.logic.positioner.Positioner; + +import java.util.*; +import java.util.concurrent.Executors; +import java.util.concurrent.ScheduledExecutorService; +import java.util.concurrent.TimeUnit; +import java.util.concurrent.locks.Lock; +import java.util.concurrent.locks.ReentrantLock; +import java.util.logging.Level; +import java.util.logging.Logger; + +public class SimplePositioner implements Positioner { + private static final Logger logger = Logger.getLogger(SimplePositioner.class.getName()); + private final int QUEUE_SIZE = 50; + private final int MIN_RSSI = -85; + private final int DEBOUNCE_TIMEOUT = 1; + private boolean debounceScheduled; + private final ScheduledExecutorService scheduler; + private final PositionSender positionSender; + private final Map batonIdToTeam; + private final Map> teamDetections; + private final List stations; + private final Map teamPositions; + + public SimplePositioner(Jdbi jdbi) { + this.debounceScheduled = false; + this.scheduler = Executors.newScheduledThreadPool(1); + this.positionSender = new PositionSender(); + this.batonIdToTeam = new HashMap<>(); + this.teamDetections = new HashMap<>(); + this.teamPositions = new HashMap<>(); + + TeamDAO teamDAO = jdbi.onDemand(TeamDAO.class); + List teams = teamDAO.getAll(); + for (Team team : teams) { + teamDetections.put(team.getId(), new CircularQueue<>(QUEUE_SIZE)); + teamPositions.put(team.getId(), new Position(team.getId())); + } + List switchovers = jdbi.onDemand(BatonSwitchoverDAO.class).getAll(); + switchovers.sort(Comparator.comparing(BatonSwitchover::getTimestamp)); + + for (BatonSwitchover switchover : switchovers) { + batonIdToTeam.put(switchover.getNewBatonId(), teamDAO.getById(switchover.getTeamId()).get()); + } + + List stationList = jdbi.onDemand(StationDAO.class).getAll(); + stationList.sort(Comparator.comparing(Station::getDistanceFromStart)); + stations = stationList.stream().map(Station::getId).toList(); + } + + public void calculatePositions() { + logger.info("SimplePositioner: Calculating positions..."); + for (Map.Entry> entry : teamDetections.entrySet()) { + List detections = teamDetections.get(entry.getKey()); + detections.sort(Comparator.comparing(Detection::getTimestamp)); + + int currentStationRssi = MIN_RSSI; + int currentStationPosition = 0; + for (Detection detection : detections) { + if (detection.getRssi() > currentStationRssi) { + currentStationRssi = detection.getRssi(); + currentStationPosition = detection.getStationId(); + } + } + + float progress = ((float) 100 / stations.size()) * currentStationPosition; + teamPositions.get(entry.getKey()).setProgress(progress); + } + + positionSender.send(teamPositions.values().stream().toList()); + logger.info("SimplePositioner: Done calculating positions"); + } + + public synchronized void handle(Detection detection) { + Team team = batonIdToTeam.get(detection.getBatonId()); + teamDetections.get(team.getId()).add(detection); + + if (!debounceScheduled) { + debounceScheduled = true; + scheduler.schedule(() -> { + try { + calculatePositions(); + } catch (Exception e) { + logger.log(Level.SEVERE, e.getMessage(), e); + } + debounceScheduled = false; + }, DEBOUNCE_TIMEOUT, TimeUnit.SECONDS); + } + } +} From 6b44e356c2a40d1ae5ed9451431e79aaba8bfc27 Mon Sep 17 00:00:00 2001 From: Topvennie Date: Mon, 1 Apr 2024 13:27:26 +0200 Subject: [PATCH 08/25] tmp --- .../nostradamus/CircularPriorityQueue.java | 28 +++ .../positioner/nostradamus/Nostradamus.java | 171 ++++++++++++++++++ .../positioner/nostradamus/TeamData.java | 23 +++ .../{ => simple}/CircularQueue.java | 2 +- .../positioner/simple/SimplePositioner.java | 3 +- .../telraam/websocket/WebSocketMessage.java | 3 +- 6 files changed, 226 insertions(+), 4 deletions(-) create mode 100644 src/main/java/telraam/logic/positioner/nostradamus/CircularPriorityQueue.java create mode 100644 src/main/java/telraam/logic/positioner/nostradamus/Nostradamus.java create mode 100644 src/main/java/telraam/logic/positioner/nostradamus/TeamData.java rename src/main/java/telraam/logic/positioner/{ => simple}/CircularQueue.java (89%) diff --git a/src/main/java/telraam/logic/positioner/nostradamus/CircularPriorityQueue.java b/src/main/java/telraam/logic/positioner/nostradamus/CircularPriorityQueue.java new file mode 100644 index 0000000..3ca788d --- /dev/null +++ b/src/main/java/telraam/logic/positioner/nostradamus/CircularPriorityQueue.java @@ -0,0 +1,28 @@ +package telraam.logic.positioner.nostradamus; + +import telraam.database.models.Detection; + +import java.util.Comparator; +import java.util.PriorityQueue; + +public class CircularPriorityQueue extends PriorityQueue { + private final int maxSize; + + // TODO: Check if in the right order. Oldest timestamp (the earliest) should be first + public CircularPriorityQueue(int maxSize) { + super(Comparator.comparing(Detection::getTimestamp)); + + this.maxSize = maxSize; + } + + @Override + public boolean add(Detection e) { + if (size() >= this.maxSize) { + if (e.getTimestamp().after(peek().getTimestamp())) { + remove(); + } + } + + return super.add(e); + } +} diff --git a/src/main/java/telraam/logic/positioner/nostradamus/Nostradamus.java b/src/main/java/telraam/logic/positioner/nostradamus/Nostradamus.java new file mode 100644 index 0000000..d2dfcb8 --- /dev/null +++ b/src/main/java/telraam/logic/positioner/nostradamus/Nostradamus.java @@ -0,0 +1,171 @@ +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.Executors; +import java.util.concurrent.ScheduledExecutorService; +import java.util.concurrent.TimeUnit; +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_FETCH = 10000; + private final int DEBOUNCE_TIMEOUT = 1; + private final int MAX_SIZE = 10000; + private final int MIN_RSSI = -85; + private final int INTERVAL = 2; + private final ScheduledExecutorService scheduler; + private boolean debounceScheduled; + private Lock lock; + + private final Jdbi jdbi; + private final TeamDAO teamDAO; + private final StationDAO stationDAO; + private final BatonSwitchoverDAO batonSwitchoverDAO; + private final PositionSender positionSender; + private Map teamDetections; + private Map teamData; + private Map batonIdToTeam; + private Map idToStation; + + public Nostradamus(Jdbi jdbi) { + this.scheduler = Executors.newScheduledThreadPool(1); + this.debounceScheduled = false; + this.lock = new ReentrantLock(); + + this.jdbi = jdbi; + this.teamDAO = jdbi.onDemand(TeamDAO.class); + this.stationDAO = jdbi.onDemand(StationDAO.class); + this.batonSwitchoverDAO = jdbi.onDemand(BatonSwitchoverDAO.class); + + this.positionSender = new PositionSender(); + + new Thread(this::fetch); + } + + // Update variables that depend on teams, stations and / or batonswitchover + private void fetch() { + while (true) { + List teams = teamDAO.getAll(); + List switchovers = batonSwitchoverDAO.getAll(); + List stations = stationDAO.getAll(); + + lock.lock(); + + teamDetections = teams.stream() + .collect( + Collectors.toMap( + team -> team, + team -> teamDetections.getOrDefault(team, new CircularPriorityQueue(MAX_SIZE)) + ) + ); + teamData = teams.stream() + .collect( + Collectors.toMap( + team -> team, + team -> teamData.getOrDefault(team, new TeamData(team.getId())) + ) + ); + batonIdToTeam = switchovers.stream() + .collect( + Collectors.toMap( + BatonSwitchover::getNewBatonId, + switchover -> teamDAO.getById(switchover.getTeamId()).get() + ) + ); + idToStation = stations.stream() + .collect( + Collectors.toMap( + Station::getId, + station -> station + ) + ); + + lock.unlock(); + + try { + Thread.sleep(INTERVAL_FETCH); + } catch (InterruptedException e) { + logger.severe(e.getMessage()); + } + } + } + + // TODO: Add more detection filtering, high enough rssi, only one detection / timestamp, ... + // TODO: Calculate average times in separate thread + // TODO: If multiple detections come out of order -> restart + // TODO: If something in fetch changes -> restart + + // TODO: Start simple, if arrives at new station -> send location and average time. Else send location given speed + private void calculatePositions() { + logger.info("Nostradamus: Calculating positions..."); + + for (Map.Entry entry: teamDetections.entrySet()) { + Map averageTimes = new HashMap<>(); + + int lastStationid = -1; + long currentStationTime = 0; + int currentStationRssi = MIN_RSSI; + int currentStationId = 0; + + for (Detection detection: entry.getValue()) { + if (detection.getTimestamp().getTime() - currentStationTime < INTERVAL) { + // Same interval + // Keep station with the highest RSSI + if (detection.getRssi() > currentStationRssi) { + currentStationId = detection.getStationId(); + currentStationRssi = detection.getRssi(); + } + } else { + // New interval + // Save old station id + lastStationid = currentStationId; + currentStationTime = detection.getTimestamp().getTime(); + currentStationRssi = detection.getRssi(); + currentStationId = detection.getStationId(); + } + } + + // Keep result of last interval if it exists + Station currentStation = idToStation.getOrDefault(lastStationid, idToStation.get(currentStationId)); + + + } + + positionSender.send(teamData.values().stream().map(TeamData::getPosition).toList()); + logger.info("Nostradamus: Done calculating positions"); + } + + @Override + public void handle(Detection detection) { + Team team = batonIdToTeam.get(detection.getBatonId()); + teamDetections.get(team).add(detection); + + if (! debounceScheduled) { + debounceScheduled = true; + scheduler.schedule(() -> { + try { + calculatePositions(); + } catch (Exception e) { + logger.severe(e.getMessage()); + } + debounceScheduled = false; + }, DEBOUNCE_TIMEOUT, TimeUnit.SECONDS); + } + } +} diff --git a/src/main/java/telraam/logic/positioner/nostradamus/TeamData.java b/src/main/java/telraam/logic/positioner/nostradamus/TeamData.java new file mode 100644 index 0000000..cdb2eff --- /dev/null +++ b/src/main/java/telraam/logic/positioner/nostradamus/TeamData.java @@ -0,0 +1,23 @@ +package telraam.logic.positioner.nostradamus; + +import lombok.Getter; +import lombok.Setter; +import telraam.database.models.Station; +import telraam.logic.positioner.Position; + +import java.sql.Timestamp; +import java.util.HashMap; +import java.util.Map; + +@Getter @Setter +public class TeamData { + private Position position; + private Station lastStation; + private Timestamp lastUpdate; + + public TeamData(int teamId) { + this.position = new Position(teamId); + this.lastStation = null; + this.lastUpdate = null; + } +} diff --git a/src/main/java/telraam/logic/positioner/CircularQueue.java b/src/main/java/telraam/logic/positioner/simple/CircularQueue.java similarity index 89% rename from src/main/java/telraam/logic/positioner/CircularQueue.java rename to src/main/java/telraam/logic/positioner/simple/CircularQueue.java index 773c633..ae101ed 100644 --- a/src/main/java/telraam/logic/positioner/CircularQueue.java +++ b/src/main/java/telraam/logic/positioner/simple/CircularQueue.java @@ -1,4 +1,4 @@ -package telraam.logic.positioner; +package telraam.logic.positioner.simple; import java.util.LinkedList; diff --git a/src/main/java/telraam/logic/positioner/simple/SimplePositioner.java b/src/main/java/telraam/logic/positioner/simple/SimplePositioner.java index 9958d1e..fa6a395 100644 --- a/src/main/java/telraam/logic/positioner/simple/SimplePositioner.java +++ b/src/main/java/telraam/logic/positioner/simple/SimplePositioner.java @@ -8,7 +8,6 @@ import telraam.database.models.Detection; import telraam.database.models.Station; import telraam.database.models.Team; -import telraam.logic.positioner.CircularQueue; import telraam.logic.positioner.Position; import telraam.logic.positioner.PositionSender; import telraam.logic.positioner.Positioner; @@ -61,7 +60,7 @@ public SimplePositioner(Jdbi jdbi) { stations = stationList.stream().map(Station::getId).toList(); } - public void calculatePositions() { + private void calculatePositions() { logger.info("SimplePositioner: Calculating positions..."); for (Map.Entry> entry : teamDetections.entrySet()) { List detections = teamDetections.get(entry.getKey()); diff --git a/src/main/java/telraam/websocket/WebSocketMessage.java b/src/main/java/telraam/websocket/WebSocketMessage.java index 0b4200f..b280ec2 100644 --- a/src/main/java/telraam/websocket/WebSocketMessage.java +++ b/src/main/java/telraam/websocket/WebSocketMessage.java @@ -1,9 +1,10 @@ package telraam.websocket; import lombok.Getter; +import lombok.NoArgsConstructor; import lombok.Setter; -@Getter @Setter +@Getter @Setter @NoArgsConstructor public class WebSocketMessage { private String topic; private T data; From 91811a70abcd629277b9bf3d88792a3d73e19080 Mon Sep 17 00:00:00 2001 From: Topvennie Date: Mon, 8 Apr 2024 17:45:21 +0200 Subject: [PATCH 09/25] refactor --- .../telraam/database/models/Detection.java | 6 + .../nostradamus/CircularPriorityQueue.java | 28 --- .../positioner/nostradamus/CircularQueue.java | 21 +++ .../positioner/nostradamus/Nostradamus.java | 171 ++++-------------- .../positioner/nostradamus/PositionList.java | 55 ++++++ .../positioner/nostradamus/TeamData.java | 52 +++++- 6 files changed, 165 insertions(+), 168 deletions(-) delete mode 100644 src/main/java/telraam/logic/positioner/nostradamus/CircularPriorityQueue.java create mode 100644 src/main/java/telraam/logic/positioner/nostradamus/CircularQueue.java create mode 100644 src/main/java/telraam/logic/positioner/nostradamus/PositionList.java diff --git a/src/main/java/telraam/database/models/Detection.java b/src/main/java/telraam/database/models/Detection.java index 5cd050b..4950feb 100644 --- a/src/main/java/telraam/database/models/Detection.java +++ b/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; + } } diff --git a/src/main/java/telraam/logic/positioner/nostradamus/CircularPriorityQueue.java b/src/main/java/telraam/logic/positioner/nostradamus/CircularPriorityQueue.java deleted file mode 100644 index 3ca788d..0000000 --- a/src/main/java/telraam/logic/positioner/nostradamus/CircularPriorityQueue.java +++ /dev/null @@ -1,28 +0,0 @@ -package telraam.logic.positioner.nostradamus; - -import telraam.database.models.Detection; - -import java.util.Comparator; -import java.util.PriorityQueue; - -public class CircularPriorityQueue extends PriorityQueue { - private final int maxSize; - - // TODO: Check if in the right order. Oldest timestamp (the earliest) should be first - public CircularPriorityQueue(int maxSize) { - super(Comparator.comparing(Detection::getTimestamp)); - - this.maxSize = maxSize; - } - - @Override - public boolean add(Detection e) { - if (size() >= this.maxSize) { - if (e.getTimestamp().after(peek().getTimestamp())) { - remove(); - } - } - - return super.add(e); - } -} diff --git a/src/main/java/telraam/logic/positioner/nostradamus/CircularQueue.java b/src/main/java/telraam/logic/positioner/nostradamus/CircularQueue.java new file mode 100644 index 0000000..997e9bf --- /dev/null +++ b/src/main/java/telraam/logic/positioner/nostradamus/CircularQueue.java @@ -0,0 +1,21 @@ +package telraam.logic.positioner.nostradamus; + +import java.util.LinkedList; + +public class CircularQueue extends LinkedList { + + private final int maxSize; + public CircularQueue(int maxSize) { + this.maxSize = maxSize; + } + + @Override + public boolean add(T e) { + if (size() >= this.maxSize) { + removeFirst(); + } + + return super.add(e); + } + +} diff --git a/src/main/java/telraam/logic/positioner/nostradamus/Nostradamus.java b/src/main/java/telraam/logic/positioner/nostradamus/Nostradamus.java index d2dfcb8..c4524bc 100644 --- a/src/main/java/telraam/logic/positioner/nostradamus/Nostradamus.java +++ b/src/main/java/telraam/logic/positioner/nostradamus/Nostradamus.java @@ -1,171 +1,78 @@ 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.Executors; -import java.util.concurrent.ScheduledExecutorService; -import java.util.concurrent.TimeUnit; 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_FETCH = 10000; - private final int DEBOUNCE_TIMEOUT = 1; - private final int MAX_SIZE = 10000; - private final int MIN_RSSI = -85; - private final int INTERVAL = 2; - private final ScheduledExecutorService scheduler; - private boolean debounceScheduled; - private Lock lock; - - private final Jdbi jdbi; - private final TeamDAO teamDAO; - private final StationDAO stationDAO; - private final BatonSwitchoverDAO batonSwitchoverDAO; + private final int INTERVAL_CALCULATE = 500; // How often to handle new detections + private final int INTERVAL_FETCH = 30000; // Interval between fetching all stations, teams, ... + private final int INTERVAL_DETECTIONS = 1; // Amount of seconds to group detections by + private final int AVERAGE_AMOUNT = 10; // Calculate the average running speed the last x intervals + private final int MIN_RSSI = -84; + private final List newDetections; // Contains not yet handled detections + private final Lock detectionLock; + private Map batonToTeam; // Baton ID to Team + private Map teamData; // All team data private final PositionSender positionSender; - private Map teamDetections; - private Map teamData; - private Map batonIdToTeam; - private Map idToStation; public Nostradamus(Jdbi jdbi) { - this.scheduler = Executors.newScheduledThreadPool(1); - this.debounceScheduled = false; - this.lock = new ReentrantLock(); - - this.jdbi = jdbi; - this.teamDAO = jdbi.onDemand(TeamDAO.class); - this.stationDAO = jdbi.onDemand(StationDAO.class); - this.batonSwitchoverDAO = jdbi.onDemand(BatonSwitchoverDAO.class); + this.newDetections = new ArrayList<>(); + this.detectionLock = new ReentrantLock(); // TODO: Right kind of lock? this.positionSender = new PositionSender(); - new Thread(this::fetch); + new Thread(this::calculatePosition); } - // Update variables that depend on teams, stations and / or batonswitchover - private void fetch() { + private void calculatePosition() { + Set changedTeams = new HashSet<>(); // List of teams that have changed station while (true) { - List teams = teamDAO.getAll(); - List switchovers = batonSwitchoverDAO.getAll(); - List stations = stationDAO.getAll(); - - lock.lock(); + changedTeams.clear(); + detectionLock.lock(); + for (Detection detection: newDetections) { + if (batonToTeam.containsKey(detection.getBatonId())) { + Team team = batonToTeam.get(detection.getBatonId()); + if (teamData.containsKey(team) && teamData.get(team).addDetection(detection)) { + changedTeams.add(team); + } + } + } + detectionLock.unlock(); // Use lock as short as possible - teamDetections = teams.stream() - .collect( - Collectors.toMap( - team -> team, - team -> teamDetections.getOrDefault(team, new CircularPriorityQueue(MAX_SIZE)) - ) - ); - teamData = teams.stream() - .collect( - Collectors.toMap( - team -> team, - team -> teamData.getOrDefault(team, new TeamData(team.getId())) - ) - ); - batonIdToTeam = switchovers.stream() - .collect( - Collectors.toMap( - BatonSwitchover::getNewBatonId, - switchover -> teamDAO.getById(switchover.getTeamId()).get() - ) - ); - idToStation = stations.stream() - .collect( - Collectors.toMap( - Station::getId, - station -> station - ) - ); + if (!changedTeams.isEmpty()) { + // Update + for (Team team: changedTeams) { + teamData.get(team).updatePosition(); + } - lock.unlock(); + positionSender.send( + changedTeams.stream().map(team -> teamData.get(team).getPosition()).toList() + ); + } try { - Thread.sleep(INTERVAL_FETCH); + Thread.sleep(INTERVAL_CALCULATE); } catch (InterruptedException e) { logger.severe(e.getMessage()); } } } - // TODO: Add more detection filtering, high enough rssi, only one detection / timestamp, ... - // TODO: Calculate average times in separate thread - // TODO: If multiple detections come out of order -> restart - // TODO: If something in fetch changes -> restart - - // TODO: Start simple, if arrives at new station -> send location and average time. Else send location given speed - private void calculatePositions() { - logger.info("Nostradamus: Calculating positions..."); - - for (Map.Entry entry: teamDetections.entrySet()) { - Map averageTimes = new HashMap<>(); - - int lastStationid = -1; - long currentStationTime = 0; - int currentStationRssi = MIN_RSSI; - int currentStationId = 0; - - for (Detection detection: entry.getValue()) { - if (detection.getTimestamp().getTime() - currentStationTime < INTERVAL) { - // Same interval - // Keep station with the highest RSSI - if (detection.getRssi() > currentStationRssi) { - currentStationId = detection.getStationId(); - currentStationRssi = detection.getRssi(); - } - } else { - // New interval - // Save old station id - lastStationid = currentStationId; - currentStationTime = detection.getTimestamp().getTime(); - currentStationRssi = detection.getRssi(); - currentStationId = detection.getStationId(); - } - } - - // Keep result of last interval if it exists - Station currentStation = idToStation.getOrDefault(lastStationid, idToStation.get(currentStationId)); - - - } - - positionSender.send(teamData.values().stream().map(TeamData::getPosition).toList()); - logger.info("Nostradamus: Done calculating positions"); - } - @Override public void handle(Detection detection) { - Team team = batonIdToTeam.get(detection.getBatonId()); - teamDetections.get(team).add(detection); - - if (! debounceScheduled) { - debounceScheduled = true; - scheduler.schedule(() -> { - try { - calculatePositions(); - } catch (Exception e) { - logger.severe(e.getMessage()); - } - debounceScheduled = false; - }, DEBOUNCE_TIMEOUT, TimeUnit.SECONDS); + if (detection.getRssi() > MIN_RSSI) { + detectionLock.lock(); + newDetections.add(detection); + detectionLock.unlock(); } } } diff --git a/src/main/java/telraam/logic/positioner/nostradamus/PositionList.java b/src/main/java/telraam/logic/positioner/nostradamus/PositionList.java new file mode 100644 index 0000000..e10540d --- /dev/null +++ b/src/main/java/telraam/logic/positioner/nostradamus/PositionList.java @@ -0,0 +1,55 @@ +package telraam.logic.positioner.nostradamus; + +import lombok.Getter; +import telraam.database.models.Detection; + +import java.sql.Timestamp; +import java.util.ArrayList; +import java.util.Comparator; + +public class PositionList extends ArrayList { + + private final int interval; + @Getter + private Detection currentPosition; + private Timestamp newestDetection; + + public PositionList(int interval) { + this.interval = interval; + this.currentPosition = new Detection(-1, 0, -100); + this.newestDetection = new Timestamp(0); + } + + /** + * @param e element to add + * @return True if the current position has changed + */ + @Override + public boolean add(Detection e) { + super.add(e); + + if (e.getTimestamp().after(newestDetection)) { + newestDetection = e.getTimestamp(); + } + + if (!e.getStationId().equals(currentPosition.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 inInterval(Timestamp oldest, Timestamp newest) { + return newest.getNanos() - oldest.getNanos() > interval; + } +} diff --git a/src/main/java/telraam/logic/positioner/nostradamus/TeamData.java b/src/main/java/telraam/logic/positioner/nostradamus/TeamData.java index cdb2eff..39547d2 100644 --- a/src/main/java/telraam/logic/positioner/nostradamus/TeamData.java +++ b/src/main/java/telraam/logic/positioner/nostradamus/TeamData.java @@ -1,23 +1,59 @@ package telraam.logic.positioner.nostradamus; import lombok.Getter; -import lombok.Setter; +import telraam.database.models.Detection; import telraam.database.models.Station; import telraam.logic.positioner.Position; -import java.sql.Timestamp; import java.util.HashMap; +import java.util.List; import java.util.Map; -@Getter @Setter public class TeamData { + private PositionList detections; + private List stations; // Station list, ordered by distance from start + private Map> averageTimes; // List of average times for each station. Going from station 2 to 3 is saved in 3. + private long previousStationArrival; // Arrival time of previous station. Used to calculate the average times + @Getter private Position position; - private Station lastStation; - private Timestamp lastUpdate; + private Station currentStation; + private int totalDistance; - public TeamData(int teamId) { + + public TeamData(int teamId, int interval, List stations, int averageAmount) { + this.detections = new PositionList(interval); + this.stations = stations; + + averageTimes = new HashMap<>(); + for (Station station: stations) { + averageTimes.put(station.getId(), new CircularQueue<>(averageAmount)); + } + + this.previousStationArrival = System.currentTimeMillis(); this.position = new Position(teamId); - this.lastStation = null; - this.lastUpdate = null; + } + + public boolean addDetection(Detection e) { + boolean newStation = detections.add(e); + Station previousStation = detections.getCurrentPosition().getStationId() + + // TODO: If station missed big problem + if (newStation && averageTimes.containsKey(detections.getCurrentPosition())) { + long now = System.currentTimeMillis(); + averageTimes.get(detections.getCurrentPosition()).add(now - previousStationArrival); + previousStationArrival = now; + } + + return newStation; + } + + // TODO: Requires the last station to be on the finish. Convert to variable + public void updatePosition() { + float progress = (float) (currentStation.getDistanceFromStart() / totalDistance); + Station nextStation = stations.get(stations.indexOf(currentStation) + 1 % stations.size()); + + + + position.setProgress(progress); } } From dd4ff537188830a0d9bdeb79c7e2923d752662be Mon Sep 17 00:00:00 2001 From: Topvennie Date: Mon, 8 Apr 2024 23:09:03 +0200 Subject: [PATCH 10/25] nostradamus basic logic --- .../java/telraam/database/models/Station.java | 4 ++ .../telraam/logic/positioner/Position.java | 11 ++-- .../positioner/nostradamus/Nostradamus.java | 63 +++++++++++++++++-- .../positioner/nostradamus/TeamData.java | 59 ++++++++++------- 4 files changed, 105 insertions(+), 32 deletions(-) diff --git a/src/main/java/telraam/database/models/Station.java b/src/main/java/telraam/database/models/Station.java index 442e412..b1f1220 100644 --- a/src/main/java/telraam/database/models/Station.java +++ b/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; + } } diff --git a/src/main/java/telraam/logic/positioner/Position.java b/src/main/java/telraam/logic/positioner/Position.java index 3a4218e..300837f 100644 --- a/src/main/java/telraam/logic/positioner/Position.java +++ b/src/main/java/telraam/logic/positioner/Position.java @@ -2,18 +2,21 @@ import lombok.Getter; import lombok.Setter; -import telraam.database.models.Team; -import telraam.websocket.WebSocketMessageSingleton; @Getter @Setter public class Position { private int teamId; - private float progress; // Progress of the lap. Between 0-1 - private float speed; // Current speed. Progress / second + private double progress; // Progress of the lap. Between 0-1 + private double speed; // Current speed. progress / second public Position(int teamId) { this.teamId = teamId; this.progress = 0; this.speed = 0; } + + public void update(double progress, double speed) { + this.progress = progress; + this.speed = speed; + } } diff --git a/src/main/java/telraam/logic/positioner/nostradamus/Nostradamus.java b/src/main/java/telraam/logic/positioner/nostradamus/Nostradamus.java index c4524bc..0d8e363 100644 --- a/src/main/java/telraam/logic/positioner/nostradamus/Nostradamus.java +++ b/src/main/java/telraam/logic/positioner/nostradamus/Nostradamus.java @@ -1,8 +1,11 @@ package telraam.logic.positioner.nostradamus; import org.jdbi.v3.core.Jdbi; -import telraam.database.models.Detection; -import telraam.database.models.Team; +import telraam.database.daos.BatonDAO; +import telraam.database.daos.BatonSwitchoverDAO; +import telraam.database.daos.StationDAO; +import telraam.database.daos.TeamDAO; +import telraam.database.models.*; import telraam.logic.positioner.PositionSender; import telraam.logic.positioner.Positioner; @@ -10,42 +13,88 @@ 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 = 500; // How often to handle new detections - private final int INTERVAL_FETCH = 30000; // Interval between fetching all stations, teams, ... + private final int INTERVAL_FETCH = 60000; // Interval between fetching all stations, teams, ... private final int INTERVAL_DETECTIONS = 1; // Amount of seconds to group detections by private final int AVERAGE_AMOUNT = 10; // Calculate the average running speed the last x intervals private final int MIN_RSSI = -84; + private final Jdbi jdbi; private final List newDetections; // Contains not yet handled detections private final Lock detectionLock; + private final Lock dataLock; private Map batonToTeam; // Baton ID to Team private Map teamData; // All team data private final PositionSender positionSender; public Nostradamus(Jdbi jdbi) { + this.jdbi = jdbi; this.newDetections = new ArrayList<>(); - this.detectionLock = new ReentrantLock(); // TODO: Right kind of lock? + this.detectionLock = new ReentrantLock(); + this.dataLock = new ReentrantLock(); + + // Will be filled by fetch + this.batonToTeam = new HashMap<>(); + setTeamData(); this.positionSender = new PositionSender(); + new Thread(this::fetch); new Thread(this::calculatePosition); } + private void setTeamData() { + List stations = jdbi.onDemand(StationDAO.class).getAll(); + List teams = jdbi.onDemand(TeamDAO.class).getAll(); + + teamData = teams.stream().collect(Collectors.toMap( + team -> team, + team -> new TeamData(team.getId(), INTERVAL_DETECTIONS, stations, AVERAGE_AMOUNT) + )); + } + + private void fetch() { + List switchovers = jdbi.onDemand(BatonSwitchoverDAO.class).getAll(); + List teams = jdbi.onDemand(TeamDAO.class).getAll(); + + Map teamIdToTeam = teams.stream().collect(Collectors.toMap(Team::getId, team -> team)); + Map batonToTeam = switchovers.stream().collect(Collectors.toMap( + BatonSwitchover::getNewBatonId, + switchover -> teamIdToTeam.get(switchover.getTeamId()) + )); + + if (!this.batonToTeam.equals(batonToTeam)) { + dataLock.lock(); + this.batonToTeam = batonToTeam; + dataLock.unlock(); + } + + // zzzzzzzz + try { + Thread.sleep(INTERVAL_FETCH); + } catch (InterruptedException e) { + logger.severe(e.getMessage()); + } + } + private void calculatePosition() { Set changedTeams = new HashSet<>(); // List of teams that have changed station while (true) { + dataLock.lock(); changedTeams.clear(); detectionLock.lock(); for (Detection detection: newDetections) { if (batonToTeam.containsKey(detection.getBatonId())) { Team team = batonToTeam.get(detection.getBatonId()); - if (teamData.containsKey(team) && teamData.get(team).addDetection(detection)) { + if (teamData.get(team).addDetection(detection)) { changedTeams.add(team); } } } + newDetections.clear(); detectionLock.unlock(); // Use lock as short as possible if (!changedTeams.isEmpty()) { @@ -54,11 +103,15 @@ private void calculatePosition() { teamData.get(team).updatePosition(); } + // Send new data to the websocket positionSender.send( changedTeams.stream().map(team -> teamData.get(team).getPosition()).toList() ); } + dataLock.unlock(); + + // zzzzzzzz try { Thread.sleep(INTERVAL_CALCULATE); } catch (InterruptedException e) { diff --git a/src/main/java/telraam/logic/positioner/nostradamus/TeamData.java b/src/main/java/telraam/logic/positioner/nostradamus/TeamData.java index 39547d2..9f3c0be 100644 --- a/src/main/java/telraam/logic/positioner/nostradamus/TeamData.java +++ b/src/main/java/telraam/logic/positioner/nostradamus/TeamData.java @@ -5,55 +5,68 @@ import telraam.database.models.Station; import telraam.logic.positioner.Position; +import java.util.Comparator; import java.util.HashMap; import java.util.List; import java.util.Map; +import java.util.stream.Collectors; public class TeamData { - private PositionList detections; - private List stations; // Station list, ordered by distance from start - private Map> averageTimes; // List of average times for each station. Going from station 2 to 3 is saved in 3. + private final PositionList detections; + private final List stations; // Station list, ordered by distance from start + private final Map idToStation; // Map going from a station id to the station + private final Map> averageTimes; // List of average times for each station. Going from station 2 to 3 is saved in 3. private long previousStationArrival; // Arrival time of previous station. Used to calculate the average times + private Station currentStation; // Current station location + private Station previousStation; // Previous station location + private final Double totalDistance; @Getter - private Position position; - private Station currentStation; - private int totalDistance; + private final Position position; public TeamData(int teamId, int interval, List stations, int averageAmount) { this.detections = new PositionList(interval); this.stations = stations; - - averageTimes = new HashMap<>(); - for (Station station: stations) { - averageTimes.put(station.getId(), new CircularQueue<>(averageAmount)); - } - + this.stations.sort(Comparator.comparing(Station::getDistanceFromStart)); + this.idToStation = stations.stream().collect(Collectors.toMap(Station::getId, station -> station)); + this.averageTimes = stations.stream().collect(Collectors.toMap(station -> station, station -> new CircularQueue<>(averageAmount))); this.previousStationArrival = System.currentTimeMillis(); + this.currentStation = new Station(-10); // Will never trigger `isNextStation` for the first station + this.totalDistance = this.stations.get(this.stations.size() - 1).getDistanceFromStart(); // Requires last station to be located at the start this.position = new Position(teamId); } public boolean addDetection(Detection e) { boolean newStation = detections.add(e); - Station previousStation = detections.getCurrentPosition().getStationId() - // TODO: If station missed big problem - if (newStation && averageTimes.containsKey(detections.getCurrentPosition())) { - long now = System.currentTimeMillis(); - averageTimes.get(detections.getCurrentPosition()).add(now - previousStationArrival); - previousStationArrival = now; + if (newStation) { + previousStation = currentStation; + currentStation = idToStation.get(e.getStationId()); + if (isNextStation()) { + // Only add the time if it goes forward by exactly one station + averageTimes.get(previousStation).add(System.currentTimeMillis() - previousStationArrival); + } + previousStationArrival = System.currentTimeMillis(); } return newStation; } - // TODO: Requires the last station to be on the finish. Convert to variable - public void updatePosition() { - float progress = (float) (currentStation.getDistanceFromStart() / totalDistance); - Station nextStation = stations.get(stations.indexOf(currentStation) + 1 % stations.size()); + private boolean isNextStation() { + return (stations.indexOf(currentStation) - stations.indexOf(previousStation) + stations.size()) % stations.size() == 1; + } + // TODO: Smoothen out + public void updatePosition() { + double progress = currentStation.getDistanceFromStart() / totalDistance; + Station nextStation = stations.get((stations.indexOf(currentStation) + 1) % stations.size()); + double distance = (nextStation.getDistanceFromStart() - currentStation.getDistanceFromStart() + totalDistance) % totalDistance; + double speed = ((distance / totalDistance) - progress) / getAverage(); // Progress / second + position.update(progress, speed); + } - position.setProgress(progress); + private double getAverage() { + return averageTimes.get(currentStation).stream().mapToDouble(time -> time).average().orElse(0.1); } } From 0e59e398af921e00387e75fda550371a98bc02a1 Mon Sep 17 00:00:00 2001 From: Topvennie Date: Fri, 12 Apr 2024 14:56:14 +0200 Subject: [PATCH 11/25] chore: bugfixes --- src/main/java/telraam/App.java | 4 +- .../telraam/logic/positioner/Position.java | 5 +++ .../positioner/nostradamus/Nostradamus.java | 44 ++++++++++--------- .../positioner/nostradamus/PositionList.java | 15 +++++-- .../positioner/nostradamus/TeamData.java | 37 +++++++++++----- 5 files changed, 69 insertions(+), 36 deletions(-) diff --git a/src/main/java/telraam/App.java b/src/main/java/telraam/App.java index 08a3847..0d79b23 100644 --- a/src/main/java/telraam/App.java +++ b/src/main/java/telraam/App.java @@ -24,6 +24,7 @@ import telraam.logic.lapper.robust.RobustLapper; import telraam.logic.lapper.slapper.Slapper; import telraam.logic.positioner.Positioner; +import telraam.logic.positioner.nostradamus.Nostradamus; import telraam.logic.positioner.simple.SimplePositioner; import telraam.station.FetcherFactory; import telraam.station.websocket.WebsocketFetcher; @@ -142,7 +143,8 @@ public void run(AppConfiguration configuration, Environment environment) { // Set up positioners Set positioners = new HashSet<>(); - positioners.add(new SimplePositioner(this.database)); +// 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); diff --git a/src/main/java/telraam/logic/positioner/Position.java b/src/main/java/telraam/logic/positioner/Position.java index 300837f..8a10da9 100644 --- a/src/main/java/telraam/logic/positioner/Position.java +++ b/src/main/java/telraam/logic/positioner/Position.java @@ -3,20 +3,25 @@ import lombok.Getter; import lombok.Setter; +import java.sql.Timestamp; + @Getter @Setter public class Position { private int teamId; private double progress; // Progress of the lap. Between 0-1 private double speed; // Current speed. progress / second + private long unix; public Position(int teamId) { this.teamId = teamId; this.progress = 0; this.speed = 0; + this.unix = 0; } public void update(double progress, double speed) { this.progress = progress; this.speed = speed; + this.unix = System.currentTimeMillis() / 1000L;; } } diff --git a/src/main/java/telraam/logic/positioner/nostradamus/Nostradamus.java b/src/main/java/telraam/logic/positioner/nostradamus/Nostradamus.java index 0d8e363..0e1230c 100644 --- a/src/main/java/telraam/logic/positioner/nostradamus/Nostradamus.java +++ b/src/main/java/telraam/logic/positioner/nostradamus/Nostradamus.java @@ -17,17 +17,18 @@ public class Nostradamus implements Positioner { private static final Logger logger = Logger.getLogger(Nostradamus.class.getName()); - private final int INTERVAL_CALCULATE = 500; // How often to handle new detections - private final int INTERVAL_FETCH = 60000; // Interval between fetching all stations, teams, ... - private final int INTERVAL_DETECTIONS = 1; // Amount of seconds to group detections by + private final int INTERVAL_CALCULATE_MS = 500; // How often to handle new detections + private final int INTERVAL_FETCH_MS = 60000; // Interval between fetching all stations, teams, ... + private final int INTERVAL_DETECTIONS_MS = 1000; // Amount of seconds to group detections by private final int AVERAGE_AMOUNT = 10; // Calculate the average running speed the last x intervals + private final double AVERAGE_SPRINTING_SPEED_M_S = 6.84; // Average sprinting speed m / s private final int MIN_RSSI = -84; private final Jdbi jdbi; private final List newDetections; // Contains not yet handled detections private final Lock detectionLock; private final Lock dataLock; - private Map batonToTeam; // Baton ID to Team - private Map teamData; // All team data + private Map batonToTeam; // Baton ID to Team ID + private Map teamData; // All team data private final PositionSender positionSender; public Nostradamus(Jdbi jdbi) { @@ -42,8 +43,8 @@ public Nostradamus(Jdbi jdbi) { this.positionSender = new PositionSender(); - new Thread(this::fetch); - new Thread(this::calculatePosition); + new Thread(this::fetch).start(); + new Thread(this::calculatePosition).start(); } private void setTeamData() { @@ -51,19 +52,20 @@ private void setTeamData() { List teams = jdbi.onDemand(TeamDAO.class).getAll(); teamData = teams.stream().collect(Collectors.toMap( - team -> team, - team -> new TeamData(team.getId(), INTERVAL_DETECTIONS, stations, AVERAGE_AMOUNT) + Team::getId, + team -> new TeamData(team.getId(), INTERVAL_DETECTIONS_MS, stations, AVERAGE_AMOUNT, AVERAGE_SPRINTING_SPEED_M_S) )); } private void fetch() { List switchovers = jdbi.onDemand(BatonSwitchoverDAO.class).getAll(); - List teams = jdbi.onDemand(TeamDAO.class).getAll(); - Map teamIdToTeam = teams.stream().collect(Collectors.toMap(Team::getId, team -> team)); - Map batonToTeam = switchovers.stream().collect(Collectors.toMap( + Map batonToTeam = switchovers.stream().sorted( + Comparator.comparing(BatonSwitchover::getTimestamp) + ).collect(Collectors.toMap( BatonSwitchover::getNewBatonId, - switchover -> teamIdToTeam.get(switchover.getTeamId()) + BatonSwitchover::getTeamId, + (existing, replacement) -> replacement )); if (!this.batonToTeam.equals(batonToTeam)) { @@ -74,23 +76,23 @@ private void fetch() { // zzzzzzzz try { - Thread.sleep(INTERVAL_FETCH); + Thread.sleep(INTERVAL_FETCH_MS); } catch (InterruptedException e) { logger.severe(e.getMessage()); } } private void calculatePosition() { - Set changedTeams = new HashSet<>(); // List of teams that have changed station + Set changedTeams = new HashSet<>(); // List of teams that have changed station while (true) { dataLock.lock(); changedTeams.clear(); detectionLock.lock(); for (Detection detection: newDetections) { if (batonToTeam.containsKey(detection.getBatonId())) { - Team team = batonToTeam.get(detection.getBatonId()); - if (teamData.get(team).addDetection(detection)) { - changedTeams.add(team); + Integer teamId = batonToTeam.get(detection.getBatonId()); + if (teamData.get(teamId).addDetection(detection)) { + changedTeams.add(teamId); } } } @@ -99,8 +101,8 @@ private void calculatePosition() { if (!changedTeams.isEmpty()) { // Update - for (Team team: changedTeams) { - teamData.get(team).updatePosition(); + for (Integer teamId: changedTeams) { + teamData.get(teamId).updatePosition(); } // Send new data to the websocket @@ -113,7 +115,7 @@ private void calculatePosition() { // zzzzzzzz try { - Thread.sleep(INTERVAL_CALCULATE); + Thread.sleep(INTERVAL_CALCULATE_MS); } catch (InterruptedException e) { logger.severe(e.getMessage()); } diff --git a/src/main/java/telraam/logic/positioner/nostradamus/PositionList.java b/src/main/java/telraam/logic/positioner/nostradamus/PositionList.java index e10540d..7be0fa8 100644 --- a/src/main/java/telraam/logic/positioner/nostradamus/PositionList.java +++ b/src/main/java/telraam/logic/positioner/nostradamus/PositionList.java @@ -2,20 +2,24 @@ 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 PositionList extends ArrayList { private final int interval; + private final List stations; @Getter private Detection currentPosition; private Timestamp newestDetection; - public PositionList(int interval) { + public PositionList(int interval, List 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); } @@ -32,13 +36,14 @@ public boolean add(Detection e) { newestDetection = e.getTimestamp(); } - if (!e.getStationId().equals(currentPosition.getStationId())) { + 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(); @@ -49,7 +54,11 @@ public boolean add(Detection e) { 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.getNanos() - oldest.getNanos() > interval; + return newest.getTime() - oldest.getTime() < interval; } } diff --git a/src/main/java/telraam/logic/positioner/nostradamus/TeamData.java b/src/main/java/telraam/logic/positioner/nostradamus/TeamData.java index 9f3c0be..03c554e 100644 --- a/src/main/java/telraam/logic/positioner/nostradamus/TeamData.java +++ b/src/main/java/telraam/logic/positioner/nostradamus/TeamData.java @@ -5,10 +5,7 @@ import telraam.database.models.Station; import telraam.logic.positioner.Position; -import java.util.Comparator; -import java.util.HashMap; -import java.util.List; -import java.util.Map; +import java.util.*; import java.util.stream.Collectors; public class TeamData { @@ -24,16 +21,31 @@ public class TeamData { private final Position position; - public TeamData(int teamId, int interval, List stations, int averageAmount) { - this.detections = new PositionList(interval); + public TeamData(int teamId, int interval, List stations, int averageAmount, double sprintingSpeed) { this.stations = stations; this.stations.sort(Comparator.comparing(Station::getDistanceFromStart)); + this.detections = new PositionList(interval, this.stations); this.idToStation = stations.stream().collect(Collectors.toMap(Station::getId, station -> station)); - this.averageTimes = stations.stream().collect(Collectors.toMap(station -> station, station -> new CircularQueue<>(averageAmount))); +// this.averageTimes = stations.stream().collect(Collectors.toMap(station -> station, station -> new CircularQueue<>(averageAmount))); this.previousStationArrival = System.currentTimeMillis(); this.currentStation = new Station(-10); // Will never trigger `isNextStation` for the first station this.totalDistance = this.stations.get(this.stations.size() - 1).getDistanceFromStart(); // Requires last station to be located at the start this.position = new Position(teamId); + this.averageTimes = getAverageTimes(averageAmount, sprintingSpeed); + } + + // Construct the average times and add a default value + // TODO: Populate with existing detections + private Map> getAverageTimes(int averageAmount, double sprintingSpeed) { + Map> averageTimes = new HashMap<>(); + for (Station station: stations) { + int index = stations.indexOf(station); + averageTimes.put(station, new CircularQueue<>(averageAmount)); + double distance = (stations.get((index + 1) % stations.size()).getDistanceFromStart() - station.getDistanceFromStart() + totalDistance) % totalDistance; + averageTimes.get(station).add((long) (distance / sprintingSpeed)); + } + + return averageTimes; } public boolean addDetection(Detection e) { @@ -44,9 +56,12 @@ public boolean addDetection(Detection e) { currentStation = idToStation.get(e.getStationId()); if (isNextStation()) { // Only add the time if it goes forward by exactly one station - averageTimes.get(previousStation).add(System.currentTimeMillis() - previousStationArrival); + if (averageTimes.containsKey(previousStation)) { + // While only be false for the first time an interval in ran as previousStation is still null + averageTimes.get(previousStation).add(System.currentTimeMillis() / 1000 - previousStationArrival); + } } - previousStationArrival = System.currentTimeMillis(); + previousStationArrival = System.currentTimeMillis() / 1000; } return newStation; @@ -60,8 +75,8 @@ private boolean isNextStation() { public void updatePosition() { double progress = currentStation.getDistanceFromStart() / totalDistance; Station nextStation = stations.get((stations.indexOf(currentStation) + 1) % stations.size()); - double distance = (nextStation.getDistanceFromStart() - currentStation.getDistanceFromStart() + totalDistance) % totalDistance; - double speed = ((distance / totalDistance) - progress) / getAverage(); // Progress / second +// double distance = (nextStation.getDistanceFromStart() - currentStation.getDistanceFromStart() + totalDistance) % totalDistance; + double speed = (((nextStation.getDistanceFromStart() / totalDistance) - progress + 1) % 1) / getAverage(); // Progress / second position.update(progress, speed); } From b540279f1e8fb4a5b3d6fd393f4dfed107de3083 Mon Sep 17 00:00:00 2001 From: Topvennie Date: Sat, 13 Apr 2024 18:39:13 +0200 Subject: [PATCH 12/25] chore: infite speed fix --- .../positioner/nostradamus/TeamData.java | 20 ++++++++++++++++--- 1 file changed, 17 insertions(+), 3 deletions(-) diff --git a/src/main/java/telraam/logic/positioner/nostradamus/TeamData.java b/src/main/java/telraam/logic/positioner/nostradamus/TeamData.java index 03c554e..ee2044c 100644 --- a/src/main/java/telraam/logic/positioner/nostradamus/TeamData.java +++ b/src/main/java/telraam/logic/positioner/nostradamus/TeamData.java @@ -51,7 +51,7 @@ private Map> getAverageTimes(int averageAmount, double sprin public boolean addDetection(Detection e) { boolean newStation = detections.add(e); - if (newStation) { + if ((newStation && isForwardStation()) || currentStation.getId() == -10) { previousStation = currentStation; currentStation = idToStation.get(e.getStationId()); if (isNextStation()) { @@ -67,8 +67,16 @@ public boolean addDetection(Detection e) { return newStation; } + private int getStationDiff() { + return (stations.indexOf(currentStation) - stations.indexOf(previousStation) + stations.size()) % stations.size(); + } + + private boolean isForwardStation() { + return getStationDiff() > 0; + } + private boolean isNextStation() { - return (stations.indexOf(currentStation) - stations.indexOf(previousStation) + stations.size()) % stations.size() == 1; + return getStationDiff() == 1; } // TODO: Smoothen out @@ -77,11 +85,17 @@ public void updatePosition() { Station nextStation = stations.get((stations.indexOf(currentStation) + 1) % stations.size()); // double distance = (nextStation.getDistanceFromStart() - currentStation.getDistanceFromStart() + totalDistance) % totalDistance; double speed = (((nextStation.getDistanceFromStart() / totalDistance) - progress + 1) % 1) / getAverage(); // Progress / second +// if (Double.isInfinite(speed)) { +// System.out.println("aiaiaia"); +// System.out.println("Averages"); +// averageTimes.get(currentStation).forEach(time -> System.out.println("Time: " + time)); +// System.out.println("Average: " + getAverage()); +// } position.update(progress, speed); } private double getAverage() { - return averageTimes.get(currentStation).stream().mapToDouble(time -> time).average().orElse(0.1); + return averageTimes.get(currentStation).stream().mapToDouble(time -> time).average().getAsDouble(); } } From 52fad65af86003395c8d282c2508188b270894dc Mon Sep 17 00:00:00 2001 From: Topvennie Date: Sun, 14 Apr 2024 15:10:42 +0200 Subject: [PATCH 13/25] fix math --- .../telraam/logic/positioner/Position.java | 12 ++++----- .../positioner/nostradamus/TeamData.java | 26 ++++++++----------- 2 files changed, 17 insertions(+), 21 deletions(-) diff --git a/src/main/java/telraam/logic/positioner/Position.java b/src/main/java/telraam/logic/positioner/Position.java index 8a10da9..e9d858f 100644 --- a/src/main/java/telraam/logic/positioner/Position.java +++ b/src/main/java/telraam/logic/positioner/Position.java @@ -1,27 +1,27 @@ package telraam.logic.positioner; +import com.fasterxml.jackson.databind.PropertyNamingStrategies; +import com.fasterxml.jackson.databind.annotation.JsonNaming; import lombok.Getter; import lombok.Setter; -import java.sql.Timestamp; - -@Getter @Setter +@Getter @Setter @JsonNaming(PropertyNamingStrategies.SnakeCaseStrategy.class) public class Position { private int teamId; private double progress; // Progress of the lap. Between 0-1 private double speed; // Current speed. progress / second - private long unix; + private long timestamp; public Position(int teamId) { this.teamId = teamId; this.progress = 0; this.speed = 0; - this.unix = 0; + this.timestamp = 0; } public void update(double progress, double speed) { this.progress = progress; this.speed = speed; - this.unix = System.currentTimeMillis() / 1000L;; + this.timestamp = System.currentTimeMillis() / 1000L;; } } diff --git a/src/main/java/telraam/logic/positioner/nostradamus/TeamData.java b/src/main/java/telraam/logic/positioner/nostradamus/TeamData.java index ee2044c..0fe5a67 100644 --- a/src/main/java/telraam/logic/positioner/nostradamus/TeamData.java +++ b/src/main/java/telraam/logic/positioner/nostradamus/TeamData.java @@ -28,7 +28,7 @@ public TeamData(int teamId, int interval, List stations, int averageAmo this.idToStation = stations.stream().collect(Collectors.toMap(Station::getId, station -> station)); // this.averageTimes = stations.stream().collect(Collectors.toMap(station -> station, station -> new CircularQueue<>(averageAmount))); this.previousStationArrival = System.currentTimeMillis(); - this.currentStation = new Station(-10); // Will never trigger `isNextStation` for the first station + this.currentStation = new Station(-1); // Will never trigger `isNextStation` for the first station this.totalDistance = this.stations.get(this.stations.size() - 1).getDistanceFromStart(); // Requires last station to be located at the start this.position = new Position(teamId); this.averageTimes = getAverageTimes(averageAmount, sprintingSpeed); @@ -51,7 +51,7 @@ private Map> getAverageTimes(int averageAmount, double sprin public boolean addDetection(Detection e) { boolean newStation = detections.add(e); - if ((newStation && isForwardStation()) || currentStation.getId() == -10) { + if ((newStation && isForwardStation(currentStation, idToStation.get(e.getStationId()))) || currentStation.getId() == -1) { previousStation = currentStation; currentStation = idToStation.get(e.getStationId()); if (isNextStation()) { @@ -62,35 +62,31 @@ public boolean addDetection(Detection e) { } } previousStationArrival = System.currentTimeMillis() / 1000; + + return true; } - return newStation; + return false; } - private int getStationDiff() { - return (stations.indexOf(currentStation) - stations.indexOf(previousStation) + stations.size()) % stations.size(); + private int getStationDiff(Station oldStation, Station newStation) { + return (stations.indexOf(newStation) - stations.indexOf(oldStation) + stations.size()) % stations.size(); } - private boolean isForwardStation() { - return getStationDiff() > 0; + private boolean isForwardStation(Station oldStation, Station newStation) { + int stationDiff = getStationDiff(oldStation, newStation); + return stationDiff > 0 && stationDiff < 3; } private boolean isNextStation() { - return getStationDiff() == 1; + return getStationDiff(previousStation, currentStation) == 1; } // TODO: Smoothen out public void updatePosition() { double progress = currentStation.getDistanceFromStart() / totalDistance; Station nextStation = stations.get((stations.indexOf(currentStation) + 1) % stations.size()); -// double distance = (nextStation.getDistanceFromStart() - currentStation.getDistanceFromStart() + totalDistance) % totalDistance; double speed = (((nextStation.getDistanceFromStart() / totalDistance) - progress + 1) % 1) / getAverage(); // Progress / second -// if (Double.isInfinite(speed)) { -// System.out.println("aiaiaia"); -// System.out.println("Averages"); -// averageTimes.get(currentStation).forEach(time -> System.out.println("Time: " + time)); -// System.out.println("Average: " + getAverage()); -// } position.update(progress, speed); } From 1f8088ae027942acd05819ad1ee3b209dfbf2f88 Mon Sep 17 00:00:00 2001 From: Topvennie Date: Fri, 19 Apr 2024 00:50:40 +0200 Subject: [PATCH 14/25] Smooth out animations --- .../telraam/logic/positioner/Position.java | 6 +- .../positioner/nostradamus/Nostradamus.java | 2 +- .../positioner/nostradamus/PositionList.java | 4 -- .../positioner/nostradamus/TeamData.java | 56 ++++++++++++++----- 4 files changed, 45 insertions(+), 23 deletions(-) diff --git a/src/main/java/telraam/logic/positioner/Position.java b/src/main/java/telraam/logic/positioner/Position.java index e9d858f..6582deb 100644 --- a/src/main/java/telraam/logic/positioner/Position.java +++ b/src/main/java/telraam/logic/positioner/Position.java @@ -7,10 +7,10 @@ @Getter @Setter @JsonNaming(PropertyNamingStrategies.SnakeCaseStrategy.class) public class Position { - private int teamId; + private final int teamId; private double progress; // Progress of the lap. Between 0-1 private double speed; // Current speed. progress / second - private long timestamp; + private double timestamp; public Position(int teamId) { this.teamId = teamId; @@ -22,6 +22,6 @@ public Position(int teamId) { public void update(double progress, double speed) { this.progress = progress; this.speed = speed; - this.timestamp = System.currentTimeMillis() / 1000L;; + this.timestamp = System.currentTimeMillis() / 1000D; } } diff --git a/src/main/java/telraam/logic/positioner/nostradamus/Nostradamus.java b/src/main/java/telraam/logic/positioner/nostradamus/Nostradamus.java index 0e1230c..22831f9 100644 --- a/src/main/java/telraam/logic/positioner/nostradamus/Nostradamus.java +++ b/src/main/java/telraam/logic/positioner/nostradamus/Nostradamus.java @@ -19,7 +19,7 @@ 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 private final int INTERVAL_FETCH_MS = 60000; // Interval between fetching all stations, teams, ... - private final int INTERVAL_DETECTIONS_MS = 1000; // Amount of seconds to group detections by + private final int INTERVAL_DETECTIONS_MS = 3000; // Amount of milliseconds to group detections by private final int AVERAGE_AMOUNT = 10; // Calculate the average running speed the last x intervals private final double AVERAGE_SPRINTING_SPEED_M_S = 6.84; // Average sprinting speed m / s private final int MIN_RSSI = -84; diff --git a/src/main/java/telraam/logic/positioner/nostradamus/PositionList.java b/src/main/java/telraam/logic/positioner/nostradamus/PositionList.java index 7be0fa8..c4aaffb 100644 --- a/src/main/java/telraam/logic/positioner/nostradamus/PositionList.java +++ b/src/main/java/telraam/logic/positioner/nostradamus/PositionList.java @@ -24,10 +24,6 @@ public PositionList(int interval, List stations) { this.newestDetection = new Timestamp(0); } - /** - * @param e element to add - * @return True if the current position has changed - */ @Override public boolean add(Detection e) { super.add(e); diff --git a/src/main/java/telraam/logic/positioner/nostradamus/TeamData.java b/src/main/java/telraam/logic/positioner/nostradamus/TeamData.java index 0fe5a67..024a426 100644 --- a/src/main/java/telraam/logic/positioner/nostradamus/TeamData.java +++ b/src/main/java/telraam/logic/positioner/nostradamus/TeamData.java @@ -12,8 +12,8 @@ public class TeamData { private final PositionList detections; private final List stations; // Station list, ordered by distance from start private final Map idToStation; // Map going from a station id to the station - private final Map> averageTimes; // List of average times for each station. Going from station 2 to 3 is saved in 3. - private long previousStationArrival; // Arrival time of previous station. Used to calculate the average times + private final Map> averageTimes; // List of average times for each station. Going from station 2 to 3 is saved in 3. + private Double previousStationArrival; // Arrival time of previous station. Used to calculate the average times private Station currentStation; // Current station location private Station previousStation; // Previous station location private final Double totalDistance; @@ -27,22 +27,22 @@ public TeamData(int teamId, int interval, List stations, int averageAmo this.detections = new PositionList(interval, this.stations); this.idToStation = stations.stream().collect(Collectors.toMap(Station::getId, station -> station)); // this.averageTimes = stations.stream().collect(Collectors.toMap(station -> station, station -> new CircularQueue<>(averageAmount))); - this.previousStationArrival = System.currentTimeMillis(); - this.currentStation = new Station(-1); // Will never trigger `isNextStation` for the first station + this.previousStationArrival = System.currentTimeMillis() / 1000D; + this.currentStation = new Station(-10); // Will never trigger `isNextStation` for the first station this.totalDistance = this.stations.get(this.stations.size() - 1).getDistanceFromStart(); // Requires last station to be located at the start - this.position = new Position(teamId); this.averageTimes = getAverageTimes(averageAmount, sprintingSpeed); + this.position = new Position(teamId); } // Construct the average times and add a default value // TODO: Populate with existing detections - private Map> getAverageTimes(int averageAmount, double sprintingSpeed) { - Map> averageTimes = new HashMap<>(); + private Map> getAverageTimes(int averageAmount, double sprintingSpeed) { + Map> averageTimes = new HashMap<>(); for (Station station: stations) { int index = stations.indexOf(station); averageTimes.put(station, new CircularQueue<>(averageAmount)); double distance = (stations.get((index + 1) % stations.size()).getDistanceFromStart() - station.getDistanceFromStart() + totalDistance) % totalDistance; - averageTimes.get(station).add((long) (distance / sprintingSpeed)); + averageTimes.get(station).add((distance / sprintingSpeed)); } return averageTimes; @@ -58,10 +58,11 @@ public boolean addDetection(Detection e) { // Only add the time if it goes forward by exactly one station if (averageTimes.containsKey(previousStation)) { // While only be false for the first time an interval in ran as previousStation is still null - averageTimes.get(previousStation).add(System.currentTimeMillis() / 1000 - previousStationArrival); + averageTimes.get(previousStation).add(System.currentTimeMillis() / 1000D - previousStationArrival); } } - previousStationArrival = System.currentTimeMillis() / 1000; + + previousStationArrival = System.currentTimeMillis() / 1000D; return true; } @@ -82,16 +83,41 @@ private boolean isNextStation() { return getStationDiff(previousStation, currentStation) == 1; } - // TODO: Smoothen out public void updatePosition() { - double progress = currentStation.getDistanceFromStart() / totalDistance; + // Arrive at second x + double currentTime = System.currentTimeMillis() / 1000D; + double nextStationArrival = currentTime + getMedian(); + + // Current at progress y + double secondsSince = currentTime - position.getTimestamp(); + double theoreticalProgress = (position.getProgress() + (position.getSpeed() * secondsSince)) % 1; + + // Progress in z amount of seconds Station nextStation = stations.get((stations.indexOf(currentStation) + 1) % stations.size()); - double speed = (((nextStation.getDistanceFromStart() / totalDistance) - progress + 1) % 1) / getAverage(); // Progress / second + double goalProgress = nextStation.getDistanceFromStart() / totalDistance; + double speed, progress; + // TODO: Do in a different way + // TODO: Right now assumes the difference between each station is the same + if ((goalProgress - theoreticalProgress + 1) % 1 >= 1 - ((double) 1 / stations.size()) || averageTimes.get(currentStation).size() < 5) { + // Animation was already further than the next station so just tp + progress = currentStation.getDistanceFromStart() / totalDistance; + speed = (((nextStation.getDistanceFromStart() / totalDistance) - progress + 1) % 1) / getMedian(); + } else { + // Animation is either behind or maximum in front by one station. Adjust so we're synced at the next station + progress = theoreticalProgress; + speed = ((goalProgress - theoreticalProgress + 1) % 1) / (nextStationArrival - currentTime); + } position.update(progress, speed); } - private double getAverage() { - return averageTimes.get(currentStation).stream().mapToDouble(time -> time).average().getAsDouble(); + private double getMedian() { + List sortedList = new ArrayList<>(averageTimes.get(currentStation)); + Collections.sort(sortedList); + + int size = sortedList.size(); + return size % 2 == 0 ? (sortedList.get(size / 2 - 1) + sortedList.get(size / 2)) / 2D : sortedList.get(size / 2); } } + +// TODO: Possible problem: Might arrive to early at station -> Move backwards stations by 5 m From ab0b0cb3807967793c7113e18602a628c60d43d6 Mon Sep 17 00:00:00 2001 From: Topvennie Date: Fri, 19 Apr 2024 14:48:51 +0200 Subject: [PATCH 15/25] chore: seconds to milliseconds --- .../telraam/logic/positioner/Position.java | 4 +-- .../positioner/nostradamus/Nostradamus.java | 4 +-- .../positioner/nostradamus/TeamData.java | 28 +++++++++---------- 3 files changed, 18 insertions(+), 18 deletions(-) diff --git a/src/main/java/telraam/logic/positioner/Position.java b/src/main/java/telraam/logic/positioner/Position.java index 6582deb..1a0e002 100644 --- a/src/main/java/telraam/logic/positioner/Position.java +++ b/src/main/java/telraam/logic/positioner/Position.java @@ -10,7 +10,7 @@ public class Position { private final int teamId; private double progress; // Progress of the lap. Between 0-1 private double speed; // Current speed. progress / second - private double timestamp; + private long timestamp; public Position(int teamId) { this.teamId = teamId; @@ -22,6 +22,6 @@ public Position(int teamId) { public void update(double progress, double speed) { this.progress = progress; this.speed = speed; - this.timestamp = System.currentTimeMillis() / 1000D; + this.timestamp = System.currentTimeMillis(); } } diff --git a/src/main/java/telraam/logic/positioner/nostradamus/Nostradamus.java b/src/main/java/telraam/logic/positioner/nostradamus/Nostradamus.java index 22831f9..3e91aba 100644 --- a/src/main/java/telraam/logic/positioner/nostradamus/Nostradamus.java +++ b/src/main/java/telraam/logic/positioner/nostradamus/Nostradamus.java @@ -21,7 +21,7 @@ public class Nostradamus implements Positioner { private final int INTERVAL_FETCH_MS = 60000; // Interval between fetching all stations, teams, ... private final int INTERVAL_DETECTIONS_MS = 3000; // Amount of milliseconds to group detections by private final int AVERAGE_AMOUNT = 10; // Calculate the average running speed the last x intervals - private final double AVERAGE_SPRINTING_SPEED_M_S = 6.84; // Average sprinting speed m / s + private final double AVERAGE_SPRINTING_SPEED_M_MS = 0.00684; // Average sprinting speed m / ms private final int MIN_RSSI = -84; private final Jdbi jdbi; private final List newDetections; // Contains not yet handled detections @@ -53,7 +53,7 @@ private void setTeamData() { teamData = teams.stream().collect(Collectors.toMap( Team::getId, - team -> new TeamData(team.getId(), INTERVAL_DETECTIONS_MS, stations, AVERAGE_AMOUNT, AVERAGE_SPRINTING_SPEED_M_S) + team -> new TeamData(team.getId(), INTERVAL_DETECTIONS_MS, stations, AVERAGE_AMOUNT, AVERAGE_SPRINTING_SPEED_M_MS) )); } diff --git a/src/main/java/telraam/logic/positioner/nostradamus/TeamData.java b/src/main/java/telraam/logic/positioner/nostradamus/TeamData.java index 024a426..cec2948 100644 --- a/src/main/java/telraam/logic/positioner/nostradamus/TeamData.java +++ b/src/main/java/telraam/logic/positioner/nostradamus/TeamData.java @@ -12,8 +12,8 @@ public class TeamData { private final PositionList detections; private final List stations; // Station list, ordered by distance from start private final Map idToStation; // Map going from a station id to the station - private final Map> averageTimes; // List of average times for each station. Going from station 2 to 3 is saved in 3. - private Double previousStationArrival; // Arrival time of previous station. Used to calculate the average times + private final Map> averageTimes; // List of average times for each station. Going from station 2 to 3 is saved in 3. + private long previousStationArrival; // Arrival time of previous station. Used to calculate the average times private Station currentStation; // Current station location private Station previousStation; // Previous station location private final Double totalDistance; @@ -27,7 +27,7 @@ public TeamData(int teamId, int interval, List stations, int averageAmo this.detections = new PositionList(interval, this.stations); this.idToStation = stations.stream().collect(Collectors.toMap(Station::getId, station -> station)); // this.averageTimes = stations.stream().collect(Collectors.toMap(station -> station, station -> new CircularQueue<>(averageAmount))); - this.previousStationArrival = System.currentTimeMillis() / 1000D; + this.previousStationArrival = System.currentTimeMillis(); this.currentStation = new Station(-10); // Will never trigger `isNextStation` for the first station this.totalDistance = this.stations.get(this.stations.size() - 1).getDistanceFromStart(); // Requires last station to be located at the start this.averageTimes = getAverageTimes(averageAmount, sprintingSpeed); @@ -36,13 +36,13 @@ public TeamData(int teamId, int interval, List stations, int averageAmo // Construct the average times and add a default value // TODO: Populate with existing detections - private Map> getAverageTimes(int averageAmount, double sprintingSpeed) { - Map> averageTimes = new HashMap<>(); + private Map> getAverageTimes(int averageAmount, double sprintingSpeed) { + Map> averageTimes = new HashMap<>(); for (Station station: stations) { int index = stations.indexOf(station); averageTimes.put(station, new CircularQueue<>(averageAmount)); double distance = (stations.get((index + 1) % stations.size()).getDistanceFromStart() - station.getDistanceFromStart() + totalDistance) % totalDistance; - averageTimes.get(station).add((distance / sprintingSpeed)); + averageTimes.get(station).add((long) (distance / sprintingSpeed)); } return averageTimes; @@ -58,11 +58,11 @@ public boolean addDetection(Detection e) { // Only add the time if it goes forward by exactly one station if (averageTimes.containsKey(previousStation)) { // While only be false for the first time an interval in ran as previousStation is still null - averageTimes.get(previousStation).add(System.currentTimeMillis() / 1000D - previousStationArrival); + averageTimes.get(previousStation).add(System.currentTimeMillis() - previousStationArrival); } } - previousStationArrival = System.currentTimeMillis() / 1000D; + previousStationArrival = System.currentTimeMillis(); return true; } @@ -84,13 +84,13 @@ private boolean isNextStation() { } public void updatePosition() { - // Arrive at second x - double currentTime = System.currentTimeMillis() / 1000D; + // Arrive at millisecond x + double currentTime = System.currentTimeMillis(); double nextStationArrival = currentTime + getMedian(); // Current at progress y - double secondsSince = currentTime - position.getTimestamp(); - double theoreticalProgress = (position.getProgress() + (position.getSpeed() * secondsSince)) % 1; + double milliSecondsSince = currentTime - position.getTimestamp(); + double theoreticalProgress = (position.getProgress() + (position.getSpeed() * milliSecondsSince)) % 1; // Progress in z amount of seconds Station nextStation = stations.get((stations.indexOf(currentStation) + 1) % stations.size()); @@ -112,11 +112,11 @@ public void updatePosition() { } private double getMedian() { - List sortedList = new ArrayList<>(averageTimes.get(currentStation)); + List sortedList = new ArrayList<>(averageTimes.get(currentStation)); Collections.sort(sortedList); int size = sortedList.size(); - return size % 2 == 0 ? (sortedList.get(size / 2 - 1) + sortedList.get(size / 2)) / 2D : sortedList.get(size / 2); + return size % 2 == 0 ? (sortedList.get(size / 2 - 1) + sortedList.get(size / 2)) / 2D : (sortedList.get(size / 2)); } } From 05eb54f8243ce7263bff7890a606bb6449a710f4 Mon Sep 17 00:00:00 2001 From: Topvennie Date: Fri, 19 Apr 2024 18:21:24 +0200 Subject: [PATCH 16/25] Refactor: Group common data --- .../positioner/nostradamus/Nostradamus.java | 4 +- .../positioner/nostradamus/PositionList.java | 1 + .../positioner/nostradamus/StationData.java | 25 ++++ .../positioner/nostradamus/TeamData.java | 123 ++++++++---------- 4 files changed, 86 insertions(+), 67 deletions(-) create mode 100644 src/main/java/telraam/logic/positioner/nostradamus/StationData.java diff --git a/src/main/java/telraam/logic/positioner/nostradamus/Nostradamus.java b/src/main/java/telraam/logic/positioner/nostradamus/Nostradamus.java index 3e91aba..8f5004d 100644 --- a/src/main/java/telraam/logic/positioner/nostradamus/Nostradamus.java +++ b/src/main/java/telraam/logic/positioner/nostradamus/Nostradamus.java @@ -23,6 +23,7 @@ public class Nostradamus implements Positioner { private final int AVERAGE_AMOUNT = 10; // Calculate the average running speed the last x intervals private final double AVERAGE_SPRINTING_SPEED_M_MS = 0.00684; // Average sprinting speed m / ms private final int MIN_RSSI = -84; + private final int FINISH_OFFSET = 0; private final Jdbi jdbi; private final List newDetections; // Contains not yet handled detections private final Lock detectionLock; @@ -49,11 +50,12 @@ public Nostradamus(Jdbi jdbi) { private void setTeamData() { List stations = jdbi.onDemand(StationDAO.class).getAll(); + stations.sort(Comparator.comparing(Station::getDistanceFromStart)); List teams = jdbi.onDemand(TeamDAO.class).getAll(); teamData = teams.stream().collect(Collectors.toMap( Team::getId, - team -> new TeamData(team.getId(), INTERVAL_DETECTIONS_MS, stations, AVERAGE_AMOUNT, AVERAGE_SPRINTING_SPEED_M_MS) + team -> new TeamData(team.getId(), INTERVAL_DETECTIONS_MS, stations, AVERAGE_AMOUNT, AVERAGE_SPRINTING_SPEED_M_MS, FINISH_OFFSET) )); } diff --git a/src/main/java/telraam/logic/positioner/nostradamus/PositionList.java b/src/main/java/telraam/logic/positioner/nostradamus/PositionList.java index c4aaffb..e1c8944 100644 --- a/src/main/java/telraam/logic/positioner/nostradamus/PositionList.java +++ b/src/main/java/telraam/logic/positioner/nostradamus/PositionList.java @@ -24,6 +24,7 @@ public PositionList(int interval, List stations) { this.newestDetection = new Timestamp(0); } + // Returns True if it's a new station @Override public boolean add(Detection e) { super.add(e); diff --git a/src/main/java/telraam/logic/positioner/nostradamus/StationData.java b/src/main/java/telraam/logic/positioner/nostradamus/StationData.java new file mode 100644 index 0000000..e65101d --- /dev/null +++ b/src/main/java/telraam/logic/positioner/nostradamus/StationData.java @@ -0,0 +1,25 @@ +package telraam.logic.positioner.nostradamus; + +import telraam.database.models.Station; + +import java.util.List; + +public record StationData( + Station station, // The station + Station nextStation, // The next station + List averageTimes, // 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 + double currentProgress, // The progress value of this station + double nextProgress // The progress value of the next station +) { + public StationData(List stations, int index, int averageAmount, int totalDistance) { + this( + stations.get(index), + stations.get((index + 1) % stations.size()), + new CircularQueue<>(averageAmount), + index, + stations.get(index).getDistanceFromStart() / totalDistance, + stations.get((index + 1) % stations.size()).getDistanceFromStart() / totalDistance + ); + } +} diff --git a/src/main/java/telraam/logic/positioner/nostradamus/TeamData.java b/src/main/java/telraam/logic/positioner/nostradamus/TeamData.java index cec2948..ba5f047 100644 --- a/src/main/java/telraam/logic/positioner/nostradamus/TeamData.java +++ b/src/main/java/telraam/logic/positioner/nostradamus/TeamData.java @@ -9,59 +9,53 @@ import java.util.stream.Collectors; public class TeamData { - private final PositionList detections; - private final List stations; // Station list, ordered by distance from start - private final Map idToStation; // Map going from a station id to the station - private final Map> averageTimes; // List of average times for each station. Going from station 2 to 3 is saved in 3. + private final PositionList detections; // List with all relevant detections + private final Map stations; // Station list private long previousStationArrival; // Arrival time of previous station. Used to calculate the average times - private Station currentStation; // Current station location - private Station previousStation; // Previous station location - private final Double totalDistance; + private StationData currentStation; // Current station location + private StationData previousStation; // Previous station location + private final int totalDistance; // Total distance of the track @Getter - private final Position position; - - - public TeamData(int teamId, int interval, List stations, int averageAmount, double sprintingSpeed) { - this.stations = stations; - this.stations.sort(Comparator.comparing(Station::getDistanceFromStart)); - this.detections = new PositionList(interval, this.stations); - this.idToStation = stations.stream().collect(Collectors.toMap(Station::getId, station -> station)); -// this.averageTimes = stations.stream().collect(Collectors.toMap(station -> station, station -> new CircularQueue<>(averageAmount))); + private final Position position; // Data to send to the websocket + private final int maxDeviance; // Maximum deviance the animation can have from the reality + + + public TeamData(int teamId, int interval, List stations, int averageAmount, double sprintingSpeed, int finishOffset) { + stations.sort(Comparator.comparing(Station::getDistanceFromStart)); + this.totalDistance = (int) (stations.get(stations.size() - 1).getDistanceFromStart() + finishOffset); + this.stations = stations.stream().collect(Collectors.toMap( + Station::getId, + station -> new StationData( + stations, + stations.indexOf(station), + averageAmount, + totalDistance + ) + )); + // Pre-populate with some data + this.stations.forEach((stationId, stationData) -> stationData.averageTimes().add( + (long) (((stationData.nextStation().getDistanceFromStart() - stationData.station().getDistanceFromStart() + totalDistance) % totalDistance) / sprintingSpeed) + )); + this.detections = new PositionList(interval, stations); this.previousStationArrival = System.currentTimeMillis(); - this.currentStation = new Station(-10); // Will never trigger `isNextStation` for the first station - this.totalDistance = this.stations.get(this.stations.size() - 1).getDistanceFromStart(); // Requires last station to be located at the start - this.averageTimes = getAverageTimes(averageAmount, sprintingSpeed); + this.currentStation = new StationData(new Station(-10), new Station(-9), new CircularQueue<>(0), -10, -10, -10); // Will never trigger `isNextStation` for the first station this.position = new Position(teamId); + this.maxDeviance = 1 / stations.size(); } - // Construct the average times and add a default value - // TODO: Populate with existing detections - private Map> getAverageTimes(int averageAmount, double sprintingSpeed) { - Map> averageTimes = new HashMap<>(); - for (Station station: stations) { - int index = stations.indexOf(station); - averageTimes.put(station, new CircularQueue<>(averageAmount)); - double distance = (stations.get((index + 1) % stations.size()).getDistanceFromStart() - station.getDistanceFromStart() + totalDistance) % totalDistance; - averageTimes.get(station).add((long) (distance / sprintingSpeed)); - } - - return averageTimes; - } - + // Add a new detection + // Returns true if the team is at a next station public boolean addDetection(Detection e) { boolean newStation = detections.add(e); - if ((newStation && isForwardStation(currentStation, idToStation.get(e.getStationId()))) || currentStation.getId() == -1) { + if ((newStation && isForwardStation(currentStation.index(), stations.get(e.getStationId()).index())) || currentStation.index() == -10) { + // Is at a new station that is in front of the previous one. previousStation = currentStation; - currentStation = idToStation.get(e.getStationId()); + currentStation = stations.get(e.getStationId()); if (isNextStation()) { // Only add the time if it goes forward by exactly one station - if (averageTimes.containsKey(previousStation)) { - // While only be false for the first time an interval in ran as previousStation is still null - averageTimes.get(previousStation).add(System.currentTimeMillis() - previousStationArrival); - } + previousStation.averageTimes().add(System.currentTimeMillis() - previousStationArrival); } - previousStationArrival = System.currentTimeMillis(); return true; @@ -70,40 +64,36 @@ public boolean addDetection(Detection e) { return false; } - private int getStationDiff(Station oldStation, Station newStation) { - return (stations.indexOf(newStation) - stations.indexOf(oldStation) + stations.size()) % stations.size(); - } - - private boolean isForwardStation(Station oldStation, Station newStation) { - int stationDiff = getStationDiff(oldStation, newStation); + private boolean isForwardStation(int oldStation, int newStation) { + int stationDiff = (newStation - oldStation + stations.size()) % stations.size(); return stationDiff > 0 && stationDiff < 3; } private boolean isNextStation() { - return getStationDiff(previousStation, currentStation) == 1; + return Objects.equals(previousStation.nextStation().getId(), currentStation.station().getId()); } + // Update the position data public void updatePosition() { - // Arrive at millisecond x - double currentTime = System.currentTimeMillis(); - double nextStationArrival = currentTime + getMedian(); + long currentTime = System.currentTimeMillis(); - // Current at progress y - double milliSecondsSince = currentTime - position.getTimestamp(); + // Animation is currently at progress x + long milliSecondsSince = currentTime - position.getTimestamp(); double theoreticalProgress = (position.getProgress() + (position.getSpeed() * milliSecondsSince)) % 1; - // Progress in z amount of seconds - Station nextStation = stations.get((stations.indexOf(currentStation) + 1) % stations.size()); - double goalProgress = nextStation.getDistanceFromStart() / totalDistance; + // Arrive at next station at timestamp y and progress z + long nextStationArrival = currentTime + getMedian(); + double goalProgress = currentStation.nextProgress(); + double speed, progress; - // TODO: Do in a different way - // TODO: Right now assumes the difference between each station is the same - if ((goalProgress - theoreticalProgress + 1) % 1 >= 1 - ((double) 1 / stations.size()) || averageTimes.get(currentStation).size() < 5) { - // Animation was already further than the next station so just tp - progress = currentStation.getDistanceFromStart() / totalDistance; - speed = (((nextStation.getDistanceFromStart() / totalDistance) - progress + 1) % 1) / getMedian(); + // Determine whether to speed up / slow down the animation or teleport it + double difference = (goalProgress - theoreticalProgress + 1) % 1; + if ((difference >= maxDeviance && difference <= 1 - maxDeviance) || previousStation.averageTimes().size() < 5) { + // Animation was too far behind or ahead so teleport + progress = currentStation.currentProgress(); + speed = ((currentStation.nextProgress() - progress + 1) % 1) / getMedian(); } else { - // Animation is either behind or maximum in front by one station. Adjust so we're synced at the next station + // Animation is close enough, adjust so that we're synced at the next station progress = theoreticalProgress; speed = ((goalProgress - theoreticalProgress + 1) % 1) / (nextStationArrival - currentTime); } @@ -111,13 +101,14 @@ public void updatePosition() { position.update(progress, speed); } - private double getMedian() { - List sortedList = new ArrayList<>(averageTimes.get(currentStation)); + // Get the medium of the average times + private long getMedian() { + List sortedList = new ArrayList<>(currentStation.averageTimes()); Collections.sort(sortedList); int size = sortedList.size(); - return size % 2 == 0 ? (sortedList.get(size / 2 - 1) + sortedList.get(size / 2)) / 2D : (sortedList.get(size / 2)); + return size % 2 == 0 ? (sortedList.get(size / 2 - 1) + sortedList.get(size / 2)) / 2 : (sortedList.get(size / 2)); } } -// TODO: Possible problem: Might arrive to early at station -> Move backwards stations by 5 m +// TODO: Possible problem: Might arrive to early at station -> Move backwards stations by 5 meter From a762b44299e2396fb2b37d4c54960b627b5130a5 Mon Sep 17 00:00:00 2001 From: Topvennie Date: Sat, 20 Apr 2024 00:21:20 +0200 Subject: [PATCH 17/25] chore: smoothen out some more --- .../telraam/logic/positioner/Position.java | 1 + .../positioner/nostradamus/TeamData.java | 21 +++++++++++++------ .../station/websocket/WebsocketFetcher.java | 1 - 3 files changed, 16 insertions(+), 7 deletions(-) diff --git a/src/main/java/telraam/logic/positioner/Position.java b/src/main/java/telraam/logic/positioner/Position.java index 1a0e002..5914a97 100644 --- a/src/main/java/telraam/logic/positioner/Position.java +++ b/src/main/java/telraam/logic/positioner/Position.java @@ -23,5 +23,6 @@ public void update(double progress, double speed) { this.progress = progress; this.speed = speed; this.timestamp = System.currentTimeMillis(); + System.out.println("Progress: " + progress + " | Speed: " + speed + " | Timestamp: " + timestamp); } } diff --git a/src/main/java/telraam/logic/positioner/nostradamus/TeamData.java b/src/main/java/telraam/logic/positioner/nostradamus/TeamData.java index ba5f047..cdf9830 100644 --- a/src/main/java/telraam/logic/positioner/nostradamus/TeamData.java +++ b/src/main/java/telraam/logic/positioner/nostradamus/TeamData.java @@ -17,7 +17,7 @@ public class TeamData { private final int totalDistance; // Total distance of the track @Getter private final Position position; // Data to send to the websocket - private final int maxDeviance; // Maximum deviance the animation can have from the reality + private final double maxDeviance; // Maximum deviance the animation can have from the reality public TeamData(int teamId, int interval, List stations, int averageAmount, double sprintingSpeed, int finishOffset) { @@ -40,7 +40,7 @@ public TeamData(int teamId, int interval, List stations, int averageAmo this.previousStationArrival = System.currentTimeMillis(); this.currentStation = new StationData(new Station(-10), new Station(-9), new CircularQueue<>(0), -10, -10, -10); // Will never trigger `isNextStation` for the first station this.position = new Position(teamId); - this.maxDeviance = 1 / stations.size(); + this.maxDeviance = (double) 1 / stations.size(); } // Add a new detection @@ -52,11 +52,13 @@ public boolean addDetection(Detection e) { // Is at a new station that is in front of the previous one. previousStation = currentStation; currentStation = stations.get(e.getStationId()); + System.out.println("Station: " + currentStation.index()); + long now = System.currentTimeMillis(); if (isNextStation()) { // Only add the time if it goes forward by exactly one station - previousStation.averageTimes().add(System.currentTimeMillis() - previousStationArrival); + previousStation.averageTimes().add(now - previousStationArrival); } - previousStationArrival = System.currentTimeMillis(); + previousStationArrival = now; return true; } @@ -87,13 +89,20 @@ public void updatePosition() { double speed, progress; // Determine whether to speed up / slow down the animation or teleport it - double difference = (goalProgress - theoreticalProgress + 1) % 1; - if ((difference >= maxDeviance && difference <= 1 - maxDeviance) || previousStation.averageTimes().size() < 5) { + double difference = (currentStation.currentProgress() - theoreticalProgress + 1) % 1; + if ((difference >= maxDeviance && difference <= 1 - maxDeviance) || currentStation.averageTimes().size() < 3) { + System.out.println("Too fast"); + System.out.println("Size: " + currentStation.averageTimes().size()); + System.out.print("Average times: "); + currentStation.averageTimes().forEach(time -> System.out.print(" | " + time)); + System.out.println(""); + System.out.println("Goal: " + currentStation.currentProgress() + " Theoretical: " + theoreticalProgress + " Difference: " + (currentStation.currentProgress() - theoreticalProgress + 1) % 1); // Animation was too far behind or ahead so teleport progress = currentStation.currentProgress(); speed = ((currentStation.nextProgress() - progress + 1) % 1) / getMedian(); } else { // Animation is close enough, adjust so that we're synced at the next station + System.out.println("Good"); progress = theoreticalProgress; speed = ((goalProgress - theoreticalProgress + 1) % 1) / (nextStationArrival - currentTime); } diff --git a/src/main/java/telraam/station/websocket/WebsocketFetcher.java b/src/main/java/telraam/station/websocket/WebsocketFetcher.java index 24c78b4..62e9996 100644 --- a/src/main/java/telraam/station/websocket/WebsocketFetcher.java +++ b/src/main/java/telraam/station/websocket/WebsocketFetcher.java @@ -90,7 +90,6 @@ public void fetch() { return; } - WebsocketClient websocketClient = new WebsocketClient(url); websocketClient.addOnOpenHandler(() -> { websocketClient.sendMessage(wsMessageEncoded); From ec495ea6846d1b4f212b17007308947f3d775d55 Mon Sep 17 00:00:00 2001 From: Topvennie Date: Sat, 20 Apr 2024 14:23:19 +0200 Subject: [PATCH 18/25] chore; remove prints statements --- src/main/java/telraam/logic/positioner/Position.java | 1 - .../telraam/logic/positioner/nostradamus/TeamData.java | 8 -------- 2 files changed, 9 deletions(-) diff --git a/src/main/java/telraam/logic/positioner/Position.java b/src/main/java/telraam/logic/positioner/Position.java index 5914a97..1a0e002 100644 --- a/src/main/java/telraam/logic/positioner/Position.java +++ b/src/main/java/telraam/logic/positioner/Position.java @@ -23,6 +23,5 @@ public void update(double progress, double speed) { this.progress = progress; this.speed = speed; this.timestamp = System.currentTimeMillis(); - System.out.println("Progress: " + progress + " | Speed: " + speed + " | Timestamp: " + timestamp); } } diff --git a/src/main/java/telraam/logic/positioner/nostradamus/TeamData.java b/src/main/java/telraam/logic/positioner/nostradamus/TeamData.java index cdf9830..c173ea8 100644 --- a/src/main/java/telraam/logic/positioner/nostradamus/TeamData.java +++ b/src/main/java/telraam/logic/positioner/nostradamus/TeamData.java @@ -52,7 +52,6 @@ public boolean addDetection(Detection e) { // Is at a new station that is in front of the previous one. previousStation = currentStation; currentStation = stations.get(e.getStationId()); - System.out.println("Station: " + currentStation.index()); long now = System.currentTimeMillis(); if (isNextStation()) { // Only add the time if it goes forward by exactly one station @@ -91,18 +90,11 @@ public void updatePosition() { // Determine whether to speed up / slow down the animation or teleport it double difference = (currentStation.currentProgress() - theoreticalProgress + 1) % 1; if ((difference >= maxDeviance && difference <= 1 - maxDeviance) || currentStation.averageTimes().size() < 3) { - System.out.println("Too fast"); - System.out.println("Size: " + currentStation.averageTimes().size()); - System.out.print("Average times: "); - currentStation.averageTimes().forEach(time -> System.out.print(" | " + time)); - System.out.println(""); - System.out.println("Goal: " + currentStation.currentProgress() + " Theoretical: " + theoreticalProgress + " Difference: " + (currentStation.currentProgress() - theoreticalProgress + 1) % 1); // Animation was too far behind or ahead so teleport progress = currentStation.currentProgress(); speed = ((currentStation.nextProgress() - progress + 1) % 1) / getMedian(); } else { // Animation is close enough, adjust so that we're synced at the next station - System.out.println("Good"); progress = theoreticalProgress; speed = ((goalProgress - theoreticalProgress + 1) % 1) / (nextStationArrival - currentTime); } From 78f1ea511797967352f8773eef40c1ce54213fb3 Mon Sep 17 00:00:00 2001 From: Topvennie Date: Sat, 20 Apr 2024 15:54:49 +0200 Subject: [PATCH 19/25] chore: sync up faster --- src/main/java/telraam/api/TeamResource.java | 3 -- .../{PositionList.java => DetectionList.java} | 4 +-- .../positioner/nostradamus/Nostradamus.java | 6 ++-- .../positioner/nostradamus/StationData.java | 22 ++++++++++--- .../positioner/nostradamus/TeamData.java | 31 +++++++++---------- 5 files changed, 38 insertions(+), 28 deletions(-) rename src/main/java/telraam/logic/positioner/nostradamus/{PositionList.java => DetectionList.java} (94%) diff --git a/src/main/java/telraam/api/TeamResource.java b/src/main/java/telraam/api/TeamResource.java index 5983eff..9bf36c3 100644 --- a/src/main/java/telraam/api/TeamResource.java +++ b/src/main/java/telraam/api/TeamResource.java @@ -51,9 +51,6 @@ public Team update(Team team, Optional 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(), diff --git a/src/main/java/telraam/logic/positioner/nostradamus/PositionList.java b/src/main/java/telraam/logic/positioner/nostradamus/DetectionList.java similarity index 94% rename from src/main/java/telraam/logic/positioner/nostradamus/PositionList.java rename to src/main/java/telraam/logic/positioner/nostradamus/DetectionList.java index e1c8944..4e128a2 100644 --- a/src/main/java/telraam/logic/positioner/nostradamus/PositionList.java +++ b/src/main/java/telraam/logic/positioner/nostradamus/DetectionList.java @@ -9,7 +9,7 @@ import java.util.Comparator; import java.util.List; -public class PositionList extends ArrayList { +public class DetectionList extends ArrayList { private final int interval; private final List stations; @@ -17,7 +17,7 @@ public class PositionList extends ArrayList { private Detection currentPosition; private Timestamp newestDetection; - public PositionList(int interval, List stations) { + public DetectionList(int interval, List stations) { this.interval = interval; this.stations = stations.stream().sorted(Comparator.comparing(Station::getDistanceFromStart)).map(Station::getId).toList(); this.currentPosition = new Detection(-1, 0, -100); diff --git a/src/main/java/telraam/logic/positioner/nostradamus/Nostradamus.java b/src/main/java/telraam/logic/positioner/nostradamus/Nostradamus.java index 8f5004d..f2a2669 100644 --- a/src/main/java/telraam/logic/positioner/nostradamus/Nostradamus.java +++ b/src/main/java/telraam/logic/positioner/nostradamus/Nostradamus.java @@ -1,11 +1,13 @@ package telraam.logic.positioner.nostradamus; import org.jdbi.v3.core.Jdbi; -import telraam.database.daos.BatonDAO; import telraam.database.daos.BatonSwitchoverDAO; import telraam.database.daos.StationDAO; import telraam.database.daos.TeamDAO; -import telraam.database.models.*; +import telraam.database.models.BatonSwitchover; +import telraam.database.models.Detection; +import telraam.database.models.Station; +import telraam.database.models.Team; import telraam.logic.positioner.PositionSender; import telraam.logic.positioner.Positioner; diff --git a/src/main/java/telraam/logic/positioner/nostradamus/StationData.java b/src/main/java/telraam/logic/positioner/nostradamus/StationData.java index e65101d..c3b3537 100644 --- a/src/main/java/telraam/logic/positioner/nostradamus/StationData.java +++ b/src/main/java/telraam/logic/positioner/nostradamus/StationData.java @@ -2,24 +2,36 @@ import telraam.database.models.Station; +import java.util.ArrayList; import java.util.List; public record StationData( Station station, // The station Station nextStation, // The next station - List averageTimes, // List containing the times (in ms) that was needed to run from this station to the next one. + List 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 - double currentProgress, // The progress value of this station - double nextProgress // The progress value of the next station + 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 stations, int index, int averageAmount, int totalDistance) { this( stations.get(index), stations.get((index + 1) % stations.size()), new CircularQueue<>(averageAmount), index, - stations.get(index).getDistanceFromStart() / totalDistance, - stations.get((index + 1) % stations.size()).getDistanceFromStart() / totalDistance + (float) (stations.get(index).getDistanceFromStart() / totalDistance), + (float) (stations.get((index + 1) % stations.size()).getDistanceFromStart() / totalDistance) ); } } diff --git a/src/main/java/telraam/logic/positioner/nostradamus/TeamData.java b/src/main/java/telraam/logic/positioner/nostradamus/TeamData.java index c173ea8..df2e844 100644 --- a/src/main/java/telraam/logic/positioner/nostradamus/TeamData.java +++ b/src/main/java/telraam/logic/positioner/nostradamus/TeamData.java @@ -9,7 +9,7 @@ import java.util.stream.Collectors; public class TeamData { - private final PositionList detections; // List with all relevant detections + private final DetectionList detections; // List with all relevant detections private final Map stations; // Station list private long previousStationArrival; // Arrival time of previous station. Used to calculate the average times private StationData currentStation; // Current station location @@ -17,7 +17,7 @@ public class TeamData { private final int totalDistance; // Total distance of the track @Getter private final Position position; // Data to send to the websocket - private final double maxDeviance; // Maximum deviance the animation can have from the reality + private final float maxDeviance; // Maximum deviance the animation can have from the reality public TeamData(int teamId, int interval, List stations, int averageAmount, double sprintingSpeed, int finishOffset) { @@ -33,18 +33,18 @@ public TeamData(int teamId, int interval, List stations, int averageAmo ) )); // Pre-populate with some data - this.stations.forEach((stationId, stationData) -> stationData.averageTimes().add( + this.stations.forEach((stationId, stationData) -> stationData.times().add( (long) (((stationData.nextStation().getDistanceFromStart() - stationData.station().getDistanceFromStart() + totalDistance) % totalDistance) / sprintingSpeed) )); - this.detections = new PositionList(interval, stations); + this.detections = new DetectionList(interval, stations); this.previousStationArrival = System.currentTimeMillis(); - this.currentStation = new StationData(new Station(-10), new Station(-9), new CircularQueue<>(0), -10, -10, -10); // Will never trigger `isNextStation` for the first station + this.currentStation = new StationData(); // Will never trigger `isNextStation` for the first station this.position = new Position(teamId); - this.maxDeviance = (double) 1 / stations.size(); + this.maxDeviance = (float) 1 / stations.size(); } // Add a new detection - // Returns true if the team is at a next station + // Returns true if the team is at a new station public boolean addDetection(Detection e) { boolean newStation = detections.add(e); @@ -55,7 +55,7 @@ public boolean addDetection(Detection e) { long now = System.currentTimeMillis(); if (isNextStation()) { // Only add the time if it goes forward by exactly one station - previousStation.averageTimes().add(now - previousStationArrival); + previousStation.times().add(now - previousStationArrival); } previousStationArrival = now; @@ -67,7 +67,7 @@ public boolean addDetection(Detection e) { private boolean isForwardStation(int oldStation, int newStation) { int stationDiff = (newStation - oldStation + stations.size()) % stations.size(); - return stationDiff > 0 && stationDiff < 3; + return stationDiff < 3; } private boolean isNextStation() { @@ -83,16 +83,17 @@ public void updatePosition() { double theoreticalProgress = (position.getProgress() + (position.getSpeed() * milliSecondsSince)) % 1; // Arrive at next station at timestamp y and progress z - long nextStationArrival = currentTime + getMedian(); + double median = getMedian(currentStation.times()); + double nextStationArrival = currentTime + median; double goalProgress = currentStation.nextProgress(); double speed, progress; // Determine whether to speed up / slow down the animation or teleport it double difference = (currentStation.currentProgress() - theoreticalProgress + 1) % 1; - if ((difference >= maxDeviance && difference <= 1 - maxDeviance) || currentStation.averageTimes().size() < 3) { + if ((difference >= maxDeviance && difference <= 1 - maxDeviance)) { // Animation was too far behind or ahead so teleport progress = currentStation.currentProgress(); - speed = ((currentStation.nextProgress() - progress + 1) % 1) / getMedian(); + speed = ((currentStation.nextProgress() - progress + 1) % 1) / median; } else { // Animation is close enough, adjust so that we're synced at the next station progress = theoreticalProgress; @@ -103,13 +104,11 @@ public void updatePosition() { } // Get the medium of the average times - private long getMedian() { - List sortedList = new ArrayList<>(currentStation.averageTimes()); + private long getMedian(List times) { + List sortedList = new ArrayList<>(times); Collections.sort(sortedList); int size = sortedList.size(); return size % 2 == 0 ? (sortedList.get(size / 2 - 1) + sortedList.get(size / 2)) / 2 : (sortedList.get(size / 2)); } } - -// TODO: Possible problem: Might arrive to early at station -> Move backwards stations by 5 meter From 5a882fc5033991f769c97160f73d49bde5b5ff7d Mon Sep 17 00:00:00 2001 From: Topvennie Date: Sat, 20 Apr 2024 20:13:55 +0200 Subject: [PATCH 20/25] chore: Send speed 0 when no data is received --- .../java/telraam/logic/positioner/Position.java | 17 ++++++++++++----- .../positioner/nostradamus/Nostradamus.java | 15 ++++++++++++++- .../logic/positioner/nostradamus/TeamData.java | 15 ++++++++++----- 3 files changed, 36 insertions(+), 11 deletions(-) diff --git a/src/main/java/telraam/logic/positioner/Position.java b/src/main/java/telraam/logic/positioner/Position.java index 1a0e002..43f8e26 100644 --- a/src/main/java/telraam/logic/positioner/Position.java +++ b/src/main/java/telraam/logic/positioner/Position.java @@ -9,19 +9,26 @@ public class Position { private final int teamId; private double progress; // Progress of the lap. Between 0-1 - private double speed; // Current speed. progress / second - private long timestamp; + 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 = 0; + this.timestamp = System.currentTimeMillis(); } - public void update(double progress, double speed) { + public Position(int teamId, double progress) { + this.teamId = teamId; this.progress = progress; - this.speed = speed; + 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; + } } diff --git a/src/main/java/telraam/logic/positioner/nostradamus/Nostradamus.java b/src/main/java/telraam/logic/positioner/nostradamus/Nostradamus.java index f2a2669..325368e 100644 --- a/src/main/java/telraam/logic/positioner/nostradamus/Nostradamus.java +++ b/src/main/java/telraam/logic/positioner/nostradamus/Nostradamus.java @@ -8,6 +8,7 @@ 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; @@ -26,6 +27,7 @@ public class Nostradamus implements Positioner { private final double AVERAGE_SPRINTING_SPEED_M_MS = 0.00684; // Average sprinting speed m / ms private final int MIN_RSSI = -84; private final int FINISH_OFFSET = 0; + private final int MAX_NO_DATA_MS = 30000; private final Jdbi jdbi; private final List newDetections; // Contains not yet handled detections private final Lock detectionLock; @@ -102,6 +104,7 @@ private void calculatePosition() { } newDetections.clear(); detectionLock.unlock(); // Use lock as short as possible + dataLock.unlock(); if (!changedTeams.isEmpty()) { // Update @@ -115,7 +118,17 @@ private void calculatePosition() { ); } - dataLock.unlock(); + long now = System.currentTimeMillis(); + for (Map.Entry entry: teamData.entrySet()) { + if (now - entry.getValue().getPreviousStationArrival() > MAX_NO_DATA_MS) { + positionSender.send( + Collections.singletonList(new Position( + entry.getKey(), + entry.getValue().getPosition().getProgress() + )) + ); + } + } // zzzzzzzz try { diff --git a/src/main/java/telraam/logic/positioner/nostradamus/TeamData.java b/src/main/java/telraam/logic/positioner/nostradamus/TeamData.java index df2e844..f98dc9b 100644 --- a/src/main/java/telraam/logic/positioner/nostradamus/TeamData.java +++ b/src/main/java/telraam/logic/positioner/nostradamus/TeamData.java @@ -11,6 +11,7 @@ public class TeamData { private final DetectionList detections; // List with all relevant detections private final Map stations; // Station list + @Getter private long previousStationArrival; // Arrival time of previous station. Used to calculate the average times private StationData currentStation; // Current station location private StationData previousStation; // Previous station location @@ -74,13 +75,17 @@ private boolean isNextStation() { return Objects.equals(previousStation.nextStation().getId(), currentStation.station().getId()); } + private double normalize(double number) { + return (number + 1) % 1; + } + // Update the position data public void updatePosition() { long currentTime = System.currentTimeMillis(); // Animation is currently at progress x long milliSecondsSince = currentTime - position.getTimestamp(); - double theoreticalProgress = (position.getProgress() + (position.getSpeed() * milliSecondsSince)) % 1; + double theoreticalProgress = normalize(position.getProgress() + (position.getSpeed() * milliSecondsSince)); // Arrive at next station at timestamp y and progress z double median = getMedian(currentStation.times()); @@ -89,18 +94,18 @@ public void updatePosition() { double speed, progress; // Determine whether to speed up / slow down the animation or teleport it - double difference = (currentStation.currentProgress() - theoreticalProgress + 1) % 1; + double difference = normalize(currentStation.currentProgress() - theoreticalProgress); if ((difference >= maxDeviance && difference <= 1 - maxDeviance)) { // Animation was too far behind or ahead so teleport progress = currentStation.currentProgress(); - speed = ((currentStation.nextProgress() - progress + 1) % 1) / median; + speed = normalize(currentStation.nextProgress() - progress) / median; } else { // Animation is close enough, adjust so that we're synced at the next station progress = theoreticalProgress; - speed = ((goalProgress - theoreticalProgress + 1) % 1) / (nextStationArrival - currentTime); + speed = normalize(goalProgress - theoreticalProgress) / (nextStationArrival - currentTime); } - position.update(progress, speed); + position.update(progress, speed, currentTime); } // Get the medium of the average times From 6803c63fc8965d316034fe4e82ca943844740e8b Mon Sep 17 00:00:00 2001 From: Topvennie Date: Sat, 20 Apr 2024 20:33:39 +0200 Subject: [PATCH 21/25] Docs: Added comments --- .../positioner/nostradamus/CircularQueue.java | 1 + .../positioner/nostradamus/DetectionList.java | 2 +- .../positioner/nostradamus/Nostradamus.java | 38 ++++++++++--------- .../positioner/nostradamus/StationData.java | 1 + .../positioner/nostradamus/TeamData.java | 14 +++---- 5 files changed, 31 insertions(+), 25 deletions(-) diff --git a/src/main/java/telraam/logic/positioner/nostradamus/CircularQueue.java b/src/main/java/telraam/logic/positioner/nostradamus/CircularQueue.java index 997e9bf..1b2faae 100644 --- a/src/main/java/telraam/logic/positioner/nostradamus/CircularQueue.java +++ b/src/main/java/telraam/logic/positioner/nostradamus/CircularQueue.java @@ -2,6 +2,7 @@ import java.util.LinkedList; +// LinkedList with a maximum length public class CircularQueue extends LinkedList { private final int maxSize; diff --git a/src/main/java/telraam/logic/positioner/nostradamus/DetectionList.java b/src/main/java/telraam/logic/positioner/nostradamus/DetectionList.java index 4e128a2..30d5848 100644 --- a/src/main/java/telraam/logic/positioner/nostradamus/DetectionList.java +++ b/src/main/java/telraam/logic/positioner/nostradamus/DetectionList.java @@ -24,7 +24,7 @@ public DetectionList(int interval, List stations) { this.newestDetection = new Timestamp(0); } - // Returns True if it's a new station + // Returns True if the added detection results in a new station @Override public boolean add(Detection e) { super.add(e); diff --git a/src/main/java/telraam/logic/positioner/nostradamus/Nostradamus.java b/src/main/java/telraam/logic/positioner/nostradamus/Nostradamus.java index 325368e..4b503b3 100644 --- a/src/main/java/telraam/logic/positioner/nostradamus/Nostradamus.java +++ b/src/main/java/telraam/logic/positioner/nostradamus/Nostradamus.java @@ -20,21 +20,21 @@ 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 - private final int INTERVAL_FETCH_MS = 60000; // Interval between fetching all stations, teams, ... + private final int INTERVAL_CALCULATE_MS = 500; // How often to handle new detections (in milliseconds) + private final int INTERVAL_FETCH_MS = 60000; // Interval between fetching baton switchovers (in milliseconds) private final int INTERVAL_DETECTIONS_MS = 3000; // Amount of milliseconds to group detections by - private final int AVERAGE_AMOUNT = 10; // Calculate the average running speed the last x intervals - private final double AVERAGE_SPRINTING_SPEED_M_MS = 0.00684; // Average sprinting speed m / ms - private final int MIN_RSSI = -84; - private final int FINISH_OFFSET = 0; - private final int MAX_NO_DATA_MS = 30000; + 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 AVERAGE_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 newDetections; // Contains not yet handled detections - private final Lock detectionLock; - private final Lock dataLock; private Map batonToTeam; // Baton ID to Team ID - private Map teamData; // All team data + private final Map teamData; // All team data private final PositionSender positionSender; + private final Lock detectionLock; + private final Lock dataLock; public Nostradamus(Jdbi jdbi) { this.jdbi = jdbi; @@ -44,7 +44,7 @@ public Nostradamus(Jdbi jdbi) { // Will be filled by fetch this.batonToTeam = new HashMap<>(); - setTeamData(); + this.teamData = getTeamData(); this.positionSender = new PositionSender(); @@ -52,17 +52,19 @@ public Nostradamus(Jdbi jdbi) { new Thread(this::calculatePosition).start(); } - private void setTeamData() { + // Initiate the team data map + private Map getTeamData() { List stations = jdbi.onDemand(StationDAO.class).getAll(); stations.sort(Comparator.comparing(Station::getDistanceFromStart)); List teams = jdbi.onDemand(TeamDAO.class).getAll(); - teamData = teams.stream().collect(Collectors.toMap( + return teams.stream().collect(Collectors.toMap( Team::getId, - team -> new TeamData(team.getId(), INTERVAL_DETECTIONS_MS, stations, AVERAGE_AMOUNT, AVERAGE_SPRINTING_SPEED_M_MS, FINISH_OFFSET) + team -> new TeamData(team.getId(), INTERVAL_DETECTIONS_MS, stations, AVERAGE_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 switchovers = jdbi.onDemand(BatonSwitchoverDAO.class).getAll(); @@ -80,7 +82,7 @@ private void fetch() { dataLock.unlock(); } - // zzzzzzzz + // Sleep tight try { Thread.sleep(INTERVAL_FETCH_MS); } catch (InterruptedException e) { @@ -88,11 +90,12 @@ private void fetch() { } } + // handle all new detections and update positions accordingly private void calculatePosition() { Set changedTeams = new HashSet<>(); // List of teams that have changed station while (true) { - dataLock.lock(); changedTeams.clear(); + dataLock.lock(); detectionLock.lock(); for (Detection detection: newDetections) { if (batonToTeam.containsKey(detection.getBatonId())) { @@ -118,6 +121,7 @@ private void calculatePosition() { ); } + // Send a stationary position if no new station data was received recently long now = System.currentTimeMillis(); for (Map.Entry entry: teamData.entrySet()) { if (now - entry.getValue().getPreviousStationArrival() > MAX_NO_DATA_MS) { @@ -130,7 +134,7 @@ private void calculatePosition() { } } - // zzzzzzzz + // Goodnight try { Thread.sleep(INTERVAL_CALCULATE_MS); } catch (InterruptedException e) { diff --git a/src/main/java/telraam/logic/positioner/nostradamus/StationData.java b/src/main/java/telraam/logic/positioner/nostradamus/StationData.java index c3b3537..fbc9d8d 100644 --- a/src/main/java/telraam/logic/positioner/nostradamus/StationData.java +++ b/src/main/java/telraam/logic/positioner/nostradamus/StationData.java @@ -5,6 +5,7 @@ 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 diff --git a/src/main/java/telraam/logic/positioner/nostradamus/TeamData.java b/src/main/java/telraam/logic/positioner/nostradamus/TeamData.java index f98dc9b..6fba92d 100644 --- a/src/main/java/telraam/logic/positioner/nostradamus/TeamData.java +++ b/src/main/java/telraam/logic/positioner/nostradamus/TeamData.java @@ -11,14 +11,14 @@ public class TeamData { private final DetectionList detections; // List with all relevant detections private final Map stations; // Station list - @Getter - private long previousStationArrival; // Arrival time of previous station. Used to calculate the average times private StationData currentStation; // Current station location private StationData previousStation; // Previous station location + @Getter + private long previousStationArrival; // Arrival time of previous station. Used to calculate the average times private final int totalDistance; // Total distance of the track + private final float maxDeviance; // Maximum deviance the animation can have from the reality @Getter private final Position position; // Data to send to the websocket - private final float maxDeviance; // Maximum deviance the animation can have from the reality public TeamData(int teamId, int interval, List stations, int averageAmount, double sprintingSpeed, int finishOffset) { @@ -33,7 +33,7 @@ public TeamData(int teamId, int interval, List stations, int averageAmo totalDistance ) )); - // Pre-populate with some data + // Pre-populate with some default values this.stations.forEach((stationId, stationData) -> stationData.times().add( (long) (((stationData.nextStation().getDistanceFromStart() - stationData.station().getDistanceFromStart() + totalDistance) % totalDistance) / sprintingSpeed) )); @@ -88,7 +88,7 @@ public void updatePosition() { double theoreticalProgress = normalize(position.getProgress() + (position.getSpeed() * milliSecondsSince)); // Arrive at next station at timestamp y and progress z - double median = getMedian(currentStation.times()); + double median = getMedian(); double nextStationArrival = currentTime + median; double goalProgress = currentStation.nextProgress(); @@ -109,8 +109,8 @@ public void updatePosition() { } // Get the medium of the average times - private long getMedian(List times) { - List sortedList = new ArrayList<>(times); + private long getMedian() { + List sortedList = new ArrayList<>(currentStation.times()); Collections.sort(sortedList); int size = sortedList.size(); From f23c84cfab24b1f17cf7760839ff94fc785399ef Mon Sep 17 00:00:00 2001 From: Topvennie Date: Tue, 23 Apr 2024 15:08:39 +0200 Subject: [PATCH 22/25] deleted simplepositioner --- .../positioner/simple/CircularQueue.java | 21 ---- .../positioner/simple/SimplePositioner.java | 102 ------------------ 2 files changed, 123 deletions(-) delete mode 100644 src/main/java/telraam/logic/positioner/simple/CircularQueue.java delete mode 100644 src/main/java/telraam/logic/positioner/simple/SimplePositioner.java diff --git a/src/main/java/telraam/logic/positioner/simple/CircularQueue.java b/src/main/java/telraam/logic/positioner/simple/CircularQueue.java deleted file mode 100644 index ae101ed..0000000 --- a/src/main/java/telraam/logic/positioner/simple/CircularQueue.java +++ /dev/null @@ -1,21 +0,0 @@ -package telraam.logic.positioner.simple; - -import java.util.LinkedList; - -public class CircularQueue extends LinkedList { - - private final int maxSize; - public CircularQueue(int maxSize) { - this.maxSize = maxSize; - } - - @Override - public boolean add(T e) { - if (size() >= this.maxSize) { - removeFirst(); - } - - return super.add(e); - } - -} diff --git a/src/main/java/telraam/logic/positioner/simple/SimplePositioner.java b/src/main/java/telraam/logic/positioner/simple/SimplePositioner.java deleted file mode 100644 index fa6a395..0000000 --- a/src/main/java/telraam/logic/positioner/simple/SimplePositioner.java +++ /dev/null @@ -1,102 +0,0 @@ -package telraam.logic.positioner.simple; - -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.Executors; -import java.util.concurrent.ScheduledExecutorService; -import java.util.concurrent.TimeUnit; -import java.util.concurrent.locks.Lock; -import java.util.concurrent.locks.ReentrantLock; -import java.util.logging.Level; -import java.util.logging.Logger; - -public class SimplePositioner implements Positioner { - private static final Logger logger = Logger.getLogger(SimplePositioner.class.getName()); - private final int QUEUE_SIZE = 50; - private final int MIN_RSSI = -85; - private final int DEBOUNCE_TIMEOUT = 1; - private boolean debounceScheduled; - private final ScheduledExecutorService scheduler; - private final PositionSender positionSender; - private final Map batonIdToTeam; - private final Map> teamDetections; - private final List stations; - private final Map teamPositions; - - public SimplePositioner(Jdbi jdbi) { - this.debounceScheduled = false; - this.scheduler = Executors.newScheduledThreadPool(1); - this.positionSender = new PositionSender(); - this.batonIdToTeam = new HashMap<>(); - this.teamDetections = new HashMap<>(); - this.teamPositions = new HashMap<>(); - - TeamDAO teamDAO = jdbi.onDemand(TeamDAO.class); - List teams = teamDAO.getAll(); - for (Team team : teams) { - teamDetections.put(team.getId(), new CircularQueue<>(QUEUE_SIZE)); - teamPositions.put(team.getId(), new Position(team.getId())); - } - List switchovers = jdbi.onDemand(BatonSwitchoverDAO.class).getAll(); - switchovers.sort(Comparator.comparing(BatonSwitchover::getTimestamp)); - - for (BatonSwitchover switchover : switchovers) { - batonIdToTeam.put(switchover.getNewBatonId(), teamDAO.getById(switchover.getTeamId()).get()); - } - - List stationList = jdbi.onDemand(StationDAO.class).getAll(); - stationList.sort(Comparator.comparing(Station::getDistanceFromStart)); - stations = stationList.stream().map(Station::getId).toList(); - } - - private void calculatePositions() { - logger.info("SimplePositioner: Calculating positions..."); - for (Map.Entry> entry : teamDetections.entrySet()) { - List detections = teamDetections.get(entry.getKey()); - detections.sort(Comparator.comparing(Detection::getTimestamp)); - - int currentStationRssi = MIN_RSSI; - int currentStationPosition = 0; - for (Detection detection : detections) { - if (detection.getRssi() > currentStationRssi) { - currentStationRssi = detection.getRssi(); - currentStationPosition = detection.getStationId(); - } - } - - float progress = ((float) 100 / stations.size()) * currentStationPosition; - teamPositions.get(entry.getKey()).setProgress(progress); - } - - positionSender.send(teamPositions.values().stream().toList()); - logger.info("SimplePositioner: Done calculating positions"); - } - - public synchronized void handle(Detection detection) { - Team team = batonIdToTeam.get(detection.getBatonId()); - teamDetections.get(team.getId()).add(detection); - - if (!debounceScheduled) { - debounceScheduled = true; - scheduler.schedule(() -> { - try { - calculatePositions(); - } catch (Exception e) { - logger.log(Level.SEVERE, e.getMessage(), e); - } - debounceScheduled = false; - }, DEBOUNCE_TIMEOUT, TimeUnit.SECONDS); - } - } -} From 74adeb20f70845c3dbacd4a0a4d3f19bee0f172a Mon Sep 17 00:00:00 2001 From: Topvennie Date: Tue, 23 Apr 2024 15:09:45 +0200 Subject: [PATCH 23/25] renamed the only reference left to average as it somehow still causes confusion --- .../telraam/logic/positioner/nostradamus/Nostradamus.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/telraam/logic/positioner/nostradamus/Nostradamus.java b/src/main/java/telraam/logic/positioner/nostradamus/Nostradamus.java index 4b503b3..7cbab86 100644 --- a/src/main/java/telraam/logic/positioner/nostradamus/Nostradamus.java +++ b/src/main/java/telraam/logic/positioner/nostradamus/Nostradamus.java @@ -24,7 +24,7 @@ public class Nostradamus implements Positioner { private final int INTERVAL_FETCH_MS = 60000; // 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 AVERAGE_AMOUNT = 10; // Calculate the median running speed of the last x intervals + 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 @@ -60,7 +60,7 @@ private Map getTeamData() { return teams.stream().collect(Collectors.toMap( Team::getId, - team -> new TeamData(team.getId(), INTERVAL_DETECTIONS_MS, stations, AVERAGE_AMOUNT, AVERAGE_SPRINTING_SPEED_M_MS, FINISH_OFFSET_M) + team -> new TeamData(team.getId(), INTERVAL_DETECTIONS_MS, stations, MEDIAN_AMOUNT, AVERAGE_SPRINTING_SPEED_M_MS, FINISH_OFFSET_M) )); } From 44ea73fa262c0f4069a32b5597fdf8ce7666d69d Mon Sep 17 00:00:00 2001 From: Topvennie Date: Tue, 23 Apr 2024 15:11:05 +0200 Subject: [PATCH 24/25] shorten fetch interval --- .../java/telraam/logic/positioner/nostradamus/Nostradamus.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/telraam/logic/positioner/nostradamus/Nostradamus.java b/src/main/java/telraam/logic/positioner/nostradamus/Nostradamus.java index 7cbab86..558ac38 100644 --- a/src/main/java/telraam/logic/positioner/nostradamus/Nostradamus.java +++ b/src/main/java/telraam/logic/positioner/nostradamus/Nostradamus.java @@ -21,7 +21,7 @@ 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 = 60000; // Interval between fetching baton switchovers (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 From bdaa067d4c710c1d1e77207a2d7b760c607961fd Mon Sep 17 00:00:00 2001 From: Topvennie Date: Tue, 23 Apr 2024 15:29:36 +0200 Subject: [PATCH 25/25] removed simplepositioner --- src/main/java/telraam/App.java | 18 +++++++----------- 1 file changed, 7 insertions(+), 11 deletions(-) diff --git a/src/main/java/telraam/App.java b/src/main/java/telraam/App.java index 0d79b23..4f4348c 100644 --- a/src/main/java/telraam/App.java +++ b/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; @@ -25,13 +25,10 @@ import telraam.logic.lapper.slapper.Slapper; import telraam.logic.positioner.Positioner; import telraam.logic.positioner.nostradamus.Nostradamus; -import telraam.logic.positioner.simple.SimplePositioner; 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; @@ -93,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 @@ -143,7 +140,6 @@ public void run(AppConfiguration configuration, Environment environment) { // Set up positioners Set positioners = new HashSet<>(); -// positioners.add(new SimplePositioner(this.database)); positioners.add(new Nostradamus(this.database)); // Start fetch thread for each station