From 8f9d7cd573374138af22084694169e9300e67c2c Mon Sep 17 00:00:00 2001
From: InfinityDevTech <infinitydevtechnologies@gmail.com>
Date: Mon, 10 Jul 2023 13:15:15 -0400
Subject: [PATCH 1/2] Kotlin! Clean repo and code

---
 PhotonCore/consumer-rules.pro => .gitignore |  0
 .idea/compiler.xml                          |  6 ++++++
 .idea/gradle.xml                            | 17 +++++++++++++++++
 .idea/misc.xml                              |  8 ++++++++
 .idea/vcs.xml                               |  6 ++++++
 PhotonCore/.gitignore                       |  1 -
 PhotonCore/proguard-rules.pro               | 21 ---------------------
 7 files changed, 37 insertions(+), 22 deletions(-)
 rename PhotonCore/consumer-rules.pro => .gitignore (100%)
 create mode 100644 .idea/compiler.xml
 create mode 100644 .idea/gradle.xml
 create mode 100644 .idea/misc.xml
 create mode 100644 .idea/vcs.xml
 delete mode 100644 PhotonCore/.gitignore
 delete mode 100644 PhotonCore/proguard-rules.pro

diff --git a/PhotonCore/consumer-rules.pro b/.gitignore
similarity index 100%
rename from PhotonCore/consumer-rules.pro
rename to .gitignore
diff --git a/.idea/compiler.xml b/.idea/compiler.xml
new file mode 100644
index 0000000..659bf43
--- /dev/null
+++ b/.idea/compiler.xml
@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="CompilerConfiguration">
+    <bytecodeTargetLevel target="16" />
+  </component>
+</project>
\ No newline at end of file
diff --git a/.idea/gradle.xml b/.idea/gradle.xml
new file mode 100644
index 0000000..abca41b
--- /dev/null
+++ b/.idea/gradle.xml
@@ -0,0 +1,17 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="GradleSettings">
+    <option name="linkedExternalProjectsSettings">
+      <GradleProjectSettings>
+        <option name="externalProjectPath" value="$PROJECT_DIR$" />
+        <option name="gradleJvm" value="corretto-16" />
+        <option name="modules">
+          <set>
+            <option value="$PROJECT_DIR$" />
+            <option value="$PROJECT_DIR$/PhotonCore" />
+          </set>
+        </option>
+      </GradleProjectSettings>
+    </option>
+  </component>
+</project>
\ No newline at end of file
diff --git a/.idea/misc.xml b/.idea/misc.xml
new file mode 100644
index 0000000..370437b
--- /dev/null
+++ b/.idea/misc.xml
@@ -0,0 +1,8 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="ExternalStorageConfigurationManager" enabled="true" />
+  <component name="FrameworkDetectionExcludesConfiguration">
+    <file type="web" url="file://$PROJECT_DIR$" />
+  </component>
+  <component name="ProjectRootManager" version="2" languageLevel="JDK_16" project-jdk-name="corretto-16" project-jdk-type="JavaSDK" />
+</project>
\ No newline at end of file
diff --git a/.idea/vcs.xml b/.idea/vcs.xml
new file mode 100644
index 0000000..35eb1dd
--- /dev/null
+++ b/.idea/vcs.xml
@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="VcsDirectoryMappings">
+    <mapping directory="" vcs="Git" />
+  </component>
+</project>
\ No newline at end of file
diff --git a/PhotonCore/.gitignore b/PhotonCore/.gitignore
deleted file mode 100644
index 42afabf..0000000
--- a/PhotonCore/.gitignore
+++ /dev/null
@@ -1 +0,0 @@
-/build
\ No newline at end of file
diff --git a/PhotonCore/proguard-rules.pro b/PhotonCore/proguard-rules.pro
deleted file mode 100644
index 481bb43..0000000
--- a/PhotonCore/proguard-rules.pro
+++ /dev/null
@@ -1,21 +0,0 @@
-# Add project specific ProGuard rules here.
-# You can control the set of applied configuration files using the
-# proguardFiles setting in build.gradle.
-#
-# For more details, see
-#   http://developer.android.com/guide/developing/tools/proguard.html
-
-# If your project uses WebView with JS, uncomment the following
-# and specify the fully qualified class name to the JavaScript interface
-# class:
-#-keepclassmembers class fqcn.of.javascript.interface.for.webview {
-#   public *;
-#}
-
-# Uncomment this to preserve the line number information for
-# debugging stack traces.
-#-keepattributes SourceFile,LineNumberTable
-
-# If you keep the line number information, uncomment this to
-# hide the original source file name.
-#-renamesourcefileattribute SourceFile
\ No newline at end of file

From 298b1994b0b1bb70c3d8e0d36eb700217890b6b2 Mon Sep 17 00:00:00 2001
From: InfinityDevTech <infinitydevtechnologies@gmail.com>
Date: Mon, 10 Jul 2023 15:00:06 -0400
Subject: [PATCH 2/2] Actually added kotlin code this time :)

---
 .gitignore                                    |   4 +
 .idea/gradle.xml                              |   1 -
 .idea/misc.xml                                |   2 +-
 PhotonCore/build.gradle                       |  14 +-
 .../photoncore/Neutrino/MB1242/MB1242Ex.java  | 253 -----------
 .../photoncore/Neutrino/MB1242/MB1242Ex.kt    | 185 ++++++++
 .../Rev2MSensor/Rev2mDistanceSensorEx.java    |  70 ---
 .../Rev2MSensor/Rev2mDistanceSensorEx.kt      |  56 +++
 .../BroadcomColorSensorImplEx.java            | 429 ------------------
 .../BroadcomColorSensorImplEx.kt              | 412 +++++++++++++++++
 .../RevColorSensor/RevColorSensorV3Ex.java    | 169 -------
 .../RevColorSensor/RevColorSensorV3Ex.kt      | 161 +++++++
 .../photoncore/PhotonCore.java                | 426 -----------------
 .../photoncore/PhotonCore.kt                  | 340 ++++++++++++++
 .../photoncore/PhotonLynxModule.java          |  83 ----
 .../photoncore/PhotonLynxModule.kt            |  68 +++
 .../photoncore/ReflectionUtils.java           |  50 --
 .../photoncore/ReflectionUtils.kt             |  51 +++
 build.gradle                                  |   4 +
 19 files changed, 1290 insertions(+), 1488 deletions(-)
 delete mode 100644 PhotonCore/src/main/java/com/outoftheboxrobotics/photoncore/Neutrino/MB1242/MB1242Ex.java
 create mode 100644 PhotonCore/src/main/java/com/outoftheboxrobotics/photoncore/Neutrino/MB1242/MB1242Ex.kt
 delete mode 100644 PhotonCore/src/main/java/com/outoftheboxrobotics/photoncore/Neutrino/Rev2MSensor/Rev2mDistanceSensorEx.java
 create mode 100644 PhotonCore/src/main/java/com/outoftheboxrobotics/photoncore/Neutrino/Rev2MSensor/Rev2mDistanceSensorEx.kt
 delete mode 100644 PhotonCore/src/main/java/com/outoftheboxrobotics/photoncore/Neutrino/RevColorSensor/BroadcomColorSensorImplEx.java
 create mode 100644 PhotonCore/src/main/java/com/outoftheboxrobotics/photoncore/Neutrino/RevColorSensor/BroadcomColorSensorImplEx.kt
 delete mode 100644 PhotonCore/src/main/java/com/outoftheboxrobotics/photoncore/Neutrino/RevColorSensor/RevColorSensorV3Ex.java
 create mode 100644 PhotonCore/src/main/java/com/outoftheboxrobotics/photoncore/Neutrino/RevColorSensor/RevColorSensorV3Ex.kt
 delete mode 100644 PhotonCore/src/main/java/com/outoftheboxrobotics/photoncore/PhotonCore.java
 create mode 100644 PhotonCore/src/main/java/com/outoftheboxrobotics/photoncore/PhotonCore.kt
 delete mode 100644 PhotonCore/src/main/java/com/outoftheboxrobotics/photoncore/PhotonLynxModule.java
 create mode 100644 PhotonCore/src/main/java/com/outoftheboxrobotics/photoncore/PhotonLynxModule.kt
 delete mode 100644 PhotonCore/src/main/java/com/outoftheboxrobotics/photoncore/ReflectionUtils.java
 create mode 100644 PhotonCore/src/main/java/com/outoftheboxrobotics/photoncore/ReflectionUtils.kt

diff --git a/.gitignore b/.gitignore
index e69de29..989a4d5 100644
--- a/.gitignore
+++ b/.gitignore
@@ -0,0 +1,4 @@
+.gradle
+.idea
+
+PhotonCore/build
\ No newline at end of file
diff --git a/.idea/gradle.xml b/.idea/gradle.xml
index abca41b..2f72e47 100644
--- a/.idea/gradle.xml
+++ b/.idea/gradle.xml
@@ -4,7 +4,6 @@
     <option name="linkedExternalProjectsSettings">
       <GradleProjectSettings>
         <option name="externalProjectPath" value="$PROJECT_DIR$" />
-        <option name="gradleJvm" value="corretto-16" />
         <option name="modules">
           <set>
             <option value="$PROJECT_DIR$" />
diff --git a/.idea/misc.xml b/.idea/misc.xml
index 370437b..955f2f8 100644
--- a/.idea/misc.xml
+++ b/.idea/misc.xml
@@ -4,5 +4,5 @@
   <component name="FrameworkDetectionExcludesConfiguration">
     <file type="web" url="file://$PROJECT_DIR$" />
   </component>
-  <component name="ProjectRootManager" version="2" languageLevel="JDK_16" project-jdk-name="corretto-16" project-jdk-type="JavaSDK" />
+  <component name="ProjectRootManager" version="2" languageLevel="JDK_16" default="true" project-jdk-name="corretto-16" project-jdk-type="JavaSDK" />
 </project>
