From 1de67fc9988e046169e0e89f6142ae0e935d9f9c Mon Sep 17 00:00:00 2001
From: Michael <michael.r.candrian@gmail.com>
Date: Wed, 26 Feb 2025 18:58:12 -0800
Subject: [PATCH] Update EncoderMapper.java

---
 .../java/org/xerosw/util/EncoderMapper.java   | 107 ++++++++----------
 1 file changed, 50 insertions(+), 57 deletions(-)

diff --git a/src/main/java/org/xerosw/util/EncoderMapper.java b/src/main/java/org/xerosw/util/EncoderMapper.java
index 1e98031b..6dd391ec 100644
--- a/src/main/java/org/xerosw/util/EncoderMapper.java
+++ b/src/main/java/org/xerosw/util/EncoderMapper.java
@@ -19,98 +19,91 @@
 /// Encoders
 /// </pre>
 public class EncoderMapper {
-    private double kEncoder2Robot_ ;
-    private double rmax_ ;
-    private double rmin_ ;
-    private double rc_ ;    
-    private double emax_ ;
-    private double emin_ ;
-    private double ec_ ;
+    private double kEncoder2Robot_;
+    private double rmax_;
+    private double rmin_;
+    private double rc_;
+    private double emax_;
+    private double emin_;
+    private double ec_;
 
     /// \brief create a new encoder mapper objects
     /// \param rmax the maximum range of the robot physical characteristic
     /// \param rmin the minimum range of the robot physical characteristic
-    /// \param emax the maximum range of the electrical value returned from the encoder
-    /// \param emin the minimum range of the electrical value returned from the encoder
+    /// \param emax the maximum range of the electrical value returned from the
+    /// encoder
+    /// \param emin the minimum range of the electrical value returned from the
+    /// encoder
     public EncoderMapper(double rmax, double rmin, double emax, double emin) {
-        rmax_ = rmax ;
-        rmin_ = rmin ;
-        emax_ = emax ;
-        emin_ = emin ;
-        kEncoder2Robot_ = (rmax - rmin) / (emax - emin) ;   
+        rmax_ = rmax;
+        rmin_ = rmin;
+        emax_ = emax;
+        emin_ = emin;
+        kEncoder2Robot_ = (rmax - rmin) / (emax - emin);
     }
 
     /// \brief calibrate the encoder mapper.
-    /// This establishes the wrap point of the encoder versus the physical system being measured.
+    /// This establishes the wrap point of the encoder versus the physical system
+    /// being measured.
     /// \param robot a reading from the robot
     /// \param encoder a reading from the encoder
     public void calibrate(double robot, double encoder) {
-        ec_ = encoder ;
-        rc_ = robot ;
+        ec_ = encoder;
+        rc_ = robot;
     }
 
     /// \brief convert an encoder value to a physical robot value
     /// \param encoder the encoder value
     /// \returns the robot value
     public double toRobot(double encoder) {
-        double ret ;
-        double offset; 
-
-        encoder = clamp(encoder, emax_, emin_) ;
-        offset = normalize(ec_ - (rc_ - rmin_) / kEncoder2Robot_, emax_, emin_) ;
-        ret = normalize((encoder - offset) * kEncoder2Robot_ + rmin_, rmax_, rmin_) ;
-        
-        return ret ;
+        double ret;
+        double offset;
+
+        encoder = clamp(encoder, emax_, emin_);
+        offset = normalize(ec_ - (rc_ - rmin_) / kEncoder2Robot_, emax_, emin_);
+        ret = normalize((encoder - offset) * kEncoder2Robot_ + rmin_, rmax_, rmin_);
+
+        return ret;
     }
 
     /// \brief convert an robot value to a encoder value
     /// \param robot the robot value
     /// \returns the encoder value
     public double toEncoder(double robot) {
-        double ret ;
-        double offset ;
+        double ret;
+        double offset;
 
-        robot = clamp(robot, rmax_, rmin_) ;
-        offset = normalize(ec_ - (rc_ - rmin_) / kEncoder2Robot_,  emax_, emin_) ;
-        ret = normalize(offset + (robot - rmin_) / kEncoder2Robot_,  emax_, emin_) ;
-        
-        return ret ;
-    }    
+        robot = clamp(robot, rmax_, rmin_);
+        offset = normalize(ec_ - (rc_ - rmin_) / kEncoder2Robot_, emax_, emin_);
+        ret = normalize(offset + (robot - rmin_) / kEncoder2Robot_, emax_, emin_);
 
-    private double normalize(double value, double vmax, double vmin) {
-        if (vmax < vmin)
-        {
-            double temp = vmax ;
-            vmax = vmin ;
-            vmin = temp ;
-        }
+        return ret;
+    }
 
-        while (value < vmin)
-        {
-            value += (vmax - vmin) ;
+    private double normalize(double value, double vmax, double vmin) {
+        if (vmax < vmin) {
+            double temp = vmax;
+            vmax = vmin;
+            vmin = temp;
         }
 
-        while (value > vmax)
-        {
-            value -= (vmax - vmin) ;
-        }
+        value = value + (vmax - vmin) * -Math.floor(((value - vmin) / (vmax - vmin)));
 
-        return value ;
+        return value;
     }
 
     private double clamp(double value, double vmax, double vmin) {
-        if (vmax < vmin)
-        {
-            double temp = vmax ;
-            vmax = vmin ;
-            vmin = temp ;
+        if (vmax < vmin) {
+            double temp = vmax;
+            vmax = vmin;
+            vmin = temp;
         }
 
         if (value > vmax)
-            value = vmax ;
+            value = vmax;
         else if (value < vmin)
-            value = vmin ;
+            value = vmin;
 
-        return value ;
-    }    
+        return value;
+    }
 }
\ No newline at end of file