DovesLapTimer.h

Go to the documentation of this file.

 
#ifndef _DOVES_LAP_TIMER_H
#define _DOVES_LAP_TIMER_H
 
#include "ArxTypeTraits.h"
#include "GeoMath.h"
#include "CrossingEngine.h"
using TRITYPE = double;
 
// On classic AVR (Mega, Uno) `double` is a 32-bit float (~7 significant
// digits). At real-world latitudes/longitudes that quantizes position to
// roughly 0.2-0.75 m and pushes per-fix haversine half-angle differences
// below float precision: lap *counting* still works (the 7 m crossing
// threshold absorbs it) but odometer increments and crossing interpolation
// carry large relative error. The full advertised precision requires a
// target with true 64-bit double (nRF52840, ESP32, SAMD51, RP2040, ...).
// See README "Hardware" notes.
#if defined(__SIZEOF_DOUBLE__) && (__SIZEOF_DOUBLE__ < 8)
#warning "DovesLapTimer: 'double' is only 32 bits on this target (classic AVR). Lap counting works but GPS math runs degraded - distances and interpolated crossing times will be noticeably less accurate. Use a 64-bit-double MCU (e.g. XIAO nRF52840) for full precision."
#endif
 
// CROSSING_LINE_SIDE_* now live in CrossingEngine.h (included above).
 
// Course detection constants
#define COURSE_DETECT_SPEED_THRESHOLD_MPH  20
#define COURSE_DETECT_WAYPOINT_PROXIMITY_METERS 10.0
#define COURSE_DETECT_MIN_DISTANCE_METERS  200.0
#define COURSE_DETECT_DISTANCE_TOLERANCE_PCT  0.25
#define COURSE_DETECT_MAX_REJECTIONS  3
// Completed proximity passes (full laps back at the waypoint) that matched
// no configured course length before CourseManager falls back to Lap
// Anything. Without this, a wrong/missing course config left detection in
// WAYPOINT_SET forever while the WaypointLapTimer's laps were never surfaced.
#define COURSE_DETECT_MAX_NO_MATCH_PASSES  3
// Distance failsafe: if detection is still incomplete after driving
// factor x (longest configured course length), or the floor below for very
// short courses, fall back to Lap Anything. Catches the "never returns to
// within 10m of the waypoint" hang that pass counting can't see.
#define COURSE_DETECT_FALLBACK_DISTANCE_FACTOR  4.0
#define COURSE_DETECT_FALLBACK_MIN_METERS  2000.0
#define METERS_TO_FEET  GEOMATH_METERS_TO_FEET
 
// Waypoint lap timer constants
#define WAYPOINT_LAP_MIN_DISTANCE_METERS  100.0
#define WAYPOINT_LAP_PROXIMITY_METERS  30.0
 
// Direction detection
#define DIR_UNKNOWN  0
#define DIR_FORWARD  1
#define DIR_REVERSE  2
 
// Course detection states
#define DETECT_STATE_IDLE  0
#define DETECT_STATE_WAITING_FOR_SPEED  1
#define DETECT_STATE_WAYPOINT_SET  2
#define DETECT_STATE_CANDIDATES_READY  3
#define DETECT_STATE_DETECTED  4
 
// Maximum courses supported
#define MAX_COURSES  8
 
// DOVES_MILLIS_PER_DAY, timeSinceMidnightDelta(), CROSSING_MAX_FIX_GAP_MS,
// CROSSING_PAIR_SPACING_FACTOR, LineDetectResult, and
// crossingPointBufferEntry now live in CrossingEngine.h (included above).
 
// GPS input validation: a single-fix jump beyond this is treated as a glitch
// and dropped; after GPS_JUMP_REACCEPT_COUNT consecutive far fixes the new
// position is accepted as real (signal re-acquisition) and the position is
// re-seeded without crediting the gap to the odometer.
#define GPS_MAX_PLAUSIBLE_JUMP_METERS  500.0
#define GPS_JUMP_REACCEPT_COUNT  3
 
struct DirectionDetector {
  int direction;    // DIR_UNKNOWN, DIR_FORWARD, DIR_REVERSE
  bool raceSeen;
  // Per-lap window: timestamps of physical S2/S3 crossings since the last
  // start/finish. Direction is decided only when BOTH are present, by their
  // temporal order — independent of the lap calculation's sector output.
  unsigned long lapS2CrossingTime;
  unsigned long lapS3CrossingTime;
 
  DirectionDetector()
    : direction(DIR_UNKNOWN), raceSeen(false),
      lapS2CrossingTime(0), lapS3CrossingTime(0) {}
  void reset() {
    direction = DIR_UNKNOWN;
    raceSeen = false;
    lapS2CrossingTime = 0;
    lapS3CrossingTime = 0;
  }
  void onLineCrossing(int sectorNumber, unsigned long crossingTime);
  bool isReverse() const { return direction == DIR_REVERSE; }
  bool isResolved() const { return direction != DIR_UNKNOWN; }
};
 
class DovesLapTimer {
public:
  DovesLapTimer(double crossingThresholdMeters = 7, Stream *debugSerial = NULL);
 
  int loop(double currentLat, double currentLng, float currentAltitudeMeters, float currentSpeedKnots);
 
 
  bool isObtuseTriangle(double lat1, double lon1, double lat2, double lon2, double lat3, double lon3);
 
  bool insideLineThreshold(double driverLat, double driverLon, double crossingPointALat, double crossingPointALon, double crossingPointBLat, double crossingPointBLon);
 
  int pointOnSideOfLine(double driverLat, double driverLng, double pointALat, double pointALng, double pointBLat, double pointBLng);
  double pointLineSegmentDistance(double pointX, double pointY, double startX, double startY, double endX, double endY);
  double haversine(double lat1, double lon1, double lat2, double lon2);
  double haversine3D(double prevLat, double prevLng, double prevAlt, double currentLat, double currentLng, double currentAlt);
 
 
  void reset();
  void setStartFinishLine(double pointALat, double pointALng, double pointBLat, double pointBLng);
  void setSector2Line(double pointALat, double pointALng, double pointBLat, double pointBLng);
  void setSector3Line(double pointALat, double pointALng, double pointBLat, double pointBLng);
  void updateCurrentTime(unsigned long currentTimeMilliseconds);
  void forceLinearInterpolation();
  void forceCatmullRomInterpolation();
  bool getRaceStarted() const;
  bool getCrossing() const;
  unsigned long getCurrentLapStartTime() const;
  unsigned long getCurrentLapTime() const;
  unsigned long getLastLapTime() const;
  unsigned long getBestLapTime() const;
  float getCurrentLapOdometerStart() const;
  float getCurrentLapDistance() const;
  float getLastLapDistance() const;
  float getBestLapDistance() const;
  float getTotalDistanceTraveled() const;
  int getBestLapNumber() const;
  int getLaps() const;
  float getPaceDifference() const;
  float getCurrentSpeedKmh() const;
  float getCurrentSpeedMph() const;
 
 
  // Sector timing methods
 
  unsigned long getBestSector1Time() const;
  unsigned long getBestSector2Time() const;
  unsigned long getBestSector3Time() const;
  unsigned long getCurrentLapSector1Time() const;
  unsigned long getCurrentLapSector2Time() const;
  unsigned long getCurrentLapSector3Time() const;
  unsigned long getOptimalLapTime() const;
  int getBestSector1LapNumber() const;
  int getBestSector2LapNumber() const;
  int getBestSector3LapNumber() const;
  int getCurrentSector() const;
  bool areSectorLinesConfigured() const;
  unsigned int getRejectedCrossingCount() const;
  bool isStartFinishLineConfigured() const;
 
  // Direction detection methods
 
  int getDirection() const;
  bool isDirectionResolved() const;
 
private:
#ifdef DOVES_DISABLE_DEBUG
  // Compile-time debug kill switch — see CrossingEngine.h for the rationale.
  template<typename... Args> void debug_print(Args&&...) {}
  template<typename... Args> void debug_println(Args&&...) {}
#else
  template<typename... Args>
  void debug_print(Args&&... args) {
    if(_serial) {
      _serial->print(std::forward<Args>(args)...);
    }
  }
  template<typename... Args>
  void debug_println(Args&&... args) {
    if(_serial) {
      _serial->println(std::forward<Args>(args)...);
    }
  }
#endif
 
