Cache pose calculations in PhotonPoseEstimator (#788)

* Add pose caching to Java

* Refactor strategy fallthrough

* Hopefully add pose caching to C++

* Make Java switch same order as enum and C++ switch

* C++ absolute value in timestamp check

* Fix Java NPE

* Use `units::second_t` in timestamp

Co-authored-by: Matt <matthew.morley.ca@gmail.com>

* Expand Java unit test

* Copy comments into C++

* Add tests to C++

* Run format

* Update PhotonCamera.cpp

* Probably fix bad access exception

* a

* init timestamp

* Remove prints

---------

Co-authored-by: Matt <matthew.morley.ca@gmail.com>
Co-authored-by: Joseph Eng <joseng2358@gmail.com>

photon-lib/src/main/native/include/photonlib/PhotonPoseEstimator.h

@@ -109,7 +109,12 @@ class PhotonPoseEstimator {
    *
    * @param strategy the strategy to set
    */
-  inline void SetPoseStrategy(PoseStrategy strat) { strategy = strat; }
+  inline void SetPoseStrategy(PoseStrategy strat) {
+    if (strategy != strat) {
+      InvalidatePoseCache();
+    }
+    strategy = strat;
+  }
 
   /**
    * Set the Position Estimation Strategy used in multi-tag mode when
@@ -133,6 +138,9 @@ class PhotonPoseEstimator {
    * @param referencePose the referencePose to set
    */
   inline void SetReferencePose(frc::Pose3d referencePose) {
+    if (this->referencePose != referencePose) {
+      InvalidatePoseCache();
+    }
     this->referencePose = referencePose;
   }
 
@@ -186,6 +194,10 @@ class PhotonPoseEstimator {
   frc::Pose3d lastPose;
   frc::Pose3d referencePose;
 
+  units::second_t poseCacheTimestamp;
+
+  inline void InvalidatePoseCache() { poseCacheTimestamp = -1_s; }
+
   std::optional<EstimatedRobotPose> Update(PhotonPipelineResult result,
                                            PoseStrategy strategy);