\ No newline at end of file
diff --git a/PhotonCore/build.gradle b/PhotonCore/build.gradle
index 8dee367..9f4b6ce 100644
--- a/PhotonCore/build.gradle
+++ b/PhotonCore/build.gradle
@@ -1,6 +1,7 @@
 plugins {
     id 'com.android.library'
     id 'maven-publish'
+    id 'org.jetbrains.kotlin.android'
 }
 
 repositories {
@@ -8,12 +9,12 @@ repositories {
 }
 
 android {
-    compileSdkVersion 30
+    compileSdkVersion 34
     buildToolsVersion "30.0.3"
 
     defaultConfig {
         minSdkVersion 24
-        targetSdkVersion 30
+        targetSdkVersion 34
         versionCode 1
         versionName "1.0"
 
@@ -44,12 +45,13 @@ afterEvaluate {
 }
 
 dependencies {
+    implementation 'androidx.core:core-ktx:1.10.1'
     compileOnly 'org.jetbrains:annotations:15.0'
-    compileOnly "androidx.annotation:annotation:1.4.0"
+    compileOnly "androidx.annotation:annotation:1.6.0"
 
-    compileOnly 'org.firstinspires.ftc:RobotCore:8.1.0'
-    compileOnly 'org.firstinspires.ftc:Hardware:8.1.0'
-    compileOnly 'org.firstinspires.ftc:FtcCommon:8.1.0'
+    compileOnly 'org.firstinspires.ftc:RobotCore:8.1.1'
+    compileOnly 'org.firstinspires.ftc:Hardware:8.1.1'
+    compileOnly 'org.firstinspires.ftc:FtcCommon:8.1.1'
     compileOnly 'org.reflections:reflections:0.10.2'
     compileOnly 'commons-beanutils:commons-beanutils:1.9.4'
 
diff --git a/PhotonCore/src/main/java/com/outoftheboxrobotics/photoncore/Neutrino/MB1242/MB1242Ex.java b/PhotonCore/src/main/java/com/outoftheboxrobotics/photoncore/Neutrino/MB1242/MB1242Ex.java
deleted file mode 100644
index 2988ee0..0000000
--- a/PhotonCore/src/main/java/com/outoftheboxrobotics/photoncore/Neutrino/MB1242/MB1242Ex.java
+++ /dev/null
@@ -1,253 +0,0 @@
-package com.outoftheboxrobotics.photoncore.Neutrino.MB1242;
-
-import android.content.Context;
-
-import com.outoftheboxrobotics.photoncore.PhotonLynxModule;
-import com.outoftheboxrobotics.photoncore.ReflectionUtils;
-import com.qualcomm.ftccommon.FtcEventLoop;
-import com.qualcomm.hardware.lynx.LynxI2cDeviceSynch;
-import com.qualcomm.hardware.lynx.LynxModule;
-import com.qualcomm.hardware.lynx.LynxNackException;
-import com.qualcomm.hardware.lynx.LynxUsbDevice;
-import com.qualcomm.hardware.lynx.LynxUsbDeviceDelegate;
-import com.qualcomm.hardware.lynx.LynxUsbDeviceImpl;
-import com.qualcomm.hardware.lynx.Supplier;
-import com.qualcomm.hardware.lynx.commands.LynxCommand;
-import com.qualcomm.hardware.lynx.commands.LynxRespondable;
-import com.qualcomm.hardware.lynx.commands.core.LynxI2cReadMultipleBytesCommand;
-import com.qualcomm.hardware.lynx.commands.core.LynxI2cReadStatusQueryCommand;
-import com.qualcomm.hardware.lynx.commands.core.LynxI2cReadStatusQueryResponse;
-import com.qualcomm.hardware.lynx.commands.core.LynxI2cWriteReadMultipleBytesCommand;
-import com.qualcomm.robotcore.eventloop.opmode.OpMode;
-import com.qualcomm.robotcore.eventloop.opmode.OpModeManager;
-import com.qualcomm.robotcore.eventloop.opmode.OpModeManagerImpl;
-import com.qualcomm.robotcore.eventloop.opmode.OpModeManagerNotifier;
-import com.qualcomm.robotcore.exception.RobotCoreException;
-import com.qualcomm.robotcore.hardware.DistanceSensor;
-import com.qualcomm.robotcore.hardware.HardwareDevice;
-import com.qualcomm.robotcore.hardware.HardwareMap;
-import com.qualcomm.robotcore.hardware.I2cAddr;
-import com.qualcomm.robotcore.hardware.I2cDeviceSynch;
-import com.qualcomm.robotcore.hardware.I2cDeviceSynchDevice;
-import com.qualcomm.robotcore.hardware.I2cDeviceSynchImplOnSimple;
-import com.qualcomm.robotcore.hardware.I2cDeviceSynchSimple;
-import com.qualcomm.robotcore.hardware.TimestampedData;
-import com.qualcomm.robotcore.hardware.TimestampedI2cData;
-import com.qualcomm.robotcore.hardware.configuration.LynxConstants;
-import com.qualcomm.robotcore.hardware.configuration.annotations.DeviceProperties;
-import com.qualcomm.robotcore.hardware.configuration.annotations.I2cDeviceType;
-import com.qualcomm.robotcore.hardware.usb.RobotUsbDevice;
-import com.qualcomm.robotcore.util.RobotLog;
-import com.qualcomm.robotcore.util.TypeConversion;
-
-import org.firstinspires.ftc.ftccommon.external.OnCreateEventLoop;
-import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
-
-import java.lang.reflect.Field;
-import java.util.ArrayList;
-import java.util.HashMap;
-import java.util.concurrent.ConcurrentHashMap;
-import java.util.concurrent.atomic.AtomicBoolean;
-
-@I2cDeviceType
-@DeviceProperties(
-        name = "Photon MB1242",
-        description = "ultrasonic distance sensor",
-        xmlTag = "MB1242"
-)
-public class MB1242Ex extends I2cDeviceSynchDevice<I2cDeviceSynch> implements DistanceSensor {
-    private static OpModeManagerImpl opModeManager;
-    private LynxModule module;
-    private int bus;
-    private I2cAddr address;
-    private long lastRun = 0;
-
-    private short lastReading = 0;
-
-    public MB1242Ex(I2cDeviceSynch i2cDeviceSynch) {
-        super(i2cDeviceSynch, true);
-
-        deviceClient.setI2cAddress(I2cAddr.create7bit(0x70));
-        registerArmingStateCallback(false);
-        deviceClient.engage();
-
-        setupVariables(i2cDeviceSynch);
-    }
-
-    public double getDistanceAsync(DistanceUnit unit) {
-        if(System.currentTimeMillis() > (lastRun + 20)) {
-            try {
-                module.acquireI2cLockWhile(() -> {
-                    LynxCommand<?> tx = new LynxI2cWriteReadMultipleBytesCommand(module, bus, address, 0, 2);
-
-                    tx.send();
-
-                    byte[] data = pollForReadResult(2);
-
-                    if(data.length == 2){
-                        lastReading = TypeConversion.byteArrayToShort(data);
-                        deviceClient.write(TypeConversion.intToByteArray(0x51));
-                        lastRun = System.currentTimeMillis();
-                    }
-                    return null;
-                });
-            } catch (InterruptedException e) {
-                e.printStackTrace();
-            } catch (RobotCoreException e) {
-                e.printStackTrace();
-            } catch (LynxNackException e) {
-                e.printStackTrace();
-            }
-        }
-        return unit.fromCm(lastReading);
-    }
-
-    @Override
-    public double getDistance(DistanceUnit unit) {
-        try {
-            module.acquireI2cLockWhile(() -> {
-                LynxCommand<?> tx = new LynxI2cReadMultipleBytesCommand(module, bus, address, 2);
-                tx.send();
-
-                byte[] data = pollForReadResult(2);
-
-                while(data.length == 0){
-                    tx = new LynxI2cReadMultipleBytesCommand(module, bus, address, 2);
-                    try {
-                        tx.send();
-                    } catch (InterruptedException e) {
-                        e.printStackTrace();
-                    } catch (LynxNackException e) {
-                        e.printStackTrace();
-                    }
-                    data = pollForReadResult(2);
-                }
-
-                if(data.length == 2){
-                    lastReading = TypeConversion.byteArrayToShort(data);
-                    deviceClient.write(TypeConversion.intToByteArray(0x51));
-                    lastRun = System.currentTimeMillis();
-                }
-                return null;
-            });
-        } catch (InterruptedException e) {
-            e.printStackTrace();
-        } catch (RobotCoreException e) {
-            e.printStackTrace();
-        } catch (LynxNackException e) {
-            e.printStackTrace();
-        }
-
-        return unit.fromCm(lastReading);
-    }
-
-    @Override
-    public Manufacturer getManufacturer() {
-        return Manufacturer.Other;
-    }
-
-    @Override
-    public String getDeviceName() {
-        return "MB1242 sensor";
-    }
-
-    @Override
-    public String getConnectionInfo() {
-        return "Connected lol";
-    }
-
-    @Override
-    public int getVersion() {
-        return 1;
-    }
-
-    @Override
-    protected boolean doInitialize() {
-        return true;
-    }
-
-    @Override
-    public void resetDeviceConfigurationForOpMode() {
-
-    }
-
-    @Override
-    public void close() {
-
-    }
-
-    private void setupVariables(I2cDeviceSynch i2cDeviceSynch){
-        LynxI2cDeviceSynch device = null;
-        try {
-            I2cDeviceSynchImplOnSimple simple = (I2cDeviceSynchImplOnSimple) i2cDeviceSynch;
-            //Cool first part done
-            //Now we can safely make the assumption that the underlying i2cdevicesynchsimple is a lynxi2cdevicesynch
-            Field field = ReflectionUtils.getField(simple.getClass(), "i2cDeviceSynchSimple");
-            field.setAccessible(true);
-            device = (LynxI2cDeviceSynch) field.get(simple);
-            //Lets also bump up the bus speed while we are here
-            //Tbh it doesn't affect anything when just reading 2 bytes but why not
-            device.setBusSpeed(LynxI2cDeviceSynch.BusSpeed.FAST_400K);
-        } catch (IllegalAccessException e) {
-            e.printStackTrace();
-        }
-        LynxModule module = null;
-        try {
-            //Module this is being run on, sometimes the module doesn't exist
-            module = (LynxModule) ReflectionUtils.getField(device.getClass(), "module").get(device);
-        } catch (IllegalAccessException e) {
-            e.printStackTrace();
-        }
-
-        this.module = module;
-
-        int bus = 0;
-        try {
-            //Bus that the i2c device is on. Why is this one the only one IntelliJ is mad at?
-            bus = (int) ReflectionUtils.getField(device.getClass(), "bus").get(device);
-        } catch (IllegalAccessException e) {
-            e.printStackTrace();
-        }
-
-        this.bus = bus;
-
-        this.address = I2cAddr.create7bit(0x70);
-    }
-
-    protected byte[] pollForReadResult(int creg) {
-        // Poll until the data is available
-        boolean keepTrying = true;
-        while (keepTrying)
-        {
-            LynxI2cReadStatusQueryCommand readStatus = new LynxI2cReadStatusQueryCommand(module, this.bus, creg);
-            try {
-                LynxI2cReadStatusQueryResponse response = readStatus.sendReceive();
-                long now = System.nanoTime();
-                return response.getBytes();
-            }
-            catch (LynxNackException e)
-            {
-                switch (e.getNack().getNackReasonCodeAsEnum())
-                {
-                    case I2C_MASTER_BUSY:               // TODO: REVIEW: is this ever actually returned in this situation?
-                    case I2C_OPERATION_IN_PROGRESS:
-                        // We used to sleep for 3ms while waiting for the result to avoid a "busy loop", but that
-                        // caused a serious performance hit over what we could get otherwise, at least on the CH.
-                        // Besides, we're not *truly* busy looping, we still end up waiting for the module's response
-                        // and what not.
-
-                        //try { Thread.sleep(msBusyWait); } catch (InterruptedException ignored) { Thread.currentThread().interrupt(); }
-                        continue;// This is an internal error of some sort
-                    default:
-                        keepTrying = false;
-                        break;
-                }
-            }
-            catch (InterruptedException|RuntimeException e)
-            {
-                keepTrying = false;
-            }
-        }
-        return new byte[0];
-    }
-}
\ No newline at end of file
diff --git a/PhotonCore/src/main/java/com/outoftheboxrobotics/photoncore/Neutrino/MB1242/MB1242Ex.kt b/PhotonCore/src/main/java/com/outoftheboxrobotics/photoncore/Neutrino/MB1242/MB1242Ex.kt
new file mode 100644
index 0000000..f9940ee
--- /dev/null
+++ b/PhotonCore/src/main/java/com/outoftheboxrobotics/photoncore/Neutrino/MB1242/MB1242Ex.kt
@@ -0,0 +1,185 @@
+package com.outoftheboxrobotics.photoncore.Neutrino.MB1242
+
+import com.outoftheboxrobotics.photoncore.ReflectionUtils.getField
+import com.qualcomm.hardware.lynx.LynxI2cDeviceSynch
+import com.qualcomm.hardware.lynx.LynxModule
+import com.qualcomm.hardware.lynx.LynxNackException
+import com.qualcomm.hardware.lynx.commands.LynxCommand
+import com.qualcomm.hardware.lynx.commands.core.LynxI2cReadMultipleBytesCommand
+import com.qualcomm.hardware.lynx.commands.core.LynxI2cReadStatusQueryCommand
+import com.qualcomm.hardware.lynx.commands.core.LynxI2cWriteReadMultipleBytesCommand
+import com.qualcomm.hardware.lynx.commands.standard.LynxNack.StandardReasonCode
+import com.qualcomm.robotcore.eventloop.opmode.OpModeManagerImpl
+import com.qualcomm.robotcore.exception.RobotCoreException
+import com.qualcomm.robotcore.hardware.*
+import com.qualcomm.robotcore.hardware.HardwareDevice.Manufacturer
+import com.qualcomm.robotcore.hardware.configuration.annotations.DeviceProperties
+import com.qualcomm.robotcore.hardware.configuration.annotations.I2cDeviceType
+import com.qualcomm.robotcore.util.TypeConversion
+import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit
+
+@I2cDeviceType
+@DeviceProperties(name = "Photon MB1242", description = "ultrasonic distance sensor", xmlTag = "MB1242")
+class MB1242Ex(i2cDeviceSynch: I2cDeviceSynch) : I2cDeviceSynchDevice<I2cDeviceSynch?>(i2cDeviceSynch, true),
+    DistanceSensor {
+    private var module: LynxModule? = null
+    private var bus = 0
+    private var address: I2cAddr? = null
+    private var lastRun: Long = 0
+    private var lastReading: Short = 0
+
+    init {
+        deviceClient!!.i2cAddress = I2cAddr.create7bit(0x70)
+        registerArmingStateCallback(false)
+        deviceClient!!.engage()
+        setupVariables(i2cDeviceSynch)
+    }
+
+    fun getDistanceAsync(unit: DistanceUnit): Double {
+        if (System.currentTimeMillis() > lastRun + 20) {
+            try {
+                module!!.acquireI2cLockWhile<Any?> {
+                    val tx: LynxCommand<*> = LynxI2cWriteReadMultipleBytesCommand(module, bus, address, 0, 2)
+                    tx.send()
+                    val data = pollForReadResult(2)
+                    if (data.size == 2) {
+                        lastReading = TypeConversion.byteArrayToShort(data)
+                        deviceClient!!.write(TypeConversion.intToByteArray(0x51))
+                        lastRun = System.currentTimeMillis()
+                    }
+                    null
+                }
+            } catch (e: InterruptedException) {
+                e.printStackTrace()
+            } catch (e: RobotCoreException) {
+                e.printStackTrace()
+            } catch (e: LynxNackException) {
+                e.printStackTrace()
+            }
+        }
+        return unit.fromCm(lastReading.toDouble())
+    }
+
+    override fun getDistance(unit: DistanceUnit): Double {
+        try {
+            module!!.acquireI2cLockWhile<Any?> {
+                var tx: LynxCommand<*> = LynxI2cReadMultipleBytesCommand(module, bus, address, 2)
+                tx.send()
+                var data = pollForReadResult(2)
+                while (data.size == 0) {
+                    tx = LynxI2cReadMultipleBytesCommand(module, bus, address, 2)
+                    try {
+                        tx.send()
+                    } catch (e: InterruptedException) {
+                        e.printStackTrace()
+                    } catch (e: LynxNackException) {
+                        e.printStackTrace()
+                    }
+                    data = pollForReadResult(2)
+                }
+                if (data.size == 2) {
+                    lastReading = TypeConversion.byteArrayToShort(data)
+                    deviceClient!!.write(TypeConversion.intToByteArray(0x51))
+                    lastRun = System.currentTimeMillis()
+                }
+                null
+            }
+        } catch (e: InterruptedException) {
+            e.printStackTrace()
+        } catch (e: RobotCoreException) {
+            e.printStackTrace()
+        } catch (e: LynxNackException) {
+            e.printStackTrace()
+        }
+        return unit.fromCm(lastReading.toDouble())
+    }
+
+    override fun getManufacturer(): Manufacturer {
+        return Manufacturer.Other
+    }
+
+    override fun getDeviceName(): String {
+        return "MB1242 sensor"
+    }
+
+    override fun getConnectionInfo(): String {
+        return "Connected lol"
+    }
+
+    override fun getVersion(): Int {
+        return 1
+    }
+
+    override fun doInitialize(): Boolean {
+        return true
+    }
+
+    override fun resetDeviceConfigurationForOpMode() {}
+    override fun close() {}
+    private fun setupVariables(i2cDeviceSynch: I2cDeviceSynch) {
+        var device: LynxI2cDeviceSynch? = null
+        try {
+            val simple = i2cDeviceSynch as I2cDeviceSynchImplOnSimple
+            //Cool first part done
+            //Now we can safely make the assumption that the underlying i2cdevicesynchsimple is a lynxi2cdevicesynch
+            val field = getField(simple.javaClass, "i2cDeviceSynchSimple")
+            field!!.isAccessible = true
+            device = field[simple] as LynxI2cDeviceSynch
+            //Lets also bump up the bus speed while we are here
+            //Tbh it doesn't affect anything when just reading 2 bytes but why not
+            device!!.setBusSpeed(LynxI2cDeviceSynch.BusSpeed.FAST_400K)
+        } catch (e: IllegalAccessException) {
+            e.printStackTrace()
+        }
+        var module: LynxModule? = null
+        try {
+            //Module this is being run on, sometimes the module doesn't exist
+            module = getField(device!!.javaClass, "module")!![device] as LynxModule
+        } catch (e: IllegalAccessException) {
+            e.printStackTrace()
+        }
+        this.module = module
+        var bus = 0
+        try {
+            //Bus that the i2c device is on. Why is this one the only one IntelliJ is mad at?
+            bus = getField(device!!.javaClass, "bus")!![device] as Int
+        } catch (e: IllegalAccessException) {
+            e.printStackTrace()
+        }
+        this.bus = bus
+        address = I2cAddr.create7bit(0x70)
+    }
+
+    protected fun pollForReadResult(creg: Int): ByteArray {
+        // Poll until the data is available
+        var keepTrying = true
+        while (keepTrying) {
+            val readStatus = LynxI2cReadStatusQueryCommand(module, bus, creg)
+            keepTrying = try {
+                val response = readStatus.sendReceive()
+                val now = System.nanoTime()
+                return response.bytes
+            } catch (e: LynxNackException) {
+                when (e.nack.nackReasonCodeAsEnum) {
+                    StandardReasonCode.I2C_MASTER_BUSY, StandardReasonCode.I2C_OPERATION_IN_PROGRESS ->                         // We used to sleep for 3ms while waiting for the result to avoid a "busy loop", but that
+                        // caused a serious performance hit over what we could get otherwise, at least on the CH.
+                        // Besides, we're not *truly* busy looping, we still end up waiting for the module's response
+                        // and what not.
+
+                        //try { Thread.sleep(msBusyWait); } catch (InterruptedException ignored) { Thread.currentThread().interrupt(); }
+                        continue  // This is an internal error of some sort
+                    else -> false
+                }
+            } catch (e: InterruptedException) {
+                false
+            } catch (e: RuntimeException) {
+                false
+            }
+        }
+        return ByteArray(0)
+    }
+
+    companion object {
+        private val opModeManager: OpModeManagerImpl? = null
+    }
+}
\ No newline at end of file
diff --git a/PhotonCore/src/main/java/com/outoftheboxrobotics/photoncore/Neutrino/Rev2MSensor/Rev2mDistanceSensorEx.java b/PhotonCore/src/main/java/com/outoftheboxrobotics/photoncore/Neutrino/Rev2MSensor/Rev2mDistanceSensorEx.java
deleted file mode 100644
index 3c29e99..0000000
--- a/PhotonCore/src/main/java/com/outoftheboxrobotics/photoncore/Neutrino/Rev2MSensor/Rev2mDistanceSensorEx.java
+++ /dev/null
@@ -1,70 +0,0 @@
-package com.outoftheboxrobotics.photoncore.Neutrino.Rev2MSensor;
-
-import com.qualcomm.hardware.lynx.LynxI2cDeviceSynch;
-import com.qualcomm.hardware.rev.Rev2mDistanceSensor;
-import com.qualcomm.robotcore.hardware.I2cDeviceSynch;
-import com.qualcomm.robotcore.util.ElapsedTime;
-import com.qualcomm.robotcore.util.TypeConversion;
-
-import static com.qualcomm.hardware.stmicroelectronics.VL53L0X.Register.RESULT_RANGE_STATUS;
-
-public class Rev2mDistanceSensorEx extends Rev2mDistanceSensor {
-
-    long measurement_timing_budget_us;
-
-    protected int io_timeout = 0;
-    protected ElapsedTime ioElapsedTime;
-
-    private long lastRead = 0;
-    private int rangeCache;
-
-    public Rev2mDistanceSensorEx(I2cDeviceSynch deviceClient, boolean owned) {
-        super(deviceClient, owned);
-        if(deviceClient instanceof LynxI2cDeviceSynch){
-            ((LynxI2cDeviceSynch) deviceClient).setBusSpeed(LynxI2cDeviceSynch.BusSpeed.FAST_400K);
-        }
-    }
-
-    @Override
-    protected int readRangeContinuousMillimeters() {
-        if(io_timeout > 0) {
-            ioElapsedTime.reset();
-        }
-        // assumptions: Linearity Corrective Gain is 1000 (default);
-        // fractional ranging is not enabled
-
-        if(System.currentTimeMillis() - lastRead > (measurement_timing_budget_us / 1000.0)) {
-            byte[] readBytes = deviceClient.read(RESULT_RANGE_STATUS.bVal + 10, 2);
-            rangeCache = (int) TypeConversion.byteArrayToShort(readBytes);
-            lastRead = System.currentTimeMillis();
-        }
-
-        return rangeCache;
-    }
-
-    /**
-     * Sets ranging profile of the device
-     * DEFAULT: 30 hz update, medium accuracy
-     * HIGH_SPEED: 50 hz update, low accuracy
-     * HIGH_ACCURACY: 10 hz update, high accuracy
-     */
-    public void setRangingProfile(RANGING_PROFILE rangingProfile){
-        switch (rangingProfile){
-            case DEFAULT:
-                this.setMeasurementTimingBudget(33000);
-                break;
-            case HIGH_SPEED:
-                this.setMeasurementTimingBudget(20000);
-                break;
-            case HIGH_ACCURACY:
-                this.setMeasurementTimingBudget(100000);
-                break;
-        }
-    }
-
-    public enum RANGING_PROFILE{
-        DEFAULT,
-        HIGH_SPEED,
-        HIGH_ACCURACY
-    }
-}
diff --git a/PhotonCore/src/main/java/com/outoftheboxrobotics/photoncore/Neutrino/Rev2MSensor/Rev2mDistanceSensorEx.kt b/PhotonCore/src/main/java/com/outoftheboxrobotics/photoncore/Neutrino/Rev2MSensor/Rev2mDistanceSensorEx.kt
new file mode 100644
index 0000000..1fcd839
--- /dev/null
+++ b/PhotonCore/src/main/java/com/outoftheboxrobotics/photoncore/Neutrino/Rev2MSensor/Rev2mDistanceSensorEx.kt
@@ -0,0 +1,56 @@
+package com.outoftheboxrobotics.photoncore.Neutrino.Rev2MSensor
+
+import com.qualcomm.hardware.lynx.LynxI2cDeviceSynch
+import com.qualcomm.hardware.rev.Rev2mDistanceSensor
+import com.qualcomm.robotcore.hardware.I2cDeviceSynch
+import com.qualcomm.robotcore.util.ElapsedTime
+import com.qualcomm.robotcore.util.TypeConversion
+
+class Rev2mDistanceSensorEx(deviceClient: I2cDeviceSynch?, owned: Boolean) : Rev2mDistanceSensor(deviceClient, owned) {
+    var measurement_timing_budget_us: Long = 0
+    protected var io_timeout = 0
+    protected var ioElapsedTime: ElapsedTime? = null
+    private var lastRead: Long = 0
+    private var rangeCache = 0
+
+    init {
+        if (deviceClient is LynxI2cDeviceSynch) {
+            (deviceClient as LynxI2cDeviceSynch).setBusSpeed(LynxI2cDeviceSynch.BusSpeed.FAST_400K)
+        }
+    }
+
+    override fun readRangeContinuousMillimeters(): Int {
+        if (io_timeout > 0) {
+            ioElapsedTime.reset()
+        }
+        // assumptions: Linearity Corrective Gain is 1000 (default);
+        // fractional ranging is not enabled
+        if (System.currentTimeMillis() - lastRead > measurement_timing_budget_us / 1000.0) {
+            val readBytes = deviceClient.read(Register.RESULT_RANGE_STATUS.bVal + 10, 2)
+            rangeCache = TypeConversion.byteArrayToShort(readBytes).toInt()
+            lastRead = System.currentTimeMillis()
+        }
+        return rangeCache
+    }
+
+    /**
+     * Sets ranging profile of the device
+     * DEFAULT: 30 hz update, medium accuracy
+     * HIGH_SPEED: 50 hz update, low accuracy
+     * HIGH_ACCURACY: 10 hz update, high accuracy
+     */
+    fun setRangingProfile(rangingProfile: RANGING_PROFILE?) {
+        when (rangingProfile) {
+            RANGING_PROFILE.DEFAULT -> setMeasurementTimingBudget(33000)
+            RANGING_PROFILE.HIGH_SPEED -> setMeasurementTimingBudget(20000)
+            RANGING_PROFILE.HIGH_ACCURACY -> setMeasurementTimingBudget(100000)
+            else -> {}
+        }
+    }
+
+    enum class RANGING_PROFILE {
+        DEFAULT,
+        HIGH_SPEED,
+        HIGH_ACCURACY
+    }
+}
diff --git a/PhotonCore/src/main/java/com/outoftheboxrobotics/photoncore/Neutrino/RevColorSensor/BroadcomColorSensorImplEx.java b/PhotonCore/src/main/java/com/outoftheboxrobotics/photoncore/Neutrino/RevColorSensor/BroadcomColorSensorImplEx.java
deleted file mode 100644
index feba160..0000000
--- a/PhotonCore/src/main/java/com/outoftheboxrobotics/photoncore/Neutrino/RevColorSensor/BroadcomColorSensorImplEx.java
+++ /dev/null
@@ -1,429 +0,0 @@
-/*
-Copyright (c) 2019 REV Robotics LLC
-
-All rights reserved.
-
-Redistribution and use in source and binary forms, with or without modification,
-are permitted (subject to the limitations in the disclaimer below) provided that
-the following conditions are met:
-
-Redistributions of source code must retain the above copyright notice, this list
-of conditions and the following disclaimer.
-
-Redistributions in binary form must reproduce the above copyright notice, this
-list of conditions and the following disclaimer in the documentation and/or
-other materials provided with the distribution.
-
-Neither the name of REV Robotics LLC nor the names of his contributors may be used to
-endorse or promote products derived from this software without specific prior
-written permission.
-
-NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
-LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
-THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
-ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
-FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
-DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
-SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR
-TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
-THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-*/
-package com.outoftheboxrobotics.photoncore.Neutrino.RevColorSensor;
-
-import android.graphics.Color;
-
-import com.qualcomm.hardware.broadcom.BroadcomColorSensor;
-import com.qualcomm.hardware.lynx.LynxI2cDeviceSynch;
-import com.qualcomm.robotcore.hardware.I2cAddr;
-import com.qualcomm.robotcore.hardware.I2cAddrConfig;
-import com.qualcomm.robotcore.hardware.I2cDeviceSynch;
-import com.qualcomm.robotcore.hardware.I2cDeviceSynchDeviceWithParameters;
-import com.qualcomm.robotcore.hardware.I2cDeviceSynchSimple;
-import com.qualcomm.robotcore.hardware.I2cWaitControl;
-import com.qualcomm.robotcore.hardware.Light;
-import com.qualcomm.robotcore.hardware.NormalizedRGBA;
-import com.qualcomm.robotcore.util.Range;
-import com.qualcomm.robotcore.util.RobotLog;
-import com.qualcomm.robotcore.util.TypeConversion;
-
-import java.nio.ByteOrder;
-
-/**
- * BroadcomColorSensorImpl is used to support the Rev Robotics V3 color sensor
- */
-@SuppressWarnings("WeakerAccess")
-public abstract class BroadcomColorSensorImplEx extends I2cDeviceSynchDeviceWithParameters<I2cDeviceSynchSimple, BroadcomColorSensor.Parameters>
-        implements BroadcomColorSensor, I2cAddrConfig, Light
-{
-    //----------------------------------------------------------------------------------------------
-    // State
-    //----------------------------------------------------------------------------------------------
-
-    public static final String TAG = "BroadcomColorSensorImpl";
-    NormalizedRGBA colors = new NormalizedRGBA();
-    int red = 0, green = 0, blue = 0, alpha = 0;
-    float softwareGain = 1;
-
-    long lastRead = 0, measurementDelay = 100;
-
-    //----------------------------------------------------------------------------------------------
-    // Construction
-    //----------------------------------------------------------------------------------------------
-
-    protected BroadcomColorSensorImplEx(BroadcomColorSensor.Parameters params,
-                                        I2cDeviceSynchSimple deviceClient,
-                                        boolean isOwned)
-    {
-        super(deviceClient, isOwned, params);
-        this.deviceClient.setLogging(this.parameters.loggingEnabled);
-        this.deviceClient.setLoggingTag(this.parameters.loggingTag);
-
-        // We ask for an initial call back here; that will eventually call internalInitialize()
-        this.registerArmingStateCallback(true);
-
-        this.engage();
-        internalInitialize(parameters);
-
-        if(deviceClient instanceof LynxI2cDeviceSynch){
-            ((LynxI2cDeviceSynch) deviceClient).setBusSpeed(LynxI2cDeviceSynch.BusSpeed.FAST_400K);
-        }
-    }
-
-    //----------------------------------------------------------------------------------------------
-    // Initialization
-    //----------------------------------------------------------------------------------------------
-
-    @Override
-    protected synchronized boolean internalInitialize(Parameters parameters)
-    {
-        RobotLog.vv(TAG, "internalInitialize()...");
-        try {
-            if (this.parameters.deviceId != parameters.deviceId)
-            {
-                throw new IllegalArgumentException(String.format("can't change device types (modify existing params instead): old=%d new=%d", this.parameters.deviceId, parameters.deviceId));
-            }
-
-            // Remember the parameters for future use
-            this.parameters = parameters.clone();
-
-            // Make sure we're talking to the correct I2c device
-            this.setI2cAddress(parameters.i2cAddr);
-
-            // Can't do anything if we're not really talking to the hardware
-            if (!this.deviceClient.isArmed())
-            {
-                return false;
-            }
-
-            // Verify that that's a color sensor!
-            byte id = this.getDeviceID();
-            if (id != parameters.deviceId)
-            {
-                RobotLog.ee(TAG, "unexpected Broadcom color sensor chipid: found=%d expected=%d", id, parameters.deviceId);
-                return false;
-            }
-
-            // sanity check: before
-            dumpState();
-
-            // set the gain, LED parameters and resolution
-            setHardwareGain(parameters.gain);
-            setLEDParameters(parameters.pulseModulation, parameters.ledCurrent);
-            setProximityPulseCount(parameters.proximityPulseCount);
-            setPSRateAndRes(parameters.proximityResolution, parameters.proximityMeasRate);
-            setLSRateAndRes(parameters.lightSensorResolution, parameters.lightSensorMeasRate);
-
-            // Enable the device
-            enable();
-
-            // sanity check: after
-            dumpState();
-
-            // Set up a read-ahead, if supported and requested
-            if (this.deviceClient instanceof I2cDeviceSynch && parameters.readWindow != null)
-            {
-                I2cDeviceSynch synch = ((I2cDeviceSynch)this.deviceClient);
-                synch.setReadWindow(parameters.readWindow);
-            }
-
-            return true;
-        }
-        finally
-        {
-            RobotLog.vv(TAG, "...internalInitialize()");
-        }
-    }
-
-    protected void dumpState()
-    {
-        int cb = 0x07;
-        RobotLog.logBytes(TAG, "state", read(Register.MAIN_CTRL, cb), cb);
-    }
-
-    protected synchronized void enable()
-    {
-        byte enabled = readMainCtrl();
-        RobotLog.vv(TAG, "enable() enabled=0x%02x...", enabled);
-
-        // enable ambient light sensor, ALS/IR channels, and proximity
-        write8(Register.MAIN_CTRL, MainControl.PS_EN.bVal | MainControl.LS_EN.bVal | MainControl.RGB_MODE.bVal);
-
-        enabled = readMainCtrl();
-        RobotLog.vv(TAG, "...enable() enabled=0x%02x", enabled);
-
-        return;
-    }
-
-    protected synchronized void disable()
-    {
-        byte enabled = readMainCtrl();
-        RobotLog.vv(TAG, "disable() enabled=0x%02x...", enabled);
-
-        write8(Register.MAIN_CTRL, enabled & ~(MainControl.PS_EN.bVal | MainControl.LS_EN.bVal | MainControl.RGB_MODE.bVal));
-        enabled = readMainCtrl();
-
-        RobotLog.vv(TAG, "...disable() enabled=0x%02x", enabled);
-    }
-
-    protected boolean testBits(byte value, byte desired)
-    {
-        return testBits(value, desired, desired);
-    }
-    protected boolean testBits(byte value, byte mask, byte desired)
-    {
-        return (value & mask) == desired;
-    }
-
-    protected byte readMainCtrl()
-    {
-        return read8(Register.MAIN_CTRL);
-    }
-
-    protected void setProximityPulseCount(int proximityPulseCount)
-    {
-        RobotLog.vv(TAG, "setProximityPulseCount(0x%02x)", proximityPulseCount);
-        write8(Register.PS_PULSES, proximityPulseCount);
-    }
-
-    protected void setHardwareGain(Gain gain)
-    {
-        RobotLog.vv(TAG, "setGain(0x%02x)", gain.bVal);
-        write8(Register.LS_GAIN, gain.bVal);
-    }
-
-    protected void setPDrive(LEDCurrent ledDrive)
-    {
-        RobotLog.vv(TAG, "setPDrive(0x%02x)", ledDrive.bVal);
-        write8(Register.PS_LED, ledDrive.bVal);
-    }
-
-    public byte getDeviceID()
-    {
-        return this.read8(Register.PART_ID);
-    }
-
-    protected  void setLEDParameters(LEDPulseModulation pulseMod, LEDCurrent curr)
-    {
-        int val = (pulseMod.bVal << 4) | curr.bVal;
-        RobotLog.vv(TAG, "setLEDParameters(0x%02x)", (byte) val);
-        write8(Register.PS_LED, (byte) val);
-    }
-
-    protected void setPSRateAndRes(PSResolution res, PSMeasurementRate rate)
-    {
-        int val = (res.bVal << 3) | rate.bVal;
-        RobotLog.vv(TAG, "setPSMeasRate(0x%02x)", (byte) val);
-        write8(Register.PS_MEAS_RATE, (byte) val);
-
-        switch (rate){
-            case RES:
-                break;
-            case R6_25ms:
-                measurementDelay = 7;
-                break;
-            case R12_5ms:
-                measurementDelay = 13;
-                break;
-            case R25ms:
-                measurementDelay = 25;
-                break;
-            case R50ms:
-                measurementDelay = 50;
-                break;
-            case R100ms:
-                measurementDelay = 100;
-                break;
-            case R200ms:
-                measurementDelay = 200;
-                break;
-            case R400ms:
-                measurementDelay = 400;
-                break;
-        }
-    }
-
-    protected void setLSRateAndRes(LSResolution res, LSMeasurementRate rate)
-    {
-        int val = (res.bVal << 4) | rate.bVal;
-        RobotLog.vv(TAG, "setLSMeasRate(0x%02x)", (byte) val);
-        write8(Register.LS_MEAS_RATE, (byte) val);
-    }
-
-    //----------------------------------------------------------------------------------------------
-    // Interfaces
-    //----------------------------------------------------------------------------------------------
-
-    /** In this implementation, the {@link Color} methods return 16 bit unsigned values. */
-
-    @Override
-    public synchronized int red() { updateColors(); return this.red; }
-
-    @Override
-    public synchronized int green() { updateColors(); return this.green; }
-
-    @Override
-    public synchronized int blue() { updateColors(); return this.blue; }
-
-    @Override
-    public synchronized int alpha() { updateColors(); return this.alpha; }
-
-    @Override
-    public synchronized int argb() { return getNormalizedColors().toColor(); }
-
-    @Override
-    public void setGain(float newGain) { this.softwareGain = newGain; }
-
-    @Override
-    public float getGain() { return this.softwareGain; }
-
-    private void updateColors()
-    {
-        long now = System.currentTimeMillis();
-        if(now - lastRead > measurementDelay) {
-            byte[] data;
-
-            // Read red, green and blue values
-            final int cbRead = 9;
-            data = read(Register.LS_DATA_GREEN, cbRead);
-
-            final int dib = 0;
-            this.green = TypeConversion.unsignedShortToInt(TypeConversion.byteArrayToShort(data, dib, ByteOrder.LITTLE_ENDIAN));
-            this.blue = Range.clip((int) (1.55 * TypeConversion.unsignedShortToInt(TypeConversion.byteArrayToShort(
-                    data, dib + 3, ByteOrder.LITTLE_ENDIAN))), 0, 65535);
-            this.red = Range.clip((int) (1.07 * TypeConversion.unsignedShortToInt(TypeConversion.byteArrayToShort(
-                    data, dib + 6, ByteOrder.LITTLE_ENDIAN))), 0, 65535);
-
-            this.alpha = (this.red + this.green + this.blue) / 3;
-
-            // normalize to [0, 1]
-            this.colors.red = Range.clip(((float) this.red * this.softwareGain) / parameters.colorSaturation, 0f, 1f);
-            this.colors.green = Range.clip(((float) this.green * this.softwareGain) / parameters.colorSaturation, 0f, 1f);
-            this.colors.blue = Range.clip(((float) this.blue * this.softwareGain) / parameters.colorSaturation, 0f, 1f);
-
-            // apply inverse squared law of light to get readable brightness value, stored in alpha channel
-            // scale to 65535
-            float avg = (float) (this.red + this.green + this.blue) / 3;
-            this.colors.alpha = (float) (-(65535f / (Math.pow(avg, 2) + 65535)) + 1);
-
-            lastRead = System.currentTimeMillis();
-        }
-    }
-
-    @Override
-    public NormalizedRGBA getNormalizedColors() { updateColors(); return this.colors; }
-
-    @Override
-    public synchronized void enableLed(boolean enable)
-    {
-        // We can't directly control the LED with I2C; it's always on
-        // ignore; used to throw an error, but the default opmode can try to turn off
-        // (for range sensor variant) so got constant exceptions
-    }
-
-    @Override public boolean isLightOn()
-    {
-        return true;
-    }
-
-    @Override
-    public synchronized I2cAddr getI2cAddress()
-    {
-        return this.deviceClient.getI2cAddress();
-    }
-
-    @Override
-    public synchronized void setI2cAddress(I2cAddr i2cAddr)
-    {
-        this.parameters.i2cAddr = i2cAddr;
-        this.deviceClient.setI2cAddress(i2cAddr);
-    }
-
-    //----------------------------------------------------------------------------------------------
-    // HardwareDevice
-    //----------------------------------------------------------------------------------------------
-
-    @Override
-    public String getDeviceName() { return "Broadcom I2C Color Sensor"; }
-
-    @Override
-    public Manufacturer getManufacturer() { return Manufacturer.Broadcom; }
-
-    @Override
-    public void resetDeviceConfigurationForOpMode()
-    {
-        super.resetDeviceConfigurationForOpMode();
-        this.softwareGain = 1;
-    }
-
-    //----------------------------------------------------------------------------------------------
-    // Utility
-    //----------------------------------------------------------------------------------------------
-
-    protected int readUnsignedByte(final Register reg)
-    {
-        return TypeConversion.unsignedByteToInt(read8(reg));
-    }
-
-    protected int readUnsignedShort(final Register reg, ByteOrder byteOrder)
-    {
-        byte[] data = read(reg, 2);
-        return TypeConversion.unsignedShortToInt(TypeConversion.byteArrayToShort(data, 0, byteOrder));
-    }
-
-    @Override
-    public synchronized byte read8(final Register reg)
-    {
-        return deviceClient.read8(reg.bVal);
-    }
-
-    @Override
-    public synchronized byte[] read(final Register reg, final int cb)
-    {
-        return deviceClient.read(reg.bVal, cb);
-    }
-
-    @Override
-    public synchronized void write8(Register reg, int data)
-    {
-        this.deviceClient.write8(reg.bVal, data, I2cWaitControl.WRITTEN);
-    }
-
-    @Override public void write(Register reg, byte[] data)
-    {
-        this.deviceClient.write(reg.bVal, data, I2cWaitControl.WRITTEN);
-    }
-
-    protected void delay(int ms)
-    {
-        try
-        {
-            Thread.sleep(ms);
-        }
-        catch (InterruptedException e)
-        {
-            Thread.currentThread().interrupt();
-        }
-    }
-}
diff --git a/PhotonCore/src/main/java/com/outoftheboxrobotics/photoncore/Neutrino/RevColorSensor/BroadcomColorSensorImplEx.kt b/PhotonCore/src/main/java/com/outoftheboxrobotics/photoncore/Neutrino/RevColorSensor/BroadcomColorSensorImplEx.kt
new file mode 100644
index 0000000..f049364
--- /dev/null
+++ b/PhotonCore/src/main/java/com/outoftheboxrobotics/photoncore/Neutrino/RevColorSensor/BroadcomColorSensorImplEx.kt
@@ -0,0 +1,412 @@
+/*
+Copyright (c) 2019 REV Robotics LLC
+
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without modification,
+are permitted (subject to the limitations in the disclaimer below) provided that
+the following conditions are met:
+
+Redistributions of source code must retain the above copyright notice, this list
+of conditions and the following disclaimer.
+
+Redistributions in binary form must reproduce the above copyright notice, this
+list of conditions and the following disclaimer in the documentation and/or
+other materials provided with the distribution.
+
+Neither the name of REV Robotics LLC nor the names of his contributors may be used to
+endorse or promote products derived from this software without specific prior
+written permission.
+
+NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
+LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR
+TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
+THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+package com.outoftheboxrobotics.photoncore.Neutrino.RevColorSensor
+
+import com.qualcomm.hardware.broadcom.BroadcomColorSensor
+import com.qualcomm.hardware.broadcom.BroadcomColorSensor.*
+import com.qualcomm.hardware.lynx.LynxI2cDeviceSynch
+import com.qualcomm.robotcore.hardware.*
+import com.qualcomm.robotcore.hardware.HardwareDevice.Manufacturer
+import com.qualcomm.robotcore.util.Range
+import com.qualcomm.robotcore.util.RobotLog
+import com.qualcomm.robotcore.util.TypeConversion
+import java.nio.ByteOrder
+
+/**
+ * BroadcomColorSensorImpl is used to support the Rev Robotics V3 color sensor
+ */
+abstract class BroadcomColorSensorImplEx protected constructor(
+    params: BroadcomColorSensor.Parameters?,
+    deviceClient: I2cDeviceSynchSimple?,
+    isOwned: Boolean
+) : I2cDeviceSynchDeviceWithParameters<I2cDeviceSynchSimple?, BroadcomColorSensor.Parameters>(
+    deviceClient,
+    isOwned,
+    params!!
+), BroadcomColorSensor, I2cAddrConfig, Light {
+    var colors = NormalizedRGBA()
+    var red = 0
+    var green = 0
+    var blue = 0
+    var alpha = 0
+    var softwareGain = 1f
+    var lastRead: Long = 0
+    var measurementDelay: Long = 100
+
+    //----------------------------------------------------------------------------------------------
+    // Construction
+    //----------------------------------------------------------------------------------------------
+    init {
+        this.deviceClient!!.logging = parameters.loggingEnabled
+        this.deviceClient!!.loggingTag = parameters.loggingTag
+
+        // We ask for an initial call back here; that will eventually call internalInitialize()
+        registerArmingStateCallback(true)
+        engage()
+        internalInitialize(parameters)
+        if (deviceClient is LynxI2cDeviceSynch) {
+            deviceClient.setBusSpeed(LynxI2cDeviceSynch.BusSpeed.FAST_400K)
+        }
+    }
+
+    //----------------------------------------------------------------------------------------------
+    // Initialization
+    //----------------------------------------------------------------------------------------------
+    @Synchronized
+    override fun internalInitialize(parameters: BroadcomColorSensor.Parameters): Boolean {
+        RobotLog.vv(TAG, "internalInitialize()...")
+        return try {
+            require(this.parameters.deviceId == parameters.deviceId) {
+                String.format(
+                    "can't change device types (modify existing params instead): old=%d new=%d",
+                    this.parameters.deviceId,
+                    parameters.deviceId
+                )
+            }
+
+            // Remember the parameters for future use
+            this.parameters = parameters.clone()
+
+            // Make sure we're talking to the correct I2c device
+            this.i2cAddress = parameters.i2cAddr
+
+            // Can't do anything if we're not really talking to the hardware
+            if (!deviceClient!!.isArmed) {
+                return false
+            }
+
+            // Verify that that's a color sensor!
+            val id = this.deviceID
+            if (id.toInt() != parameters.deviceId) {
+                RobotLog.ee(
+                    TAG,
+                    "unexpected Broadcom color sensor chipid: found=%d expected=%d",
+                    id,
+                    parameters.deviceId
+                )
+                return false
+            }
+
+            // sanity check: before
+            dumpState()
+
+            // set the gain, LED parameters and resolution
+            setHardwareGain(parameters.gain)
+            setLEDParameters(parameters.pulseModulation, parameters.ledCurrent)
+            setProximityPulseCount(parameters.proximityPulseCount)
+            setPSRateAndRes(
+                BroadcomColorSensor.Parameters.proximityResolution,
+                parameters.proximityMeasRate
+            )
+            setLSRateAndRes(
+                BroadcomColorSensor.Parameters.lightSensorResolution,
+                parameters.lightSensorMeasRate
+            )
+
+            // Enable the device
+            enable()
+
+            // sanity check: after
+            dumpState()
+
+            // Set up a read-ahead, if supported and requested
+            if (deviceClient is I2cDeviceSynch && parameters.readWindow != null) {
+                val synch = deviceClient as I2cDeviceSynch?
+                synch!!.readWindow = parameters.readWindow
+            }
+            true
+        } finally {
+            RobotLog.vv(TAG, "...internalInitialize()")
+        }
+    }
+
+    protected fun dumpState() {
+        val cb = 0x07
+        RobotLog.logBytes(TAG, "state", read(BroadcomColorSensor.Register.MAIN_CTRL, cb), cb)
+    }
+
+    @Synchronized
+    protected fun enable() {
+        var enabled = readMainCtrl()
+        RobotLog.vv(TAG, "enable() enabled=0x%02x...", enabled)
+
+        // enable ambient light sensor, ALS/IR channels, and proximity
+        write8(
+            BroadcomColorSensor.Register.MAIN_CTRL,
+            MainControl.PS_EN.bVal.toInt() or MainControl.LS_EN.bVal.toInt() or MainControl.RGB_MODE.bVal.toInt()
+        )
+        enabled = readMainCtrl()
+        RobotLog.vv(TAG, "...enable() enabled=0x%02x", enabled)
+        return
+    }
+
+    @Synchronized
+    protected fun disable() {
+        var enabled = readMainCtrl()
+        RobotLog.vv(TAG, "disable() enabled=0x%02x...", enabled)
+        write8(
+            BroadcomColorSensor.Register.MAIN_CTRL,
+            enabled.toInt() and (MainControl.PS_EN.bVal.toInt() or MainControl.LS_EN.bVal.toInt() or MainControl.RGB_MODE.bVal.toInt()).inv()
+        )
+        enabled = readMainCtrl()
+        RobotLog.vv(TAG, "...disable() enabled=0x%02x", enabled)
+    }
+
+    protected fun testBits(value: Byte, desired: Byte): Boolean {
+        return testBits(value, desired, desired)
+    }
+
+    protected fun testBits(value: Byte, mask: Byte, desired: Byte): Boolean {
+        return value.toInt() and mask.toInt() == desired.toInt()
+    }
+
+    protected fun readMainCtrl(): Byte {
+        return read8(BroadcomColorSensor.Register.MAIN_CTRL)
+    }
+
+    protected fun setProximityPulseCount(proximityPulseCount: Int) {
+        RobotLog.vv(TAG, "setProximityPulseCount(0x%02x)", proximityPulseCount)
+        write8(BroadcomColorSensor.Register.PS_PULSES, proximityPulseCount)
+    }
+
+    protected fun setHardwareGain(gain: BroadcomColorSensor.Gain) {
+        RobotLog.vv(TAG, "setGain(0x%02x)", gain.bVal)
+        write8(BroadcomColorSensor.Register.LS_GAIN, gain.bVal.toInt())
+    }
+
+    protected fun setPDrive(ledDrive: LEDCurrent) {
+        RobotLog.vv(TAG, "setPDrive(0x%02x)", ledDrive.bVal)
+        write8(BroadcomColorSensor.Register.PS_LED, ledDrive.bVal.toInt())
+    }
+
+    override fun getDeviceID(): Byte {
+        return read8(BroadcomColorSensor.Register.PART_ID)
+    }
+
+    protected fun setLEDParameters(pulseMod: LEDPulseModulation, curr: LEDCurrent) {
+        val `val` = pulseMod.bVal.toInt() shl 4 or curr.bVal.toInt()
+        RobotLog.vv(TAG, "setLEDParameters(0x%02x)", `val`.toByte())
+        write8(BroadcomColorSensor.Register.PS_LED, `val`.toByte().toInt())
+    }
+
+    protected fun setPSRateAndRes(res: PSResolution, rate: PSMeasurementRate) {
+        val `val` = res.bVal.toInt() shl 3 or rate.bVal.toInt()
+        RobotLog.vv(TAG, "setPSMeasRate(0x%02x)", `val`.toByte())
+        write8(BroadcomColorSensor.Register.PS_MEAS_RATE, `val`.toByte().toInt())
+        when (rate) {
+            PSMeasurementRate.RES -> {}
+            PSMeasurementRate.R6_25ms -> measurementDelay = 7
+            PSMeasurementRate.R12_5ms -> measurementDelay = 13
+            PSMeasurementRate.R25ms -> measurementDelay = 25
+            PSMeasurementRate.R50ms -> measurementDelay = 50
+            PSMeasurementRate.R100ms -> measurementDelay = 100
+            PSMeasurementRate.R200ms -> measurementDelay = 200
+            PSMeasurementRate.R400ms -> measurementDelay = 400
+        }
+    }
+
+    protected fun setLSRateAndRes(res: LSResolution, rate: LSMeasurementRate) {
+        val `val` = res.bVal.toInt() shl 4 or rate.bVal.toInt()
+        RobotLog.vv(TAG, "setLSMeasRate(0x%02x)", `val`.toByte())
+        write8(BroadcomColorSensor.Register.LS_MEAS_RATE, `val`.toByte().toInt())
+    }
+    //----------------------------------------------------------------------------------------------
+    // Interfaces
+    //----------------------------------------------------------------------------------------------
+    /** In this implementation, the [Color] methods return 16 bit unsigned values.  */
+    @Synchronized
+    override fun red(): Int {
+        updateColors()
+        return red
+    }
+
+    @Synchronized
+    override fun green(): Int {
+        updateColors()
+        return green
+    }
+
+    @Synchronized
+    override fun blue(): Int {
+        updateColors()
+        return blue
+    }
+
+    @Synchronized
+    override fun alpha(): Int {
+        updateColors()
+        return alpha
+    }
+
+    @Synchronized
+    override fun argb(): Int {
+        return normalizedColors.toColor()
+    }
+
+    override fun setGain(newGain: Float) {
+        softwareGain = newGain
+    }
+
+    override fun getGain(): Float {
+        return softwareGain
+    }
+
+    private fun updateColors() {
+        val now = System.currentTimeMillis()
+        if (now - lastRead > measurementDelay) {
+            val data: ByteArray
+
+            // Read red, green and blue values
+            val cbRead = 9
+            data = read(BroadcomColorSensor.Register.LS_DATA_GREEN, cbRead)
+            val dib = 0
+            green =
+                TypeConversion.unsignedShortToInt(TypeConversion.byteArrayToShort(data, dib, ByteOrder.LITTLE_ENDIAN))
+            blue = Range.clip(
+                (1.55 * TypeConversion.unsignedShortToInt(
+                    TypeConversion.byteArrayToShort(
+                        data, dib + 3, ByteOrder.LITTLE_ENDIAN
+                    )
+                )).toInt(), 0, 65535
+            )
+            red = Range.clip(
+                (1.07 * TypeConversion.unsignedShortToInt(
+                    TypeConversion.byteArrayToShort(
+                        data, dib + 6, ByteOrder.LITTLE_ENDIAN
+                    )
+                )).toInt(), 0, 65535
+            )
+            alpha = (red + green + blue) / 3
+
+            // normalize to [0, 1]
+            colors.red = Range.clip(red.toFloat() * softwareGain / parameters.colorSaturation, 0f, 1f)
+            colors.green = Range.clip(green.toFloat() * softwareGain / parameters.colorSaturation, 0f, 1f)
+            colors.blue = Range.clip(blue.toFloat() * softwareGain / parameters.colorSaturation, 0f, 1f)
+
+            // apply inverse squared law of light to get readable brightness value, stored in alpha channel
+            // scale to 65535
+            val avg = (red + green + blue).toFloat() / 3
+            colors.alpha = (-(65535f / (Math.pow(avg.toDouble(), 2.0) + 65535)) + 1).toFloat()
+            lastRead = System.currentTimeMillis()
+        }
+    }
+
+    override fun getNormalizedColors(): NormalizedRGBA {
+        updateColors()
+        return colors
+    }
+
+    @Synchronized
+    override fun enableLed(enable: Boolean) {
+        // We can't directly control the LED with I2C; it's always on
+        // ignore; used to throw an error, but the default opmode can try to turn off
+        // (for range sensor variant) so got constant exceptions
+    }
+
+    override fun isLightOn(): Boolean {
+        return true
+    }
+
+    @Synchronized
+    override fun getI2cAddress(): I2cAddr {
+        return deviceClient!!.i2cAddress
+    }
+
+    @Synchronized
+    override fun setI2cAddress(i2cAddr: I2cAddr) {
+        parameters.i2cAddr = i2cAddr
+        deviceClient!!.i2cAddress = i2cAddr
+    }
+
+    //----------------------------------------------------------------------------------------------
+    // HardwareDevice
+    //----------------------------------------------------------------------------------------------
+    override fun getDeviceName(): String {
+        return "Broadcom I2C Color Sensor"
+    }
+
+    override fun getManufacturer(): Manufacturer {
+        return Manufacturer.Broadcom
+    }
+
+    override fun resetDeviceConfigurationForOpMode() {
+        super.resetDeviceConfigurationForOpMode()
+        softwareGain = 1f
+    }
+
+    //----------------------------------------------------------------------------------------------
+    // Utility
+    //----------------------------------------------------------------------------------------------
+    protected fun readUnsignedByte(reg: BroadcomColorSensor.Register): Int {
+        return TypeConversion.unsignedByteToInt(read8(reg))
+    }
+
+    protected fun readUnsignedShort(reg: BroadcomColorSensor.Register, byteOrder: ByteOrder?): Int {
+        val data = read(reg, 2)
+        return TypeConversion.unsignedShortToInt(TypeConversion.byteArrayToShort(data, 0, byteOrder))
+    }
+
+    @Synchronized
+    override fun read8(reg: BroadcomColorSensor.Register): Byte {
+        return deviceClient!!.read8(reg.bVal.toInt())
+    }
+
+    @Synchronized
+    override fun read(reg: BroadcomColorSensor.Register, cb: Int): ByteArray {
+        return deviceClient!!.read(reg.bVal.toInt(), cb)
+    }
+
+    @Synchronized
+    override fun write8(reg: BroadcomColorSensor.Register, data: Int) {
+        deviceClient!!.write8(reg.bVal.toInt(), data, I2cWaitControl.WRITTEN)
+    }
+
+    override fun write(reg: BroadcomColorSensor.Register, data: ByteArray) {
+        deviceClient!!.write(reg.bVal.toInt(), data, I2cWaitControl.WRITTEN)
+    }
+
+    protected fun delay(ms: Int) {
+        try {
+            Thread.sleep(ms.toLong())
+        } catch (e: InterruptedException) {
+            Thread.currentThread().interrupt()
+        }
+    }
+
+    companion object {
+        //----------------------------------------------------------------------------------------------
+        // State
+        //----------------------------------------------------------------------------------------------
+        const val TAG = "BroadcomColorSensorImpl"
+    }
+}
diff --git a/PhotonCore/src/main/java/com/outoftheboxrobotics/photoncore/Neutrino/RevColorSensor/RevColorSensorV3Ex.java b/PhotonCore/src/main/java/com/outoftheboxrobotics/photoncore/Neutrino/RevColorSensor/RevColorSensorV3Ex.java
deleted file mode 100644
index a404e41..0000000
--- a/PhotonCore/src/main/java/com/outoftheboxrobotics/photoncore/Neutrino/RevColorSensor/RevColorSensorV3Ex.java
+++ /dev/null
@@ -1,169 +0,0 @@
-/*
-Copyright (c) 2019 REV Robotics LLC
-
-All rights reserved.
-
-Redistribution and use in source and binary forms, with or without modification,
-are permitted (subject to the limitations in the disclaimer below) provided that
-the following conditions are met:
-
-Redistributions of source code must retain the above copyright notice, this list
-of conditions and the following disclaimer.
-
-Redistributions in binary form must reproduce the above copyright notice, this
-list of conditions and the following disclaimer in the documentation and/or
-other materials provided with the distribution.
-
-Neither the name of REV Robotics LLC nor the names of his contributors may be used to
-endorse or promote products derived from this software without specific prior
-written permission.
-
-NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
-LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
-THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
-ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
-FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
-DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
-SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR
-TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
-THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-*/
-package com.outoftheboxrobotics.photoncore.Neutrino.RevColorSensor;
-
-import com.qualcomm.hardware.rev.RevColorSensorV3;
-import com.qualcomm.robotcore.hardware.ColorRangeSensor;
-import com.qualcomm.robotcore.hardware.DistanceSensor;
-import com.qualcomm.robotcore.hardware.HardwareDevice;
-import com.qualcomm.robotcore.hardware.I2cDeviceSynchSimple;
-import com.qualcomm.robotcore.hardware.NormalizedRGBA;
-import com.qualcomm.robotcore.hardware.OpticalDistanceSensor;
-import com.qualcomm.robotcore.util.Range;
-import com.qualcomm.robotcore.util.RobotLog;
-import com.qualcomm.robotcore.util.TypeConversion;
-
-import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
-
-import java.nio.ByteOrder;
-import java.util.Locale;
-
-/**
- * {@link RevColorSensorV3Ex} implements support for the REV Robotics Color Sensor V3.
- *
- * @see <a href="http://revrobotics.com">REV Robotics Website</a>
- *
- */
-@SuppressWarnings("WeakerAccess")
-public class RevColorSensorV3Ex extends RevColorSensorV3
-{
-    NormalizedRGBA colors = new NormalizedRGBA();
-    long lastRead = 0, measurementDelay = 100;
-    int red = 0, green = 0, blue = 0, alpha = 0;
-
-    public RevColorSensorV3Ex(I2cDeviceSynchSimple deviceClient, boolean owned)
-    {
-        super(deviceClient, owned);
-    }
-
-    @Override
-    protected void setPSRateAndRes(PSResolution res, PSMeasurementRate rate)
-    {
-        int val = (res.bVal << 3) | rate.bVal;
-        RobotLog.vv(TAG, "setPSMeasRate(0x%02x)", (byte) val);
-        write8(Register.PS_MEAS_RATE, (byte) val);
-
-        switch (rate){
-            case RES:
-                break;
-            case R6_25ms:
-                measurementDelay = 7;
-                break;
-            case R12_5ms:
-                measurementDelay = 13;
-                break;
-            case R25ms:
-                measurementDelay = 25;
-                break;
-            case R50ms:
-                measurementDelay = 50;
-                break;
-            case R100ms:
-                measurementDelay = 100;
-                break;
-            case R200ms:
-                measurementDelay = 200;
-                break;
-            case R400ms:
-                measurementDelay = 400;
-                break;
-        }
-    }
-
-    @Override
-    public synchronized int red() { updateColors(); return this.red; }
-
-    @Override
-    public synchronized int green() { updateColors(); return this.green; }
-
-    @Override
-    public synchronized int blue() { updateColors(); return this.blue; }
-
-    @Override
-    public synchronized int alpha() { updateColors(); return this.alpha; }
-
-    @Override
-    public synchronized int argb() { return getNormalizedColors().toColor(); }
-
-    private void updateColors()
-    {
-        long now = System.currentTimeMillis();
-        if(now - lastRead > measurementDelay) {
-            byte[] data;
-
-            // Read red, green and blue values
-            final int cbRead = 9;
-            data = read(Register.LS_DATA_GREEN, cbRead);
-
-            final int dib = 0;
-            this.green = TypeConversion.unsignedShortToInt(TypeConversion.byteArrayToShort(data, dib, ByteOrder.LITTLE_ENDIAN));
-            this.blue = Range.clip((int) (1.55 * TypeConversion.unsignedShortToInt(TypeConversion.byteArrayToShort(
-                    data, dib + 3, ByteOrder.LITTLE_ENDIAN))), 0, 65535);
-            this.red = Range.clip((int) (1.07 * TypeConversion.unsignedShortToInt(TypeConversion.byteArrayToShort(
-                    data, dib + 6, ByteOrder.LITTLE_ENDIAN))), 0, 65535);
-
-            this.alpha = (this.red + this.green + this.blue) / 3;
-
-            // normalize to [0, 1]
-            this.colors.red = Range.clip(((float) this.red * getGain()) / parameters.colorSaturation, 0f, 1f);
-            this.colors.green = Range.clip(((float) this.green * getGain()) / parameters.colorSaturation, 0f, 1f);
-            this.colors.blue = Range.clip(((float) this.blue * getGain()) / parameters.colorSaturation, 0f, 1f);
-
-            // apply inverse squared law of light to get readable brightness value, stored in alpha channel
-            // scale to 65535
-            float avg = (float) (this.red + this.green + this.blue) / 3;
-            this.colors.alpha = (float) (-(65535f / (Math.pow(avg, 2) + 65535)) + 1);
-
-            lastRead = System.currentTimeMillis();
-        }
-    }
-
-    @Override
-    public NormalizedRGBA getNormalizedColors() { updateColors(); return this.colors; }
-
-    public void setUpdateRate(UPDATE_RATE updateRate){
-        switch (updateRate){
-            case DEFAULT:
-                setPSRateAndRes(PSResolution.R11BIT, PSMeasurementRate.R100ms);
-                break;
-            case HIGH_SPEED:
-                setPSRateAndRes(PSResolution.R11BIT, PSMeasurementRate.R6_25ms);
-                break;
-        }
-    }
-
-    public enum UPDATE_RATE{
-        DEFAULT,
-        HIGH_SPEED
-    }
-}
diff --git a/PhotonCore/src/main/java/com/outoftheboxrobotics/photoncore/Neutrino/RevColorSensor/RevColorSensorV3Ex.kt b/PhotonCore/src/main/java/com/outoftheboxrobotics/photoncore/Neutrino/RevColorSensor/RevColorSensorV3Ex.kt
new file mode 100644
index 0000000..e605c6d
--- /dev/null
+++ b/PhotonCore/src/main/java/com/outoftheboxrobotics/photoncore/Neutrino/RevColorSensor/RevColorSensorV3Ex.kt
@@ -0,0 +1,161 @@
+/*
+Copyright (c) 2019 REV Robotics LLC
+
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without modification,
+are permitted (subject to the limitations in the disclaimer below) provided that
+the following conditions are met:
+
+Redistributions of source code must retain the above copyright notice, this list
+of conditions and the following disclaimer.
+
+Redistributions in binary form must reproduce the above copyright notice, this
+list of conditions and the following disclaimer in the documentation and/or
+other materials provided with the distribution.
+
+Neither the name of REV Robotics LLC nor the names of his contributors may be used to
+endorse or promote products derived from this software without specific prior
+written permission.
+
+NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
+LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR
+TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
+THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+package com.outoftheboxrobotics.photoncore.Neutrino.RevColorSensor
+
+import com.qualcomm.hardware.broadcom.BroadcomColorSensor
+import com.qualcomm.hardware.broadcom.BroadcomColorSensor.PSMeasurementRate
+import com.qualcomm.hardware.broadcom.BroadcomColorSensor.PSResolution
+import com.qualcomm.hardware.rev.RevColorSensorV3
+import com.qualcomm.robotcore.hardware.I2cDeviceSynchSimple
+import com.qualcomm.robotcore.hardware.NormalizedRGBA
+import com.qualcomm.robotcore.util.Range
+import com.qualcomm.robotcore.util.RobotLog
+import com.qualcomm.robotcore.util.TypeConversion
+import java.nio.ByteOrder
+
+/**
+ * [RevColorSensorV3Ex] implements support for the REV Robotics Color Sensor V3.
+ *
+ * @see [REV Robotics Website](http://revrobotics.com)
+ */
+class RevColorSensorV3Ex(deviceClient: I2cDeviceSynchSimple?, owned: Boolean) : RevColorSensorV3(deviceClient, owned) {
+    var colors = NormalizedRGBA()
+    var lastRead: Long = 0
+    var measurementDelay: Long = 100
+    var red = 0
+    var green = 0
+    var blue = 0
+    var alpha = 0
+    override fun setPSRateAndRes(res: PSResolution, rate: PSMeasurementRate) {
+        val `val` = res.bVal.toInt() shl 3 or rate.bVal.toInt()
+        RobotLog.vv(TAG, "setPSMeasRate(0x%02x)", `val`.toByte())
+        write8(BroadcomColorSensor.Register.PS_MEAS_RATE, `val`.toByte().toInt())
+        when (rate) {
+            PSMeasurementRate.RES -> {}
+            PSMeasurementRate.R6_25ms -> measurementDelay = 7
+            PSMeasurementRate.R12_5ms -> measurementDelay = 13
+            PSMeasurementRate.R25ms -> measurementDelay = 25
+            PSMeasurementRate.R50ms -> measurementDelay = 50
+            PSMeasurementRate.R100ms -> measurementDelay = 100
+            PSMeasurementRate.R200ms -> measurementDelay = 200
+            PSMeasurementRate.R400ms -> measurementDelay = 400
+        }
+    }
+
+    @Synchronized
+    override fun red(): Int {
+        updateColors()
+        return this.red
+    }
+
+    @Synchronized
+    override fun green(): Int {
+        updateColors()
+        return this.green
+    }
+
+    @Synchronized
+    override fun blue(): Int {
+        updateColors()
+        return this.blue
+    }
+
+    @Synchronized
+    override fun alpha(): Int {
+        updateColors()
+        return this.alpha
+    }
+
+    @Synchronized
+    override fun argb(): Int {
+        return normalizedColors.toColor()
+    }
+
+    private fun updateColors() {
+        val now = System.currentTimeMillis()
+        if (now - lastRead > measurementDelay) {
+            val data: ByteArray
+
+            // Read red, green and blue values
+            val cbRead = 9
+            data = read(BroadcomColorSensor.Register.LS_DATA_GREEN, cbRead)
+            val dib = 0
+            this.green =
+                TypeConversion.unsignedShortToInt(TypeConversion.byteArrayToShort(data, dib, ByteOrder.LITTLE_ENDIAN))
+            this.blue = Range.clip(
+                (1.55 * TypeConversion.unsignedShortToInt(
+                    TypeConversion.byteArrayToShort(
+                        data, dib + 3, ByteOrder.LITTLE_ENDIAN
+                    )
+                )).toInt(), 0, 65535
+            )
+            this.red = Range.clip(
+                (1.07 * TypeConversion.unsignedShortToInt(
+                    TypeConversion.byteArrayToShort(
+                        data, dib + 6, ByteOrder.LITTLE_ENDIAN
+                    )
+                )).toInt(), 0, 65535
+            )
+            this.alpha = (this.red + this.green + this.blue) / 3
+
+            // normalize to [0, 1]
+            this.colors.red = Range.clip(this.red.toFloat() * gain / parameters.colorSaturation, 0f, 1f)
+            this.colors.green = Range.clip(this.green.toFloat() * gain / parameters.colorSaturation, 0f, 1f)
+            this.colors.blue = Range.clip(this.blue.toFloat() * gain / parameters.colorSaturation, 0f, 1f)
+
+            // apply inverse squared law of light to get readable brightness value, stored in alpha channel
+            // scale to 65535
+            val avg = (this.red + this.green + this.blue).toFloat() / 3
+            this.colors.alpha = (-(65535f / (Math.pow(avg.toDouble(), 2.0) + 65535)) + 1).toFloat()
+            lastRead = System.currentTimeMillis()
+        }
+    }
+
+    override fun getNormalizedColors(): NormalizedRGBA {
+        updateColors()
+        return this.colors
+    }
+
+    fun setUpdateRate(updateRate: UPDATE_RATE?) {
+        when (updateRate) {
+            UPDATE_RATE.DEFAULT -> setPSRateAndRes(PSResolution.R11BIT, PSMeasurementRate.R100ms)
+            UPDATE_RATE.HIGH_SPEED -> setPSRateAndRes(PSResolution.R11BIT, PSMeasurementRate.R6_25ms)
+            else -> {}
+        }
+    }
+
+    enum class UPDATE_RATE {
+        DEFAULT,
+        HIGH_SPEED
+    }
+}
diff --git a/PhotonCore/src/main/java/com/outoftheboxrobotics/photoncore/PhotonCore.java b/PhotonCore/src/main/java/com/outoftheboxrobotics/photoncore/PhotonCore.java
deleted file mode 100644
index 305969b..0000000
--- a/PhotonCore/src/main/java/com/outoftheboxrobotics/photoncore/PhotonCore.java
+++ /dev/null
@@ -1,426 +0,0 @@
-package com.outoftheboxrobotics.photoncore;
-
-import android.content.Context;
-
-
-import com.outoftheboxrobotics.photoncore.Neutrino.Rev2MSensor.Rev2mDistanceSensorEx;
-import com.outoftheboxrobotics.photoncore.Neutrino.RevColorSensor.RevColorSensorV3Ex;
-import com.qualcomm.ftccommon.FtcEventLoop;
-import com.qualcomm.hardware.lynx.LynxI2cDeviceSynch;
-import com.qualcomm.hardware.lynx.LynxModule;
-import com.qualcomm.hardware.lynx.LynxUnsupportedCommandException;
-import com.qualcomm.hardware.lynx.LynxUsbDevice;
-import com.qualcomm.hardware.lynx.LynxUsbDeviceDelegate;
-import com.qualcomm.hardware.lynx.LynxUsbDeviceImpl;
-import com.qualcomm.hardware.lynx.commands.LynxCommand;
-import com.qualcomm.hardware.lynx.commands.LynxDatagram;
-import com.qualcomm.hardware.lynx.commands.LynxMessage;
-import com.qualcomm.hardware.lynx.commands.LynxRespondable;
-import com.qualcomm.hardware.lynx.commands.core.LynxI2cReadStatusQueryCommand;
-import com.qualcomm.hardware.lynx.commands.core.LynxI2cWriteReadMultipleBytesCommand;
-import com.qualcomm.hardware.lynx.commands.core.LynxSetMotorConstantPowerCommand;
-import com.qualcomm.hardware.lynx.commands.core.LynxSetServoPulseWidthCommand;
-import com.qualcomm.hardware.lynx.commands.standard.LynxAck;
-import com.qualcomm.hardware.rev.Rev2mDistanceSensor;
-import com.qualcomm.hardware.rev.RevColorSensorV3;
-import com.qualcomm.robotcore.eventloop.opmode.OpMode;
-import com.qualcomm.robotcore.eventloop.opmode.OpModeManager;
-import com.qualcomm.robotcore.eventloop.opmode.OpModeManagerImpl;
-import com.qualcomm.robotcore.eventloop.opmode.OpModeManagerNotifier;
-import com.qualcomm.robotcore.exception.RobotCoreException;
-import com.qualcomm.robotcore.hardware.HardwareDevice;
-import com.qualcomm.robotcore.hardware.HardwareMap;
-import com.qualcomm.robotcore.hardware.I2cDeviceSynch;
-import com.qualcomm.robotcore.hardware.I2cDeviceSynchDevice;
-import com.qualcomm.robotcore.hardware.I2cDeviceSynchSimple;
-import com.qualcomm.robotcore.hardware.configuration.LynxConstants;
-import com.qualcomm.robotcore.hardware.usb.RobotUsbDevice;
-import com.qualcomm.robotcore.util.RobotLog;
-
-import org.firstinspires.ftc.ftccommon.external.OnCreateEventLoop;
-import org.firstinspires.ftc.robotcore.internal.usb.exception.RobotUsbException;
-
-import java.lang.reflect.Field;
-import java.util.ArrayList;
-import java.util.HashMap;
-import java.util.List;
-import java.util.concurrent.ConcurrentHashMap;
-import java.util.concurrent.atomic.AtomicBoolean;
-import java.util.concurrent.atomic.AtomicInteger;
-
-public class PhotonCore implements Runnable, OpModeManagerNotifier.Notifications {
-    protected static final PhotonCore instance = new PhotonCore();
-    protected AtomicBoolean enabled, threadEnabled;
-
-    private List<LynxModule> modules;
-    private Thread thisThread = null;
-    private Object syncLock;
-
-    private final Object messageSync = new Object();
-
-    private RobotUsbDevice robotUsbDevice;
-    private HashMap<LynxModule, RobotUsbDevice> usbDeviceMap;
-
-    public static LynxModule CONTROL_HUB, EXPANSION_HUB;
-
-    private OpModeManagerImpl opModeManager;
-
-    public static class ExperimentalParameters{
-        private final AtomicBoolean singlethreadedOptimized = new AtomicBoolean(true);
-        private final AtomicInteger maximumParallelCommands = new AtomicInteger(4);
-
-        public void setSinglethreadedOptimized(boolean state){
-            this.singlethreadedOptimized.set(state);
-        }
-
-        public boolean setMaximumParallelCommands(int maximumParallelCommands){
-            if(maximumParallelCommands <= 0){
-                return false;
-            }
-            this.maximumParallelCommands.set(maximumParallelCommands);
-            return true;
-        }
-    }
-
-    public static ExperimentalParameters experimental = new ExperimentalParameters();
-
-    public PhotonCore(){
-        CONTROL_HUB = null;
-        EXPANSION_HUB = null;
-        enabled = new AtomicBoolean(false);
-        threadEnabled = new AtomicBoolean(false);
-        usbDeviceMap = new HashMap<>();
-    }
-
-    public static void enable(){
-        instance.enabled.set(true);
-        if(CONTROL_HUB != null && CONTROL_HUB.getBulkCachingMode() == LynxModule.BulkCachingMode.OFF){
-            CONTROL_HUB.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO);
-        }
-        if(EXPANSION_HUB != null && EXPANSION_HUB.getBulkCachingMode() == LynxModule.BulkCachingMode.OFF){
-            EXPANSION_HUB.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO);
-        }
-    }
-
-    public static void disable(){
-        instance.enabled.set(false);
-    }
-
-    @OnCreateEventLoop
-    public static void attachEventLoop(Context context, FtcEventLoop eventLoop) {
-        eventLoop.getOpModeManager().registerListener(instance);
-        instance.opModeManager = eventLoop.getOpModeManager();
-    }
-
-    protected static boolean registerSend(LynxCommand command) throws LynxUnsupportedCommandException, InterruptedException {
-
-        PhotonLynxModule photonModule = (PhotonLynxModule) command.getModule();
-
-        if(!instance.usbDeviceMap.containsKey(photonModule)){
-            return false;
-        }
-
-        synchronized (instance.messageSync) {
-            while (((PhotonLynxModule)photonModule).getUnfinishedCommands().size() > experimental.maximumParallelCommands.get()){
-                //RobotLog.ee("PhotonCore", ((PhotonLynxModule)CONTROL_HUB).getUnfinishedCommands().size() + " | " + ((PhotonLynxModule)EXPANSION_HUB).getUnfinishedCommands().size());
-            }
-
-            if(!experimental.singlethreadedOptimized.get()) {
-                boolean noSimilar = false;
-                while (!noSimilar) {
-                    noSimilar = true;
-                    for (LynxRespondable respondable : photonModule.getUnfinishedCommands().values()) {
-                        if (instance.isSimilar(respondable, command)) {
-                            noSimilar = false;
-                        }
-                    }
-                }
-            }
-
-            byte messageNum = photonModule.getNewMessageNumber();
-
-            command.setMessageNumber(messageNum);
-
-            try {
-                LynxDatagram datagram = new LynxDatagram(command);
-                command.setSerialization(datagram);
-
-                if (command.isAckable() || command.isResponseExpected()) {
-                    photonModule.getUnfinishedCommands().put(command.getMessageNumber(), (LynxRespondable) command);
-                }
-
-                byte[] bytes = datagram.toByteArray();
-
-                double msLatency = 0;
-                synchronized (instance.syncLock) {
-                    long start = System.nanoTime();
-                    instance.usbDeviceMap.get(photonModule).write(bytes);
-                    long stop = System.nanoTime();
-                    msLatency = (stop - start) * 1.0e-6;
-                }
-                //RobotLog.ii("PhotonCore", "Wrote " + bytes.length + " bytes " + photonModule.getUnfinishedCommands().size() + " | " + (msLatency));
-
-                if (shouldAckImmediately(command)) {
-                    command.onAckReceived(new LynxAck(photonModule, false));
-                }
-            } catch (LynxUnsupportedCommandException | RobotUsbException e) {
-                e.printStackTrace();
-            }
-        }
-
-        return true;
-    }
-
-    protected static boolean shouldParallelize(LynxCommand command){
-        return (command instanceof LynxSetMotorConstantPowerCommand) ||
-                (command instanceof LynxSetServoPulseWidthCommand);
-    }
-
-    protected static boolean shouldAckImmediately(LynxCommand command){
-        return (command instanceof LynxSetMotorConstantPowerCommand) ||
-                (command instanceof LynxSetServoPulseWidthCommand);
-    }
-
-    private boolean isSimilar(LynxRespondable respondable1, LynxRespondable respondable2){
-        return (respondable1.getDestModuleAddress() == respondable2.getDestModuleAddress()) &&
-                (respondable1.getCommandNumber() == respondable2.getCommandNumber());
-    }
-
-    protected static LynxMessage getCacheResponse(LynxCommand command){
-        return null;
-    }
-
-    @Override
-    public void run() {
-        while(threadEnabled.get()){
-
-            try {
-                Thread.sleep(5);
-            } catch (InterruptedException e) {
-                e.printStackTrace();
-            }
-        }
-    }
-
-    @Override
-    public void onOpModePreInit(OpMode opMode) {
-        if(opModeManager.getActiveOpModeName().equals(OpModeManager.DEFAULT_OP_MODE_NAME)){
-            return;
-        }
-
-        HardwareMap map = opMode.hardwareMap;
-
-        boolean replacedPrev = false;
-        boolean hasChub = false;
-        for(LynxModule module : map.getAll(LynxModule.class)){
-            if(module instanceof PhotonLynxModule){
-                replacedPrev = true;
-            }
-            if(LynxConstants.isEmbeddedSerialNumber(module.getSerialNumber())){
-                hasChub = true;
-            }
-        }
-        if(replacedPrev){
-            HashMap<String, HardwareDevice> toRemove = new HashMap<>();
-            for(LynxModule module : map.getAll(LynxModule.class)){
-                if(!(module instanceof PhotonLynxModule)){
-                    toRemove.put((String) map.getNamesOf(module).toArray()[0], module);
-                }
-            }
-            for(String s : toRemove.keySet()){
-                map.remove(s, toRemove.get(s));
-            }
-        }else{
-            CONTROL_HUB = null;
-            EXPANSION_HUB = null;
-        }
-
-        instance.modules = map.getAll(LynxModule.class);
-        ArrayList<String> moduleNames = new ArrayList<>();
-        HashMap<LynxModule, PhotonLynxModule> replacements = new HashMap<>();
-        for(LynxModule module : instance.modules){
-            moduleNames.add((String) map.getNamesOf(module).toArray()[0]);
-        }
-        LynxUsbDeviceImpl usbDevice = null, usbDevice1 = null;
-        for(String s : moduleNames){
-            LynxModule module = (LynxModule) map.get(LynxModule.class, s);
-            if(module instanceof PhotonLynxModule){
-                continue;
-            }
-            try {
-                PhotonLynxModule photonLynxModule = new PhotonLynxModule(
-                        (LynxUsbDevice) ReflectionUtils.getField(module.getClass(), "lynxUsbDevice").get(module),
-                        (Integer)ReflectionUtils.getField(module.getClass(), "moduleAddress").get(module),
-                        (Boolean)ReflectionUtils.getField(module.getClass(), "isParent").get(module),
-                        (Boolean)ReflectionUtils.getField(module.getClass(), "isUserModule").get(module)
-                );
-                RobotLog.ee("PhotonCoreLynxNames", s);
-                ReflectionUtils.deepCopy(module, photonLynxModule);
-                map.remove(s, module);
-                map.put(s, photonLynxModule);
-                replacements.put(module, photonLynxModule);
-
-                if(module.isParent() && (hasChub && LynxConstants.isEmbeddedSerialNumber(module.getSerialNumber())) && CONTROL_HUB == null){
-                    CONTROL_HUB = photonLynxModule;
-
-                    ConcurrentHashMap<Integer, LynxRespondable> unfinishedCommands = new ConcurrentHashMap<>();
-                    try {
-                        Field f1 = module.getClass().getDeclaredField("lynxUsbDevice");
-                        f1.setAccessible(true);
-                        LynxUsbDevice tmp = (LynxUsbDevice) f1.get(module);
-                        if(tmp instanceof LynxUsbDeviceDelegate){
-                            Field tmp2 = LynxUsbDeviceDelegate.class.getDeclaredField("delegate");
-                            tmp2.setAccessible(true);
-                            usbDevice = (LynxUsbDeviceImpl) tmp2.get(tmp);
-                        }else{
-                            usbDevice = (LynxUsbDeviceImpl) tmp;
-                        }
-                        Field f2 = usbDevice.getClass().getSuperclass().getDeclaredField("robotUsbDevice");
-                        f2.setAccessible(true);
-                        Field f3 = usbDevice.getClass().getDeclaredField("engageLock");
-                        f3.setAccessible(true);
-                        syncLock = f3.get(usbDevice);
-
-                        robotUsbDevice = (RobotUsbDevice) f2.get(usbDevice);
-                        usbDeviceMap.put(photonLynxModule, robotUsbDevice);
-                    } catch (IllegalAccessException e) {
-                        e.printStackTrace();
-                    } catch(NoSuchFieldException e){
-                        e.printStackTrace();
-                    }
-                }else{
-                    if(module.isParent()) {
-                        try {
-                            Field f1 = module.getClass().getDeclaredField("lynxUsbDevice");
-                            f1.setAccessible(true);
-                            LynxUsbDevice tmp = (LynxUsbDevice) f1.get(module);
-                            if (tmp instanceof LynxUsbDeviceDelegate) {
-                                Field tmp2 = LynxUsbDeviceDelegate.class.getDeclaredField("delegate");
-                                tmp2.setAccessible(true);
-                                usbDevice = (LynxUsbDeviceImpl) tmp2.get(tmp);
-                            } else {
-                                usbDevice = (LynxUsbDeviceImpl) tmp;
-                            }
-                            Field f2 = usbDevice.getClass().getSuperclass().getDeclaredField("robotUsbDevice");
-                            f2.setAccessible(true);
-                            Field f3 = usbDevice.getClass().getDeclaredField("engageLock");
-                            f3.setAccessible(true);
-                            syncLock = f3.get(usbDevice);
-
-                            robotUsbDevice = (RobotUsbDevice) f2.get(usbDevice);
-                            usbDeviceMap.put(photonLynxModule, robotUsbDevice);
-                        } catch (NoSuchFieldException e) {
-                            e.printStackTrace();
-                        }
-                    }
-                    EXPANSION_HUB = photonLynxModule;
-                }
-            } catch (IllegalAccessException e) {
-                e.printStackTrace();
-            }
-        }
-
-        for(LynxModule m : replacements.keySet()){
-            usbDevice.removeConfiguredModule(m);
-            try {
-                usbDevice.addConfiguredModule(replacements.get(m));
-            } catch (InterruptedException e) {
-                e.printStackTrace();
-            } catch (RobotCoreException e) {
-                e.printStackTrace();
-            }
-        }
-
-        HashMap<String, HardwareDevice> replacedNeutrino = new HashMap<>(), removedNeutrino = new HashMap<>();
-        for(HardwareDevice device : map.getAll(HardwareDevice.class)){
-            if(!(device instanceof LynxModule)){
-                RobotLog.i(map.getNamesOf(device).toArray()[0].toString());
-                if(device instanceof I2cDeviceSynchDevice){
-                    try {
-                        I2cDeviceSynchSimple device2 = (I2cDeviceSynchSimple) ReflectionUtils.getField(device.getClass(), "deviceClient").get(device);
-                        if(!(device2 instanceof LynxI2cDeviceSynch)){
-                            device2 = (I2cDeviceSynchSimple) ReflectionUtils.getField(device2.getClass(), "i2cDeviceSynchSimple").get(device2);
-                        }
-                        setLynxObject(device2, replacements);
-                        RobotLog.e("" + (device2 instanceof LynxI2cDeviceSynch));
-                    } catch (Exception ignored) {
-                    }
-                }else if (device instanceof I2cDeviceSynchSimple){
-                    try {
-                        I2cDeviceSynchSimple device2 = (I2cDeviceSynchSimple) ReflectionUtils.getField(device.getClass(), "deviceClient").get(device);
-                        setLynxObject(device2, replacements);
-                    } catch (Exception ignored) {
-                    }
-                }else {
-                    setLynxObject(device, replacements);
-                }
-                if(device instanceof Rev2mDistanceSensor){
-                    I2cDeviceSynch tmp = null;
-                    boolean owned = false;
-                    try {
-                        tmp = (I2cDeviceSynch) ReflectionUtils.getField(device.getClass(), "deviceClient").get(device);
-                        owned = (boolean) ReflectionUtils.getField(device.getClass(), "deviceClientIsOwned").get(device);
-                    } catch (IllegalAccessException e) {
-                        e.printStackTrace();
-                    }
-                    Rev2mDistanceSensorEx vl53L0XEx = new Rev2mDistanceSensorEx(tmp, owned);
-                    replacedNeutrino.put((String) map.getNamesOf(device).toArray()[0], vl53L0XEx);
-                    removedNeutrino.put((String) map.getNamesOf(device).toArray()[0], device);
-                }
-                if(device instanceof RevColorSensorV3){
-                    RevColorSensorV3Ex revColorSensorV3Ex;
-                    boolean owned = false;
-                    try {
-                        I2cDeviceSynchSimple tmp = (I2cDeviceSynchSimple) ReflectionUtils.getField(device.getClass(), "deviceClient").get(device);
-                        owned = (boolean) ReflectionUtils.getField(device.getClass(), "deviceClientIsOwned").get(device);
-                        revColorSensorV3Ex = new RevColorSensorV3Ex(tmp, owned);
-                        replacedNeutrino.put((String) map.getNamesOf(device).toArray()[0], revColorSensorV3Ex);
-                        removedNeutrino.put((String) map.getNamesOf(device).toArray()[0], device);
-                    } catch (IllegalAccessException e) {
-                        e.printStackTrace();
-                    }
-                }
-            }
-        }
-
-        for(String s : replacedNeutrino.keySet()){
-            map.remove(s, removedNeutrino.get(s));
-            map.put(s, replacedNeutrino.get(s));
-        }
-
-        if(thisThread == null || !thisThread.isAlive()){
-            thisThread = new Thread(this);
-            threadEnabled.set(true);
-            thisThread.start();
-        }
-    }
-
-    private void setLynxObject(Object device, HashMap<LynxModule, PhotonLynxModule> replacements){
-        Field f = ReflectionUtils.getField(device.getClass(), LynxModule.class);
-        if (f != null) {
-            f.setAccessible(true);
-            try {
-                LynxModule module = (LynxModule) f.get(device);
-                if (module == null) {
-                    return;
-                }
-                if (replacements.containsKey(module)) {
-                    f.set(device, replacements.get(module));
-                }
-            } catch (IllegalAccessException e) {
-                e.printStackTrace();
-            }
-        }
-    }
-
-    @Override
-    public void onOpModePreStart(OpMode opMode) {
-
-    }
-
-    @Override
-    public void onOpModePostStop(OpMode opMode) {
-        enabled.set(false);
-        threadEnabled.set(false);
-    }
-}
diff --git a/PhotonCore/src/main/java/com/outoftheboxrobotics/photoncore/PhotonCore.kt b/PhotonCore/src/main/java/com/outoftheboxrobotics/photoncore/PhotonCore.kt
new file mode 100644
index 0000000..574dc38
--- /dev/null
+++ b/PhotonCore/src/main/java/com/outoftheboxrobotics/photoncore/PhotonCore.kt
@@ -0,0 +1,340 @@
+package com.outoftheboxrobotics.photoncore
+
+import android.content.Context
+import com.outoftheboxrobotics.photoncore.Neutrino.Rev2MSensor.Rev2mDistanceSensorEx
+import com.outoftheboxrobotics.photoncore.Neutrino.RevColorSensor.RevColorSensorV3Ex
+import com.qualcomm.ftccommon.FtcEventLoop
+import com.qualcomm.hardware.lynx.*
+import com.qualcomm.hardware.lynx.commands.LynxCommand
+import com.qualcomm.hardware.lynx.commands.LynxDatagram
+import com.qualcomm.hardware.lynx.commands.LynxMessage
+import com.qualcomm.hardware.lynx.commands.LynxRespondable
+import com.qualcomm.hardware.lynx.commands.core.LynxSetMotorConstantPowerCommand
+import com.qualcomm.hardware.lynx.commands.core.LynxSetServoPulseWidthCommand
+import com.qualcomm.hardware.lynx.commands.standard.LynxAck
+import com.qualcomm.hardware.rev.Rev2mDistanceSensor
+import com.qualcomm.hardware.rev.RevColorSensorV3
+import com.qualcomm.robotcore.eventloop.opmode.OpMode
+import com.qualcomm.robotcore.eventloop.opmode.OpModeManager
+import com.qualcomm.robotcore.eventloop.opmode.OpModeManagerImpl
+import com.qualcomm.robotcore.eventloop.opmode.OpModeManagerNotifier.Notifications
+import com.qualcomm.robotcore.exception.RobotCoreException
+import com.qualcomm.robotcore.hardware.HardwareDevice
+import com.qualcomm.robotcore.hardware.I2cDeviceSynch
+import com.qualcomm.robotcore.hardware.I2cDeviceSynchDevice
+import com.qualcomm.robotcore.hardware.I2cDeviceSynchSimple
+import com.qualcomm.robotcore.hardware.configuration.LynxConstants
+import com.qualcomm.robotcore.hardware.usb.RobotUsbDevice
+import com.qualcomm.robotcore.util.RobotLog
+import org.firstinspires.ftc.ftccommon.external.OnCreateEventLoop
+import org.firstinspires.ftc.robotcore.internal.usb.exception.RobotUsbException
+import java.util.concurrent.ConcurrentHashMap
+import java.util.concurrent.atomic.AtomicBoolean
+import java.util.concurrent.atomic.AtomicInteger
+
+class PhotonCore : Runnable, Notifications {
+    var enabled: AtomicBoolean
+    protected var threadEnabled: AtomicBoolean
+    private var modules: List<LynxModule>? = null
+    private var thisThread: Thread? = null
+    private var syncLock: Any? = null
+    private val messageSync = Any()
+    private var robotUsbDevice: RobotUsbDevice? = null
+    private val usbDeviceMap: HashMap<LynxModule, RobotUsbDevice?>
+    private var opModeManager: OpModeManagerImpl? = null
+
+    class ExperimentalParameters {
+        val singlethreadedOptimized = AtomicBoolean(true)
+        val maximumParallelCommands = AtomicInteger(4)
+        fun setSinglethreadedOptimized(state: Boolean) {
+            singlethreadedOptimized.set(state)
+        }
+
+        fun setMaximumParallelCommands(maximumParallelCommands: Int): Boolean {
+            if (maximumParallelCommands <= 0) {
+                return false
+            }
+            this.maximumParallelCommands.set(maximumParallelCommands)
+            return true
+        }
+    }
+
+    init {
+        enabled = AtomicBoolean(false)
+        threadEnabled = AtomicBoolean(false)
+        usbDeviceMap = HashMap()
+    }
+
+    private fun isSimilar(respondable1: LynxRespondable<*>, respondable2: LynxRespondable<*>): Boolean {
+        return respondable1.destModuleAddress == respondable2.destModuleAddress && respondable1.commandNumber == respondable2.commandNumber
+    }
+
+    override fun run() {}
+
+    private fun setLynxObject(device: Any, replacements: HashMap<LynxModule, PhotonLynxModule>) {
+        val f = ReflectionUtils.getField(device.javaClass, LynxModule::class.java)
+        if (f != null) {
+            try {
+                val module = f[device] as LynxModule
+                if (replacements.containsKey(module)) {
+                    f[device] = replacements[module]
+                }
+            } catch (e: IllegalAccessException) {
+                e.printStackTrace()
+            }
+        }
+    }
+
+    override fun onOpModePreInit(opMode: OpMode) {
+        if (opModeManager!!.activeOpModeName == OpModeManager.DEFAULT_OP_MODE_NAME) return;
+
+        var replacedPrev = false
+        var hasControlHub = false
+
+        for (module in opMode.hardwareMap.getAll(LynxModule::class.java)) {
+            if (module is PhotonLynxModule) {
+                replacedPrev = true
+            }
+            if (LynxConstants.isEmbeddedSerialNumber(module.serialNumber)) {
+                hasControlHub = true
+            }
+        }
+        if (replacedPrev) {
+            val toRemove = HashMap<String, HardwareDevice>()
+            for (module in opMode.hardwareMap.getAll(LynxModule::class.java)) {
+                if (module !is PhotonLynxModule) {
+                    toRemove[opMode.hardwareMap.getNamesOf(module).toTypedArray()[0] as String] = module
+                }
+            }
+            for (s in toRemove.keys) {
+                opMode.hardwareMap.remove(s, toRemove[s])
+            }
+        } else {
+            CONTROL_HUB = null
+            EXPANSION_HUB = null
+        }
+        instance.modules = opMode.hardwareMap.getAll(LynxModule::class.java)
+        // Module names of our lynx modules
+        val moduleNames = ArrayList<String>()
+        // Each module and its replacement class
+        val replacements = HashMap<LynxModule, PhotonLynxModule>()
+
+        val replacedNeutrino = HashMap<String, HardwareDevice>()
+        val removedNeutrino = HashMap<String, HardwareDevice>()
+
+        var usbDevice: LynxUsbDeviceImpl? = null
+
+        for (module in (instance.modules as MutableList<LynxModule>?)!!) {
+            moduleNames.add(opMode.hardwareMap.getNamesOf(module).toTypedArray()[0] as String)
+        }
+
+        for (lynxModName in moduleNames) {
+            val lynxMod = opMode.hardwareMap.get(LynxModule::class.java, lynxModName) as LynxModule
+            if (lynxMod !is PhotonLynxModule) return;
+
+            try {
+                // Create Our Lynx Module with data from the original Lynx
+                val photonLynxModule = PhotonLynxModule((ReflectionUtils.getField(lynxMod.javaClass, "lynxUsbDevice")?.get(lynxMod) as LynxUsbDevice), (ReflectionUtils.getField(lynxMod.javaClass, "moduleAddress")?.get(lynxMod) as Int), (ReflectionUtils.getField(lynxMod.javaClass, "isParent")?.get(lynxMod) as Boolean), (ReflectionUtils.getField(lynxMod.javaClass, "isUserModule")?.get(lynxMod) as Boolean))
+                // Print module name in warn log.
+                RobotLog.ww("[PHOTON CORE LYNX NAMES]", lynxModName)
+                // Copy all the data from the original Lynx to the new one.
+                ReflectionUtils.deepCopy(lynxMod, photonLynxModule)
+                // Set the new Lynx module to the hardware map.
+                opMode.hardwareMap.put(lynxModName, photonLynxModule)
+                // Marked it replaced
+                replacements[lynxMod] = photonLynxModule
+                if (lynxMod.isParent && hasControlHub && LynxConstants.isEmbeddedSerialNumber(lynxMod.serialNumber) && CONTROL_HUB == null) {
+                    CONTROL_HUB = photonLynxModule
+                    try {
+                        val tmp = ReflectionUtils.getField(lynxMod.javaClass, "lynxUsbDevice")?.get(lynxMod) as LynxUsbDevice
+
+                        usbDevice = if (tmp is LynxUsbDeviceDelegate) ReflectionUtils.getField(tmp.javaClass, "delegate")?.get(tmp) as LynxUsbDeviceImpl else tmp as LynxUsbDeviceImpl
+
+                        // Get the USB and SyncLock from the corresponding files inside the usb.
+                        robotUsbDevice = ReflectionUtils.getField(usbDevice!!.javaClass.superclass, "robotUsbDevice")?.get(usbDevice) as RobotUsbDevice
+                        syncLock = ReflectionUtils.getField(usbDevice.javaClass, "engageLock")?.get(usbDevice)
+                        usbDeviceMap[photonLynxModule] = robotUsbDevice
+                    } catch (e: IllegalAccessException) { e.printStackTrace() }
+                } else {
+                    if (lynxMod.isParent) {
+                        try {
+                            val tmp = ReflectionUtils.getField(lynxMod.javaClass, "lynxUsbDevice")?.get(lynxMod) as LynxUsbDevice
+
+                            usbDevice = if (tmp is LynxUsbDeviceDelegate) ReflectionUtils.getField(LynxUsbDeviceDelegate::class.java, "delegate")?.get(tmp) as LynxUsbDeviceImpl else tmp as LynxUsbDeviceImpl
+
+                            robotUsbDevice = ReflectionUtils.getField(usbDevice!!.javaClass.superclass, "robotUsbDevice")?.get(usbDevice) as RobotUsbDevice
+                            syncLock = ReflectionUtils.getField(usbDevice.javaClass, "engageLock")?.get(usbDevice)
+                            usbDeviceMap[photonLynxModule] = robotUsbDevice
+                        } catch (e: IllegalAccessException) { e.printStackTrace() }
+                    }
+                    EXPANSION_HUB = photonLynxModule
+                }
+            } catch (e: IllegalAccessException) { e.printStackTrace() }
+        }
+        for (m in replacements.keys) {
+            usbDevice!!.removeConfiguredModule(m)
+            try {
+                usbDevice.addConfiguredModule(replacements[m])
+            } catch (e: InterruptedException) { e.printStackTrace() } catch (e: RobotCoreException) { e.printStackTrace() }
+        }
+        // For Each Device
+        for (device in opMode.hardwareMap.getAll(HardwareDevice::class.java)) {
+            if (device !is LynxModule) {
+                // Log device name
+                RobotLog.i(opMode.hardwareMap.getNamesOf(device).toTypedArray()[0].toString())
+
+                when(device) {
+                    is I2cDeviceSynchDevice<*> -> {
+                        try {
+                            var device2 = ReflectionUtils.getField(device.javaClass, "deviceClient")?.get(device) as I2cDeviceSynchSimple
+
+                            if (device2 !is LynxI2cDeviceSynch) {
+                                device2 = ReflectionUtils.getField(device2.javaClass, "i2cDeviceSynchSimple")?.get(device2) as I2cDeviceSynchSimple
+                            }
+                            setLynxObject(device2, replacements)
+                            RobotLog.e("" + (device2 is LynxI2cDeviceSynch))
+                        } catch (ignored: Exception) { }
+                    }
+                    is I2cDeviceSynchSimple -> {
+                        try {
+                            setLynxObject(ReflectionUtils.getField(device.javaClass, "deviceClient")?.get(device) as I2cDeviceSynchSimple, replacements)
+                        } catch (ignored: Exception) { }
+                    }
+                    else -> {
+                        setLynxObject(device, replacements)
+                    }
+                }
+
+                // Not in when statement because it can be both?
+                if (device is Rev2mDistanceSensor) {
+                    var tmp: I2cDeviceSynch?
+                    var owned: Boolean
+                    try {
+                        tmp = ReflectionUtils.getField(device.javaClass, "deviceClient")?.get(device) as I2cDeviceSynch
+                        owned = ReflectionUtils.getField(device.javaClass, "deviceClientIsOwned")?.get(device) as Boolean
+
+                        replacedNeutrino[opMode.hardwareMap.getNamesOf(device).toTypedArray()[0] as String] = Rev2mDistanceSensorEx(tmp, owned)
+                        removedNeutrino[opMode.hardwareMap.getNamesOf(device).toTypedArray()[0] as String] = device
+                    } catch (e: IllegalAccessException) { e.printStackTrace() }
+                }
+                if (device is RevColorSensorV3) {
+                    var owned: Boolean
+                    try {
+                        val tmp = ReflectionUtils.getField(device.javaClass, "deviceClient")?.get(device) as I2cDeviceSynchSimple
+                        owned = ReflectionUtils.getField(device.javaClass, "deviceClientIsOwned")?.get(device) as Boolean
+
+                        replacedNeutrino[opMode.hardwareMap.getNamesOf(device).toTypedArray()[0] as String] = RevColorSensorV3Ex(tmp, owned)
+                        removedNeutrino[opMode.hardwareMap.getNamesOf(device).toTypedArray()[0] as String] = device
+                    } catch (e: IllegalAccessException) { e.printStackTrace() }
+                }
+            }
+        }
+        // Replace the stuffs in the hardware map with the new stuffs.
+        for (s in replacedNeutrino.keys) {
+            opMode.hardwareMap.remove(s, removedNeutrino[s])
+            opMode.hardwareMap.put(s, replacedNeutrino[s])
+        }
+        // IF we are not in a thread, just stick ourselvs in a new one!
+        if (thisThread == null || !thisThread!!.isAlive) {
+            thisThread = Thread(this)
+            threadEnabled.set(true)
+            thisThread!!.start()
+        }
+    }
+
+    override fun onOpModePreStart(opMode: OpMode) {}
+    override fun onOpModePostStop(opMode: OpMode) {
+        enabled.set(false)
+        threadEnabled.set(false)
+    }
+
+    companion object {
+        val instance = PhotonCore()
+        var CONTROL_HUB: LynxModule? = null
+        var EXPANSION_HUB: LynxModule? = null
+        var experimental = ExperimentalParameters()
+        fun enable() {
+            instance.enabled.set(true)
+            if (CONTROL_HUB != null && CONTROL_HUB!!.bulkCachingMode == LynxModule.BulkCachingMode.OFF) {
+                CONTROL_HUB!!.bulkCachingMode = LynxModule.BulkCachingMode.AUTO
+            }
+            if (EXPANSION_HUB != null && EXPANSION_HUB!!.bulkCachingMode == LynxModule.BulkCachingMode.OFF) {
+                EXPANSION_HUB!!.bulkCachingMode = LynxModule.BulkCachingMode.AUTO
+            }
+        }
+
+        fun disable() {
+            instance.enabled.set(false)
+        }
+
+        @OnCreateEventLoop
+        fun attachEventLoop(context: Context?, eventLoop: FtcEventLoop) {
+            eventLoop.opModeManager.registerListener(instance)
+            instance.opModeManager = eventLoop.opModeManager
+        }
+
+        @Throws(LynxUnsupportedCommandException::class, InterruptedException::class)
+        fun registerSend(command: LynxCommand<*>): Boolean {
+            val photonModule = command.module as PhotonLynxModule
+            if (!instance.usbDeviceMap.containsKey(photonModule)) {
+                return false
+            }
+            synchronized(instance.messageSync) {
+                while (photonModule.unfinishedCommands.size > experimental.maximumParallelCommands.get()) {
+                    //RobotLog.ee("PhotonCore", ((PhotonLynxModule)CONTROL_HUB).getUnfinishedCommands().size() + " | " + ((PhotonLynxModule)EXPANSION_HUB).getUnfinishedCommands().size());
+                }
+                if (!experimental.singlethreadedOptimized.get()) {
+                    var noSimilar = false
+                    while (!noSimilar) {
+                        noSimilar = true
+                        for (respondable in photonModule.unfinishedCommands.values) {
+                            if (instance.isSimilar(respondable, command)) {
+                                noSimilar = false
+                            }
+                        }
+                    }
+                }
+                val messageNum = ReflectionUtils.getField(command.javaClass, "messageNumber")?.get(command) as Int
+                command.messageNumber = messageNum
+                try {
+                    val datagram = LynxDatagram(command)
+                    command.serialization = datagram
+                    if (command.isAckable || command.isResponseExpected) {
+                        photonModule.unfinishedCommands[command.messageNumber] = (command as LynxRespondable<*>)
+                    }
+                    val bytes = datagram.toByteArray()
+                    var msLatency = 0.0
+                    synchronized(instance.syncLock!!) {
+                        val start = System.nanoTime()
+                        instance.usbDeviceMap[photonModule]!!.write(bytes)
+                        val stop = System.nanoTime()
+                        msLatency = (stop - start) * 1.0e-6
+                    }
+                    //RobotLog.ii("PhotonCore", "Wrote " + bytes.length + " bytes " + photonModule.getUnfinishedCommands().size() + " | " + (msLatency));
+                    if (shouldAckImmediately(command)) {
+                        command.onAckReceived(LynxAck(photonModule, false))
+                    }
+                } catch (e: LynxUnsupportedCommandException) {
+                    e.printStackTrace()
+                } catch (e: RobotUsbException) {
+                    e.printStackTrace()
+                }
+            }
+            return true
+        }
+
+        fun shouldParallelize(command: LynxCommand<*>?): Boolean {
+            return command is LynxSetMotorConstantPowerCommand ||
+                    command is LynxSetServoPulseWidthCommand
+        }
+
+        protected fun shouldAckImmediately(command: LynxCommand<*>?): Boolean {
+            return command is LynxSetMotorConstantPowerCommand ||
+                    command is LynxSetServoPulseWidthCommand
+        }
+
+        fun getCacheResponse(command: LynxCommand<*>?): LynxMessage? {
+            return null
+        }
+    }
+}
diff --git a/PhotonCore/src/main/java/com/outoftheboxrobotics/photoncore/PhotonLynxModule.java b/PhotonCore/src/main/java/com/outoftheboxrobotics/photoncore/PhotonLynxModule.java
deleted file mode 100644
index 6dbdae2..0000000
--- a/PhotonCore/src/main/java/com/outoftheboxrobotics/photoncore/PhotonLynxModule.java
+++ /dev/null
@@ -1,83 +0,0 @@
-package com.outoftheboxrobotics.photoncore;
-
-import com.qualcomm.hardware.lynx.LynxModule;
-import com.qualcomm.hardware.lynx.LynxUnsupportedCommandException;
-import com.qualcomm.hardware.lynx.LynxUsbDevice;
-import com.qualcomm.hardware.lynx.commands.LynxCommand;
-import com.qualcomm.hardware.lynx.commands.LynxMessage;
-import com.qualcomm.hardware.lynx.commands.LynxRespondable;
-import com.qualcomm.hardware.lynx.commands.standard.LynxAck;
-
-import java.util.ArrayList;
-import java.util.concurrent.ConcurrentHashMap;
-
-public class PhotonLynxModule extends LynxModule {
-    private ArrayList<LynxMessage> skippedAcquire = new ArrayList<>();
-
-    public PhotonLynxModule(LynxUsbDevice lynxUsbDevice, int moduleAddress, boolean isParent, boolean isUserModule) {
-        super(lynxUsbDevice, moduleAddress, isParent, isUserModule);
-    }
-
-    public ConcurrentHashMap<Integer, LynxRespondable> getUnfinishedCommands(){
-        return this.unfinishedCommands;
-    }
-
-    @Override
-    protected byte getNewMessageNumber() {
-        return super.getNewMessageNumber();
-    }
-
-    @Override
-    public void sendCommand(LynxMessage command) throws InterruptedException, LynxUnsupportedCommandException {
-        if(!PhotonCore.instance.enabled.get()){
-            super.sendCommand(command);
-            return;
-        }
-        if(command instanceof LynxCommand){
-            if(PhotonCore.getCacheResponse((LynxCommand)command) != null){
-                ((LynxCommand) command).onResponseReceived(PhotonCore.getCacheResponse((LynxCommand)command));
-                return;
-            }
-            if(PhotonCore.shouldParallelize((LynxCommand)command)){
-                boolean success = PhotonCore.registerSend((LynxCommand) command);
-                if (!success) {
-                    super.sendCommand(command);
-                }
-                return;
-            }
-        }
-        super.sendCommand(command);
-    }
-
-    @Override
-    public void acquireNetworkTransmissionLock(LynxMessage message) throws InterruptedException {
-        if(!PhotonCore.instance.enabled.get()){
-            super.acquireNetworkTransmissionLock(message);
-            return;
-        }
-        if(message instanceof LynxCommand){
-            if(PhotonCore.getCacheResponse((LynxCommand)message) != null){
-                skippedAcquire.add(message);
-                return;
-            }
-            if(PhotonCore.shouldParallelize((LynxCommand) message)){
-                skippedAcquire.add(message);
-                return;
-            }
-        }
-        super.acquireNetworkTransmissionLock(message);
-    }
-
-    @Override
-    public void releaseNetworkTransmissionLock(LynxMessage message) throws InterruptedException {
-        if(!PhotonCore.instance.enabled.get()){
-            super.releaseNetworkTransmissionLock(message);
-            return;
-        }
-        if(skippedAcquire.contains(message)){
-            skippedAcquire.remove(message);
-            return;
-        }
-        super.releaseNetworkTransmissionLock(message);
-    }
-}
diff --git a/PhotonCore/src/main/java/com/outoftheboxrobotics/photoncore/PhotonLynxModule.kt b/PhotonCore/src/main/java/com/outoftheboxrobotics/photoncore/PhotonLynxModule.kt
new file mode 100644
index 0000000..21b01b8
--- /dev/null
+++ b/PhotonCore/src/main/java/com/outoftheboxrobotics/photoncore/PhotonLynxModule.kt
@@ -0,0 +1,68 @@
+package com.outoftheboxrobotics.photoncore
+
+import com.qualcomm.hardware.lynx.LynxModule
+import com.qualcomm.hardware.lynx.LynxUnsupportedCommandException
+import com.qualcomm.hardware.lynx.LynxUsbDevice
+import com.qualcomm.hardware.lynx.commands.LynxCommand
+import com.qualcomm.hardware.lynx.commands.LynxMessage
+import com.qualcomm.hardware.lynx.commands.LynxRespondable
+import java.util.concurrent.ConcurrentHashMap
+
+class PhotonLynxModule(lynxUsbDevice: LynxUsbDevice?, moduleAddress: Int, isParent: Boolean, isUserModule: Boolean) :
+    LynxModule(lynxUsbDevice, moduleAddress, isParent, isUserModule) {
+    private val skippedAcquire = ArrayList<LynxMessage>()
+    val unfinishedCommands: ConcurrentHashMap<Int, LynxRespondable<*>>
+        get() = this.unfinishedCommands
+
+    @Throws(InterruptedException::class, LynxUnsupportedCommandException::class)
+    override fun sendCommand(command: LynxMessage) {
+        if (!PhotonCore.instance.enabled.get()) {
+            super.sendCommand(command)
+            return
+        }
+        if (command is LynxCommand<*>) {
+            if (PhotonCore.getCacheResponse(command) != null) {
+                command.onResponseReceived(PhotonCore.getCacheResponse(command))
+                return
+            }
+            if (PhotonCore.shouldParallelize(command)) {
+                val success = PhotonCore.registerSend(command)
+                if (!success) { super.sendCommand(command) }
+                return
+            }
+        }
+        super.sendCommand(command)
+    }
+
+    @Throws(InterruptedException::class)
+    override fun acquireNetworkTransmissionLock(message: LynxMessage) {
+        if (!PhotonCore.instance.enabled.get()) {
+            super.acquireNetworkTransmissionLock(message)
+            return
+        }
+        if (message is LynxCommand<*>) {
+            if (PhotonCore.getCacheResponse(message) != null) {
+                skippedAcquire.add(message)
+                return
+            }
+            if (PhotonCore.shouldParallelize(message)) {
+                skippedAcquire.add(message)
+                return
+            }
+        }
+        super.acquireNetworkTransmissionLock(message)
+    }
+
+    @Throws(InterruptedException::class)
+    override fun releaseNetworkTransmissionLock(message: LynxMessage) {
+        if (!PhotonCore.instance.enabled.get()) {
+            super.releaseNetworkTransmissionLock(message)
+            return
+        }
+        if (skippedAcquire.contains(message)) {
+            skippedAcquire.remove(message)
+            return
+        }
+        super.releaseNetworkTransmissionLock(message)
+    }
+}
diff --git a/PhotonCore/src/main/java/com/outoftheboxrobotics/photoncore/ReflectionUtils.java b/PhotonCore/src/main/java/com/outoftheboxrobotics/photoncore/ReflectionUtils.java
deleted file mode 100644
index 99c263d..0000000
--- a/PhotonCore/src/main/java/com/outoftheboxrobotics/photoncore/ReflectionUtils.java
+++ /dev/null
@@ -1,50 +0,0 @@
-package com.outoftheboxrobotics.photoncore;
-
-import java.lang.reflect.Field;
-
-public class ReflectionUtils {
-    public static Field getField(Class clazz, String fieldName) {
-        try {
-            Field f = clazz.getDeclaredField(fieldName);
-            f.setAccessible(true);
-            return f;
-        } catch (NoSuchFieldException e) {
-            Class superClass = clazz.getSuperclass();
-            if (superClass != null) {
-                return getField(superClass, fieldName);
-            }
-        }
-        return null;
-    }
-
-    public static Field getField(Class clazz, Class target) {
-        for(Field f : clazz.getDeclaredFields()){
-            if(f.getType().equals(target)){
-                f.setAccessible(true);
-                return f;
-            }
-        }
-        Class superClass = clazz.getSuperclass();
-        if(superClass != null) {
-            return getField(clazz.getSuperclass(), target);
-        }else{
-            return null;
-        }
-    }
-
-    public static void deepCopy(Object org, Object target){
-        Field[] fields = org.getClass().getDeclaredFields();
-        for(Field f : fields){
-            f.setAccessible(true);
-            Field f2 = getField(target.getClass(), f.getName());
-            if(f2 != null){
-                f2.setAccessible(true);
-                try {
-                    f2.set(target, f.get(org));
-                } catch (IllegalAccessException e) {
-                    e.printStackTrace();
-                }
-            }
-        }
-    }
-}
\ No newline at end of file
diff --git a/PhotonCore/src/main/java/com/outoftheboxrobotics/photoncore/ReflectionUtils.kt b/PhotonCore/src/main/java/com/outoftheboxrobotics/photoncore/ReflectionUtils.kt
new file mode 100644
index 0000000..e76801d
--- /dev/null
+++ b/PhotonCore/src/main/java/com/outoftheboxrobotics/photoncore/ReflectionUtils.kt
@@ -0,0 +1,51 @@
+package com.outoftheboxrobotics.photoncore
+
+import java.lang.reflect.Field
+
+object ReflectionUtils {
+    @JvmStatic
+    fun getField(clazz: Class<*>, fieldName: String?): Field? {
+        try {
+            val f = clazz.getDeclaredField(fieldName)
+            f.isAccessible = true
+            return f
+        } catch (e: NoSuchFieldException) {
+            val superClass = clazz.superclass
+            if (superClass != null) {
+                return getField(superClass, fieldName)
+            }
+        }
+        return null
+    }
+
+    fun getField(clazz: Class<*>, target: Class<*>): Field? {
+        for (f in clazz.declaredFields) {
+            if (f.type == target) {
+                f.isAccessible = true
+                return f
+            }
+        }
+        val superClass = clazz.superclass
+        return if (superClass != null) {
+            getField(clazz.superclass, target)
+        } else {
+            null
+        }
+    }
+
+    fun deepCopy(org: Any, target: Any) {
+        val fields = org.javaClass.declaredFields
+        for (f in fields) {
+            f.isAccessible = true
+            val f2 = getField(target.javaClass, f.name)
+            if (f2 != null) {
+                f2.isAccessible = true
+                try {
+                    f2[target] = f[org]
+                } catch (e: IllegalAccessException) {
+                    e.printStackTrace()
+                }
+            }
+        }
+    }
+}
\ No newline at end of file
diff --git a/build.gradle b/build.gradle
index dc02319..6359abe 100644
--- a/build.gradle
+++ b/build.gradle
@@ -1,10 +1,14 @@
 buildscript{
+    ext {
+        kotlin_version = '1.9.0'
+    }
     repositories {
         google()
         mavenCentral()
     }
     dependencies {
         classpath "com.android.tools.build:gradle:4.2.2"
+        classpath "org.jetbrains.kotlin:kotlin-gradle-plugin:$kotlin_version"
         // NOTE: Do not place your application dependencies here; they belong
         // in the individual module build.gradle files
     }