  bool checkStartFinish(double currentLat, double currentLng);
  bool checkSectorLine(double currentLat, double currentLng, double pointALat, double pointALng, double pointBLat, double pointBLng, bool& crossingFlag, int sectorNumber);
  LineDetectResult _detectLineCrossing(
      double currentLat, double currentLng,
      double pointALat, double pointALng,
      double pointBLat, double pointBLng,
      bool& crossingFlag,
      int lineLabel,
      double& outLat, double& outLng,
      unsigned long& outTime,
      double& outOdometer);
  void handleLineCrossing(unsigned long crossingTime, int sectorNumber);
  void updateBestSectors();
 
  Stream *_serial;
  DirectionDetector _directionDetector;
  // The shared crossing pipeline (buffer + interpolation). ONE engine for
  // all three lines — the historical shared-buffer design, kept so the
  // by-value CourseManager timer array doesn't triple in size. loop()
  // enforces the resulting only-one-line-crossing-at-a-time exclusion.
  CrossingEngine _crossingEngine;
 
  unsigned long millisecondsSinceMidnight = 0;
  // Timing variables
  double crossingThresholdMeters;
  bool raceStarted = false;
  bool crossing = false;
  unsigned long currentLapStartTime = 0;
  unsigned long lastLapTime = 0;
  unsigned long bestLapTime = 0;
  float currentLapOdometerStart = 0.0;
  float lastLapDistance = 0.0;
  float bestLapDistance = 0.0;
  float currentSpeedkmh = 0.0;
  int bestLapNumber = 0;
  int laps = 0;
 
  // Sector timing state
  int currentSector = 0;  // 0=not started, 1/2/3=in sector
  unsigned long currentSectorStartTime = 0;
  bool crossingSector2 = false;
  bool crossingSector3 = false;
 
  // Current lap sector times (reset each lap)
  unsigned long currentLapSector1Time = 0;
  unsigned long currentLapSector2Time = 0;
  unsigned long currentLapSector3Time = 0;
 
  // Best sector times (persistent across laps)
  unsigned long bestSector1Time = 0;
  unsigned long bestSector2Time = 0;
  unsigned long bestSector3Time = 0;
 
  // Lap numbers that achieved best sectors
  int bestSector1LapNumber = 0;
  int bestSector2LapNumber = 0;
  int bestSector3LapNumber = 0;
 
  float totalDistanceTraveled = 0;
  float positionPrevAlt = 0.00;
  double positionPrevLat = 0.00;
  double positionPrevLng = 0.00;
  bool firstPositionReceived = false;  // Explicit flag for first GPS fix detection
 
  // Previous GPS fix snapshot (used as Catmull-Rom pre-crossing control point)
  crossingPointBufferEntry prevFix = {0, 0, 0, 0, 0};
  bool hasPrevFix = false;
 
  double startFinishPointALat = 0.0;
  double startFinishPointALng = 0.0;
  double startFinishPointBLat = 0.0;
  double startFinishPointBLng = 0.0;
 
  // Sector 2 line coordinates
  double sector2PointALat = 0.0;
  double sector2PointALng = 0.0;
  double sector2PointBLat = 0.0;
  double sector2PointBLng = 0.0;
 
  // Sector 3 line coordinates
  double sector3PointALat = 0.0;
  double sector3PointALng = 0.0;
  double sector3PointBLat = 0.0;
  double sector3PointBLng = 0.0;
 
  // Line configuration flags — loop() skips detection for unconfigured lines
  bool startFinishLineConfigured = false;
  bool sector2LineConfigured = false;
  bool sector3LineConfigured = false;
 
  // Consecutive fixes rejected for jumping > GPS_MAX_PLAUSIBLE_JUMP_METERS
  int consecutiveJumpCount = 0;
};
 
#endif