diff --git a/src/main/java/telraam/App.java b/src/main/java/telraam/App.java index 859270c..7b8cc07 100644 --- a/src/main/java/telraam/App.java +++ b/src/main/java/telraam/App.java @@ -24,7 +24,9 @@ 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.Stationary.Stationary; +import telraam.logic.positioner.nostradamus.v2.Nostradamus; +import telraam.logic.positioner.nostradamus.v1.NostradamusV1; import telraam.station.FetcherFactory; import telraam.util.AcceptedLapsUtil; import telraam.websocket.WebSocketConnection; @@ -141,7 +143,9 @@ public void run(AppConfiguration configuration, Environment environment) { // Set up positioners Set positioners = new HashSet<>(); - positioners.add(new Nostradamus(this.database)); + positioners.add(new Stationary(this.database)); + positioners.add(new NostradamusV1(this.database)); + positioners.add(new Nostradamus(configuration, this.database)); // Start fetch thread for each station FetcherFactory fetcherFactory = new FetcherFactory(this.database, lappers, positioners); diff --git a/src/main/java/telraam/AppConfiguration.java b/src/main/java/telraam/AppConfiguration.java index c338c65..9648e63 100644 --- a/src/main/java/telraam/AppConfiguration.java +++ b/src/main/java/telraam/AppConfiguration.java @@ -8,7 +8,6 @@ import jakarta.validation.constraints.NotNull; import lombok.Getter; import lombok.Setter; -import telraam.api.responses.Template; public class AppConfiguration extends Configuration { @NotNull @@ -27,4 +26,9 @@ public class AppConfiguration extends Configuration { @Getter @Setter @JsonProperty("database") private DataSourceFactory dataSourceFactory = new DataSourceFactory(); + + @NotNull + @Getter + @JsonProperty("finish_offset") + private int finishOffset; } diff --git a/src/main/java/telraam/database/models/Detection.java b/src/main/java/telraam/database/models/Detection.java index 4950feb..c00264b 100644 --- a/src/main/java/telraam/database/models/Detection.java +++ b/src/main/java/telraam/database/models/Detection.java @@ -37,4 +37,11 @@ public Detection(Integer id, Integer stationId, Integer rssi) { this.stationId = stationId; this.rssi = rssi; } + + public Detection(Integer id, Integer stationId, Integer rssi, Timestamp timestamp) { + this.id = id; + this.stationId = stationId; + this.rssi = rssi; + this.timestamp = timestamp; + } } diff --git a/src/main/java/telraam/logic/positioner/Position.java b/src/main/java/telraam/logic/positioner/Position.java index 43f8e26..28a4e37 100644 --- a/src/main/java/telraam/logic/positioner/Position.java +++ b/src/main/java/telraam/logic/positioner/Position.java @@ -5,30 +5,10 @@ import lombok.Getter; import lombok.Setter; -@Getter @Setter @JsonNaming(PropertyNamingStrategies.SnakeCaseStrategy.class) -public class Position { - private final int teamId; - private double progress; // Progress of the lap. Between 0-1 - private double speed; // Current speed. progress / millisecond - private long timestamp; // Timestamp in milliseconds - - public Position(int teamId) { - this.teamId = teamId; - this.progress = 0; - this.speed = 0; - this.timestamp = System.currentTimeMillis(); - } - - public Position(int teamId, double progress) { - this.teamId = teamId; - this.progress = progress; - this.speed = 0; - this.timestamp = System.currentTimeMillis(); - } - - public void update(double progress, double speed, long timestamp) { - this.progress = progress; - this.speed = speed; - this.timestamp = timestamp; - } -} +@JsonNaming(PropertyNamingStrategies.SnakeCaseStrategy.class) +public record Position ( + int teamId, + double progress, + double speed, + long timestamp +) {} diff --git a/src/main/java/telraam/logic/positioner/Stationary/Stationary.java b/src/main/java/telraam/logic/positioner/Stationary/Stationary.java new file mode 100644 index 0000000..e34c8ef --- /dev/null +++ b/src/main/java/telraam/logic/positioner/Stationary/Stationary.java @@ -0,0 +1,54 @@ +package telraam.logic.positioner.Stationary; + +import org.jdbi.v3.core.Jdbi; +import telraam.database.daos.PositionSourceDAO; +import telraam.database.daos.TeamDAO; +import telraam.database.models.Detection; +import telraam.database.models.PositionSource; +import telraam.database.models.Team; +import telraam.logic.positioner.Position; +import telraam.logic.positioner.PositionSender; +import telraam.logic.positioner.Positioner; + +import java.util.List; +import java.util.logging.Logger; + +public class Stationary implements Positioner { + private static final Logger logger = Logger.getLogger(Stationary.class.getName()); + private final String SOURCE_NAME = "stationary"; + private final int INTERVAL_UPDATE_MS = 60000; + private final Jdbi jdbi; + private final PositionSender positionSender; + public Stationary(Jdbi jdbi) { + this.jdbi = jdbi; + this.positionSender = new PositionSender(SOURCE_NAME); + + // Add as source + PositionSourceDAO positionSourceDAO = jdbi.onDemand(PositionSourceDAO.class); + if (positionSourceDAO.getByName(SOURCE_NAME).isEmpty()) { + positionSourceDAO.insert(new PositionSource(SOURCE_NAME)); + } + + new Thread(this::update).start(); + } + + private void update() { + // Keep sending updates in case Loxsi ever restarts + while (true) { + long timestamp = System.currentTimeMillis(); + List teams = jdbi.onDemand(TeamDAO.class).getAll(); + + List positions = teams.stream().map(t -> new Position(t.getId(), 0, 0, timestamp)).toList(); + positionSender.send(positions); + + try { + Thread.sleep(INTERVAL_UPDATE_MS); + } catch (InterruptedException e) { + logger.severe(e.getMessage()); + } + } + } + + @Override + public void handle(Detection detection) {} +} diff --git a/src/main/java/telraam/logic/positioner/nostradamus/CircularQueue.java b/src/main/java/telraam/logic/positioner/nostradamus/v1/CircularQueueV1.java similarity index 66% rename from src/main/java/telraam/logic/positioner/nostradamus/CircularQueue.java rename to src/main/java/telraam/logic/positioner/nostradamus/v1/CircularQueueV1.java index 1b2faae..95889cc 100644 --- a/src/main/java/telraam/logic/positioner/nostradamus/CircularQueue.java +++ b/src/main/java/telraam/logic/positioner/nostradamus/v1/CircularQueueV1.java @@ -1,12 +1,12 @@ -package telraam.logic.positioner.nostradamus; +package telraam.logic.positioner.nostradamus.v1; import java.util.LinkedList; // LinkedList with a maximum length -public class CircularQueue extends LinkedList { +public class CircularQueueV1 extends LinkedList { private final int maxSize; - public CircularQueue(int maxSize) { + public CircularQueueV1(int maxSize) { this.maxSize = maxSize; } diff --git a/src/main/java/telraam/logic/positioner/nostradamus/DetectionList.java b/src/main/java/telraam/logic/positioner/nostradamus/v1/DetectionListV1.java similarity index 92% rename from src/main/java/telraam/logic/positioner/nostradamus/DetectionList.java rename to src/main/java/telraam/logic/positioner/nostradamus/v1/DetectionListV1.java index 30d5848..db7e0f6 100644 --- a/src/main/java/telraam/logic/positioner/nostradamus/DetectionList.java +++ b/src/main/java/telraam/logic/positioner/nostradamus/v1/DetectionListV1.java @@ -1,4 +1,4 @@ -package telraam.logic.positioner.nostradamus; +package telraam.logic.positioner.nostradamus.v1; import lombok.Getter; import telraam.database.models.Detection; @@ -9,7 +9,7 @@ import java.util.Comparator; import java.util.List; -public class DetectionList extends ArrayList { +public class DetectionListV1 extends ArrayList { private final int interval; private final List stations; @@ -17,7 +17,7 @@ public class DetectionList extends ArrayList { private Detection currentPosition; private Timestamp newestDetection; - public DetectionList(int interval, List stations) { + public DetectionListV1(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/v1/NostradamusV1.java similarity index 89% rename from src/main/java/telraam/logic/positioner/nostradamus/Nostradamus.java rename to src/main/java/telraam/logic/positioner/nostradamus/v1/NostradamusV1.java index 2fa7a85..d8b875c 100644 --- a/src/main/java/telraam/logic/positioner/nostradamus/Nostradamus.java +++ b/src/main/java/telraam/logic/positioner/nostradamus/v1/NostradamusV1.java @@ -1,4 +1,4 @@ -package telraam.logic.positioner.nostradamus; +package telraam.logic.positioner.nostradamus.v1; import org.jdbi.v3.core.Jdbi; import telraam.database.daos.BatonSwitchoverDAO; @@ -16,9 +16,9 @@ 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 String SOURCE_NAME = "nostradamus"; +public class NostradamusV1 implements Positioner { + private static final Logger logger = Logger.getLogger(NostradamusV1.class.getName()); + private final String SOURCE_NAME = "nostradamus_v1"; private final int INTERVAL_CALCULATE_MS = 500; // How often to handle new detections (in milliseconds) private final int INTERVAL_FETCH_MS = 10000; // Interval between fetching baton switchovers (in milliseconds) private final int INTERVAL_DETECTIONS_MS = 3000; // Amount of milliseconds to group detections by @@ -30,12 +30,12 @@ public class Nostradamus implements Positioner { private final Jdbi jdbi; private final List newDetections; // Contains not yet handled detections private Map batonToTeam; // Baton ID to Team ID - private final 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) { + public NostradamusV1(Jdbi jdbi) { this.jdbi = jdbi; PositionSourceDAO positionSourceDAO = jdbi.onDemand(PositionSourceDAO.class); @@ -58,14 +58,14 @@ public Nostradamus(Jdbi jdbi) { } // Initiate the team data map - private Map getTeamData() { + private Map getTeamData() { List stations = jdbi.onDemand(StationDAO.class).getAll(); stations.sort(Comparator.comparing(Station::getDistanceFromStart)); List teams = jdbi.onDemand(TeamDAO.class).getAll(); return teams.stream().collect(Collectors.toMap( Team::getId, - team -> new TeamData(team.getId(), INTERVAL_DETECTIONS_MS, stations, MEDIAN_AMOUNT, AVERAGE_SPRINTING_SPEED_M_MS, FINISH_OFFSET_M) + team -> new TeamDataV1(team.getId(), INTERVAL_DETECTIONS_MS, stations, MEDIAN_AMOUNT, AVERAGE_SPRINTING_SPEED_M_MS, FINISH_OFFSET_M) )); } @@ -128,12 +128,14 @@ 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()) { + 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() + entry.getValue().getPosition().progress(), + 0, + System.currentTimeMillis() )) ); entry.getValue().setPreviousStationArrival(entry.getValue().getPreviousStationArrival() + MAX_NO_DATA_MS); diff --git a/src/main/java/telraam/logic/positioner/nostradamus/StationData.java b/src/main/java/telraam/logic/positioner/nostradamus/v1/StationDataV1.java similarity index 81% rename from src/main/java/telraam/logic/positioner/nostradamus/StationData.java rename to src/main/java/telraam/logic/positioner/nostradamus/v1/StationDataV1.java index fbc9d8d..15c7277 100644 --- a/src/main/java/telraam/logic/positioner/nostradamus/StationData.java +++ b/src/main/java/telraam/logic/positioner/nostradamus/v1/StationDataV1.java @@ -1,4 +1,4 @@ -package telraam.logic.positioner.nostradamus; +package telraam.logic.positioner.nostradamus.v1; import telraam.database.models.Station; @@ -6,7 +6,7 @@ import java.util.List; // Record containing all data necessary for TeamData -public record StationData( +public record StationDataV1( Station station, // The station Station nextStation, // The next station List times, // List containing the times (in ms) that was needed to run from this station to the next one. @@ -14,7 +14,7 @@ public record StationData( float currentProgress, // The progress value of this station float nextProgress // The progress value of the next station ) { - public StationData() { + public StationDataV1() { this( new Station(-10), new Station(-9), @@ -25,11 +25,11 @@ public StationData() { ); } - public StationData(List stations, int index, int averageAmount, int totalDistance) { + public StationDataV1(List stations, int index, int averageAmount, int totalDistance) { this( stations.get(index), stations.get((index + 1) % stations.size()), - new CircularQueue<>(averageAmount), + new CircularQueueV1<>(averageAmount), index, (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/v1/TeamDataV1.java similarity index 76% rename from src/main/java/telraam/logic/positioner/nostradamus/TeamData.java rename to src/main/java/telraam/logic/positioner/nostradamus/v1/TeamDataV1.java index c34a016..82de61b 100644 --- a/src/main/java/telraam/logic/positioner/nostradamus/TeamData.java +++ b/src/main/java/telraam/logic/positioner/nostradamus/v1/TeamDataV1.java @@ -1,4 +1,4 @@ -package telraam.logic.positioner.nostradamus; +package telraam.logic.positioner.nostradamus.v1; import lombok.Getter; import lombok.Setter; @@ -7,27 +7,30 @@ import telraam.logic.positioner.Position; import java.util.*; +import java.util.logging.Logger; import java.util.stream.Collectors; -public class TeamData { - private final DetectionList detections; // List with all relevant detections - private final Map stations; // Station list - private StationData currentStation; // Current station location - private StationData previousStation; // Previous station location +public class TeamDataV1 { + private static final Logger logger = Logger.getLogger(TeamDataV1.class.getName()); + private final DetectionListV1 detections; // List with all relevant detections + private final Map stations; // Station list + private StationDataV1 currentStation; // Current station location + private StationDataV1 previousStation; // Previous station location @Getter @Setter 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 Position position; // Data to send to the websocket + private final int teamId; - public TeamData(int teamId, int interval, List stations, int averageAmount, double sprintingSpeed, int finishOffset) { + public TeamDataV1(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( + station -> new StationDataV1( stations, stations.indexOf(station), averageAmount, @@ -38,11 +41,12 @@ public TeamData(int teamId, int interval, List stations, int averageAmo this.stations.forEach((stationId, stationData) -> stationData.times().add( (long) (((stationData.nextStation().getDistanceFromStart() - stationData.station().getDistanceFromStart() + totalDistance) % totalDistance) / sprintingSpeed) )); - this.detections = new DetectionList(interval, stations); + this.detections = new DetectionListV1(interval, stations); this.previousStationArrival = System.currentTimeMillis(); - this.currentStation = new StationData(); // Will never trigger `isNextStation` for the first station - this.position = new Position(teamId); + this.currentStation = new StationDataV1(); // Will never trigger `isNextStation` for the first station this.maxDeviance = (float) 1 / stations.size(); + this.teamId = teamId; + this.position = new Position(teamId, 0, 0, System.currentTimeMillis()); } // Add a new detection @@ -85,8 +89,8 @@ public void updatePosition() { long currentTime = System.currentTimeMillis(); // Animation is currently at progress x - long milliSecondsSince = currentTime - position.getTimestamp(); - double theoreticalProgress = normalize(position.getProgress() + (position.getSpeed() * milliSecondsSince)); + long milliSecondsSince = currentTime - position.timestamp(); + double theoreticalProgress = normalize(position.progress() + (position.speed() * milliSecondsSince)); // Arrive at next station at timestamp y and progress z double median = getMedian(); @@ -106,7 +110,7 @@ public void updatePosition() { speed = normalize(goalProgress - theoreticalProgress) / (nextStationArrival - currentTime); } - position.update(progress, speed, currentTime); + position = new Position(teamId, progress, speed, currentTime); } // Get the medium of the average times diff --git a/src/main/java/telraam/logic/positioner/nostradamus/v2/Nostradamus.java b/src/main/java/telraam/logic/positioner/nostradamus/v2/Nostradamus.java new file mode 100644 index 0000000..05652ae --- /dev/null +++ b/src/main/java/telraam/logic/positioner/nostradamus/v2/Nostradamus.java @@ -0,0 +1,150 @@ +package telraam.logic.positioner.nostradamus.v2; + +import org.jdbi.v3.core.Jdbi; +import telraam.AppConfiguration; +import telraam.database.daos.BatonSwitchoverDAO; +import telraam.database.daos.PositionSourceDAO; +import telraam.database.daos.StationDAO; +import telraam.database.models.*; +import telraam.logic.positioner.Position; +import telraam.logic.positioner.PositionSender; +import telraam.logic.positioner.Positioner; + +import java.util.*; +import java.util.concurrent.ConcurrentHashMap; +import java.util.concurrent.ConcurrentLinkedQueue; +import java.util.concurrent.atomic.AtomicInteger; +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 String SOURCE_NAME = "nostradamus"; + private final int MIN_RSSI = -84; // Minimum rssi strength that a detections needs to have + private final int INTERVAL_FETCH_MS = 10000; + private final int INTERVAL_UPDATE_MS = 200; + private final double MAX_SPEED_M_MS = 0.00972222; // Maximum speed (m / ms) = 35 km / h + private final Jdbi jdbi; + private final PositionSender positionSender; + ConcurrentHashMap> newDetections; + private final Map teamHandlers; + private final Map stationData; + private final double maxSpeedProgressMs; // Maximum speed (progress / ms) + + public Nostradamus(AppConfiguration configuration, Jdbi jdbi) { + this.jdbi = jdbi; + + // Add as source + PositionSourceDAO positionSourceDAO = jdbi.onDemand(PositionSourceDAO.class); + if (positionSourceDAO.getByName(SOURCE_NAME).isEmpty()) { + positionSourceDAO.insert(new PositionSource(SOURCE_NAME)); + } + + this.positionSender = new PositionSender(SOURCE_NAME); + this.newDetections = new ConcurrentHashMap<>(); + this.teamHandlers = new ConcurrentHashMap<>(); + this.stationData = new HashMap<>(); + + // Initialize station data list + List stations = this.jdbi.onDemand(StationDAO.class).getAll(); + stations.sort(Comparator.comparing(Station::getDistanceFromStart)); + int length = (int) (stations.get(stations.size() - 1).getDistanceFromStart() + configuration.getFinishOffset()); + for (int i = 0; i < stations.size(); i++) { + Station station = stations.get(i); + int nextIdx = (i + 1) % stations.size(); + int distanceToNext = (int) ((stations.get(nextIdx).getDistanceFromStart() - station.getDistanceFromStart() + length ) % length); + this.stationData.put(station.getId(), new StationData( + distanceToNext, + station.getDistanceFromStart() / length, + (double) distanceToNext / length, + stations.get(nextIdx).getId(), + i + )); + } + + this.maxSpeedProgressMs = MAX_SPEED_M_MS / (stations.get(stations.size() - 1).getDistanceFromStart() + configuration.getFinishOffset()); + + new Thread(this::fetch).start(); + new Thread(this::update).start(); + } + + // Fetch updates team handlers based on switchovers + private void fetch() { + while (true) { + List switchovers = jdbi.onDemand(BatonSwitchoverDAO.class).getAll(); + + Map batonToTeam = switchovers.stream() + .filter(switchover -> switchover.getNewBatonId() != null) + .sorted(Comparator.comparing(BatonSwitchover::getTimestamp)) + .collect(Collectors.toMap( + BatonSwitchover::getNewBatonId, + BatonSwitchover::getTeamId, + (existing, replacement) -> replacement + )); + + for (Map.Entry entry: batonToTeam.entrySet()) { + teamHandlers.compute(entry.getValue(), (teamId, existingTeam) -> { + if (existingTeam == null) { + return new TeamHandler(teamId, new AtomicInteger(entry.getKey()), maxSpeedProgressMs, stationData); + } else { + existingTeam.batonId.set(entry.getKey()); + return existingTeam; + } + }); + } + + try { + Thread.sleep(INTERVAL_FETCH_MS); + } catch (InterruptedException e) { + logger.severe(e.getMessage()); + } + } + } + + // Update handles all new detections and sends new positions + private void update() { + List positions = new ArrayList<>(); + + while (true) { + positions.clear(); + + for (TeamHandler team : teamHandlers.values()) { + ConcurrentLinkedQueue queue = newDetections.get(team.batonId.get()); + if (queue != null && !queue.isEmpty()) { + List copy = new ArrayList<>(); + Detection d; + while ((d = queue.poll()) != null) { + copy.add(d); + } + + team.update(copy); + } + + Position position = team.getPosition(); + if (position != null) { + positions.add(position); + } + } + + if (!positions.isEmpty()) { + positionSender.send(positions); + } + + try { + Thread.sleep(INTERVAL_UPDATE_MS); + } catch (InterruptedException e) { + logger.severe(e.getMessage()); + } + } + } + + @Override + public void handle(Detection detection) { + if (detection.getRssi() > MIN_RSSI) { + newDetections + .computeIfAbsent(detection.getBatonId(), k -> new ConcurrentLinkedQueue<>()) + .add(detection); + } + } + +} diff --git a/src/main/java/telraam/logic/positioner/nostradamus/v2/StationData.java b/src/main/java/telraam/logic/positioner/nostradamus/v2/StationData.java new file mode 100644 index 0000000..9f2b95d --- /dev/null +++ b/src/main/java/telraam/logic/positioner/nostradamus/v2/StationData.java @@ -0,0 +1,10 @@ +package telraam.logic.positioner.nostradamus.v2; + +// Record containing all data regarding a station +public record StationData( + int distanceToNext, // Meters until the next station + double progress, // Location of station in progress + double progressToNext, // Progress until you arrive at the next station + int nextStationId, // ID of the next station + int index // Index of the station when sorted by distance from the start +) {} diff --git a/src/main/java/telraam/logic/positioner/nostradamus/v2/TeamHandler.java b/src/main/java/telraam/logic/positioner/nostradamus/v2/TeamHandler.java new file mode 100644 index 0000000..c7e31de --- /dev/null +++ b/src/main/java/telraam/logic/positioner/nostradamus/v2/TeamHandler.java @@ -0,0 +1,169 @@ +package telraam.logic.positioner.nostradamus.v2; + +import telraam.database.models.Detection; +import telraam.logic.positioner.Position; + +import java.sql.Timestamp; +import java.util.*; +import java.util.concurrent.atomic.AtomicInteger; +import java.util.logging.Logger; + +public class TeamHandler { + private static final Logger logger = Logger.getLogger(TeamHandler.class.getName()); + private final double AVG_SPEED = 0.006; // Average sprinting speed (m / ms), results in a lap of 55 seconds in the 12ul + private final int INTERVAL = 2000; // Only keep detections in a x ms interval + private final int MAX_TIMES = 20; // Amount of speeds to keep track of to determine the median + private final int teamId; + public AtomicInteger batonId; + private final double maxSpeed; + private final Map stationDataMap; // Map from station id to StationData + private final Map> stationSpeeds; // Avg speed (progress / ms) to go from a stationId to the next + private int currentStation; // Current station id + private Position lastPosition; + private final Queue positions; + private final LinkedList detections; + private Detection currentStationDetection; + + public TeamHandler(int teamId, AtomicInteger batonId, double maxSpeed, Map stationDataMap) { + this.teamId = teamId; + this.batonId = batonId; + this.maxSpeed = maxSpeed; + this.stationDataMap = stationDataMap; + + this.stationSpeeds = new HashMap<>(); + this.positions = new ArrayDeque<>(); + this.detections = new LinkedList<>(); + + this.currentStation = -1; + this.lastPosition = new Position(0, 0, 0, 0); + this.detections.add(new Detection(-1, -1, -1000, new Timestamp(0))); + + // Populate the stationSpeeds map with default values + for (Map.Entry entry: stationDataMap.entrySet()) { + this.stationSpeeds.put(entry.getKey(), new ArrayList<>()); + double progress = stationDataMap.get(entry.getKey()).progressToNext(); + double time = entry.getValue().distanceToNext() / AVG_SPEED; + this.stationSpeeds.get(entry.getKey()).add(progress / time); + } + } + + public void update(List detections) { + boolean newStation = handleDetection(detections); + if (!newStation) { + return; + } + + StationData station = stationDataMap.get(currentStation); + long timestamp = System.currentTimeMillis(); + + double currentProgress = normalize(lastPosition.progress() + lastPosition.speed() * (timestamp - lastPosition.timestamp())); // Where is the animation now + + double maxDeviation = station.progressToNext(); + if (circularDistance(currentProgress, station.progress()) > maxDeviation) { + // Don't let the animation deviate too much from the reality + currentProgress = station.progress(); + } + + long intervalTime = (long) (station.progressToNext() / getMedianSpeed(currentStation)); // How many ms until it should reach the next station + double goalProgress = normalize(station.progress() + station.progressToNext()); // Where is the next station + double speed = normalize(goalProgress - currentProgress) / intervalTime; + + if (speed > maxSpeed) { + // Sanity check + currentProgress = stationDataMap.get(currentStation).progress(); + speed = getMedianSpeed(currentStation); + } + + positions.clear(); + positions.add(new Position(teamId, currentProgress, speed, timestamp)); + } + + public Position getPosition() { + if (!positions.isEmpty()) { + lastPosition = positions.poll(); + return lastPosition; + } + + return null; + } + + private boolean handleDetection(List newDetections) { + boolean newStation = false; + + newDetections.sort(Comparator.comparing(Detection::getTimestamp)); + for (Detection detection: newDetections) { + if (!detection.getTimestamp().after(detections.getLast().getTimestamp())) { + // Only keep newer detections + continue; + } + + detections.add(detection); // Newest detection is now at the end of the list + + if (detection.getStationId() == currentStation) { + // We've already determined that we have arrived at this station + continue; + } + + // Filter out old detections + long lastDetection = detections.stream().max(Comparator.comparing(d -> d.getTimestamp().getTime())).get().getTimestamp().getTime(); + detections.removeIf(d -> lastDetection - d.getTimestamp().getTime() > INTERVAL); + + // Determine new position + int newStationId = detections.stream().max(Comparator.comparing(Detection::getRssi)).get().getStationId(); // detections will at least contain the last detection + if (currentStation != newStationId && stationAfter(newStationId)) { + // New position! + // Add new speed + if (currentStationDetection != null && newStationId == stationDataMap.get(currentStation).nextStationId()) { // Necessary for the first station switch + double progress = normalize(stationDataMap.get(newStationId).progress() - stationDataMap.get(currentStation).progress()); + double time = detection.getTimestamp().getTime() - currentStationDetection.getTimestamp().getTime(); + stationSpeeds.get(currentStation).add(progress / time); + } + + // Update station variables + currentStation = newStationId; + currentStationDetection = detection; + newStation = true; + } + } + + return newStation; + } + + private boolean stationAfter(int newStationId) { + if (currentStationDetection == null) { + return true; + } + + int stations = stationDataMap.size(); + return (((stationDataMap.get(newStationId).index() - stationDataMap.get(currentStation).index()) % stations) + stations) % stations < 4; + } + + private double getMedianSpeed(int stationId) { + List times = stationSpeeds.get(stationId); + if (times.size() > MAX_TIMES) { + times.subList(0, times.size() - MAX_TIMES).clear(); + } + + List copy = new ArrayList<>(times); + Collections.sort(copy); + + double median; + if (copy.size() % 2 == 0) { + median = (copy.get(copy.size() / 2) + copy.get(copy.size() / 2 - 1)) / 2; + } else { + median = copy.get(copy.size() / 2); + } + + return median; + } + + private double circularDistance(double a, double b) { + double diff = Math.abs(a - b); + return Math.min(diff, 1 - diff); + } + + private double normalize(double amount) { + return ((amount % 1) + 1) % 1; + } + +} diff --git a/src/main/resources/telraam/devConfig.yml b/src/main/resources/telraam/devConfig.yml index f3b5a8f..db4adc8 100644 --- a/src/main/resources/telraam/devConfig.yml +++ b/src/main/resources/telraam/devConfig.yml @@ -46,6 +46,9 @@ database: # the minimum amount of time an connection must sit idle in the pool before it is eligible for eviction minIdleTime: 1 minute +# Distance between the start and the last station +finish_offset: 20 + # Logging settings. logging: