Use composition for better controller usage

src/main/java/org/hyperonline/hyperlib/controller/HYPER_CANSparkMax.java

@@ -3,13 +3,12 @@
 import com.ctre.phoenix.motorcontrol.NeutralMode;
 import com.revrobotics.*;
 import edu.wpi.first.util.sendable.SendableBuilder;
+import org.hyperonline.hyperlib.controller.meta.RawController;
 import org.hyperonline.hyperlib.controller.sensor.HYPER_SparkMaxAbsoluteEncoder;
 import org.hyperonline.hyperlib.controller.sensor.HYPER_SparkMaxAnalogSensor;
 import org.hyperonline.hyperlib.controller.sensor.HYPER_SparkMaxLimitSwitch;
 import org.hyperonline.hyperlib.controller.sensor.HYPER_SparkMaxRelativeEncoder;
 
-import java.util.function.DoubleConsumer;
-
 /**
  * wrapper for added behavior on the {@link CANSparkMax}.
  *
@@ -24,10 +23,7 @@
  *
  * @author Chris McGroarty
  */
-public class HYPER_CANSparkMax extends CANSparkMax implements SendableMotorController {
-
-    public DoubleConsumer consumeSpeed = speed -> this.set(speed);
-
+public class HYPER_CANSparkMax extends CANSparkMax implements RawController {
     /**
      * Create a new object to control a SPARK MAX motor Controller.
      *

src/main/java/org/hyperonline/hyperlib/controller/HYPER_PWMSparkMax.java

@@ -2,8 +2,9 @@
 
 import com.ctre.phoenix.motorcontrol.NeutralMode;
 import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
+import org.hyperonline.hyperlib.controller.meta.RawController;
 
-public class HYPER_PWMSparkMax extends PWMSparkMax implements SendableMotorController {
+public class HYPER_PWMSparkMax extends PWMSparkMax implements RawController {
     /**
      * Common initialization code called by all constructors.
      *

src/main/java/org/hyperonline/hyperlib/controller/HYPER_TalonSRX.java

@@ -2,6 +2,7 @@
 
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
 import edu.wpi.first.util.sendable.SendableBuilder;
+import org.hyperonline.hyperlib.controller.meta.RawController;
 
 import java.util.function.DoubleConsumer;
 
@@ -15,7 +16,7 @@
  *
  * @author Chris McGroarty
  */
-public class HYPER_TalonSRX extends WPI_TalonSRX implements SendableMotorController {
+public class HYPER_TalonSRX extends WPI_TalonSRX implements RawController {
 
   private final boolean m_useSensor;
 

src/main/java/org/hyperonline/hyperlib/controller/HYPER_VictorSPX.java

@@ -1,6 +1,7 @@
 package org.hyperonline.hyperlib.controller;
 
 import com.ctre.phoenix.motorcontrol.can.WPI_VictorSPX;
+import org.hyperonline.hyperlib.controller.meta.RawController;
 
 import java.util.function.DoubleConsumer;
 
@@ -14,7 +15,7 @@
  *
  * @author Chris McGroarty
  */
-public class HYPER_VictorSPX extends WPI_VictorSPX implements SendableMotorController {
+public class HYPER_VictorSPX extends WPI_VictorSPX implements RawController {
   public DoubleConsumer consumeSpeed = speed -> this.set(speed);
 
   /**

src/main/java/org/hyperonline/hyperlib/controller/meta/MetaController.java

@@ -0,0 +1,48 @@
+package org.hyperonline.hyperlib.controller.meta;
+
+import org.hyperonline.hyperlib.controller.SendableMotorController;
+import org.hyperonline.hyperlib.controller.meta.groups.ControllerGroup;
+
+/**
+ * Interface that represents a meta-controller as opposed to a raw controller (see HYPER_* classes)
+ * @param <T> type of controller this wraps - could also be a MetaController itself
+ *
+ * @author Dheeraj Prakash
+ */
+public interface MetaController<T extends SendableMotorController> extends SendableMotorController {
+    /**
+     * Get the controller that this meta-controller wraps
+     * @return the controller this wraps
+     */
+    T getController();
+
+    /**
+     * Travel down the meta-controller chain and obtain the raw controller being wrapped.
+     * @return the raw controller.
+     *
+     * FIXME: make this return not just SendableMotorController
+     */
+    default SendableMotorController getRawController() {
+        SendableMotorController tmp = this.getController();
+        while (true) {
+            if (tmp instanceof MetaController<?>) tmp = ((MetaController<?>) tmp).getController();
+            else if (tmp instanceof RawController) return tmp;
+        }
+    }
+
+    /**
+     * Travel down the meta-controller chain and obtain the {@link ControllerGroup} being used.
+     * Useful to get the slave controller instead of master at the end of the chain.
+     * @return ControllerGroup
+     *
+     * FIXME: make this return the correct subclass
+     */
+    default ControllerGroup<?, ?> getControllerGroup() {
+        SendableMotorController tmp = this.getController();
+        while (true) {
+            if (tmp instanceof ControllerGroup<?,?>) return (ControllerGroup<?, ?>) tmp;
+            else if (tmp instanceof MetaController<?>) tmp = ((MetaController<?>) tmp).getController();
+            else return null;
+        }
+    }
+}

src/main/java/org/hyperonline/hyperlib/controller/meta/RawController.java

@@ -0,0 +1,9 @@
+package org.hyperonline.hyperlib.controller.meta;
+
+import org.hyperonline.hyperlib.controller.SendableMotorController;
+
+/**
+ * Interface to represent a raw (not {@link MetaController}) controller.
+ * Intentionally has no methods, just serves as an endpoint in the MetaController composition chain.
+ */
+public interface RawController extends SendableMotorController {}

src/main/java/org/hyperonline/hyperlib/controller/meta/groups/CANSparkMaxGroup.java

@@ -0,0 +1,17 @@
+package org.hyperonline.hyperlib.controller.meta.groups;
+
+import org.hyperonline.hyperlib.controller.HYPER_CANSparkMax;
+import org.hyperonline.hyperlib.controller.meta.MetaController;
+
+/**
+ * Class that groups together two {@link HYPER_CANSparkMax}es as master and slave.
+ * Useful to link controllers together to pass as one elsewhere.
+ *
+ * @author Dheeraj Prakash
+ */
+public class CANSparkMaxGroup extends ControllerGroup<HYPER_CANSparkMax, HYPER_CANSparkMax> implements MetaController<HYPER_CANSparkMax> {
+    public CANSparkMaxGroup(HYPER_CANSparkMax master, HYPER_CANSparkMax slave, boolean invertSlave) {
+        super(master, slave);
+        slave.follow(master, invertSlave);
+    }
+}

src/main/java/org/hyperonline/hyperlib/controller/meta/groups/ControllerGroup.java

@@ -0,0 +1,88 @@
+package org.hyperonline.hyperlib.controller.meta.groups;
+
+import com.ctre.phoenix.motorcontrol.NeutralMode;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import org.hyperonline.hyperlib.controller.meta.MetaController;
+import org.hyperonline.hyperlib.controller.SendableMotorController;
+
+/**
+ * Base class that facilitates {@link CANSparkMaxGroup} and {@link PhoenixControllerGroup} with common functionality
+ * (delegating methods to the two controllers involved).
+ * Intentionally package private - external users don't need to see this.
+ * @param <M> master motor type
+ * @param <S> slave motor type
+ *
+ * @author Dheeraj Prakash
+ */
+public abstract class ControllerGroup<M extends SendableMotorController, S extends SendableMotorController> implements MetaController<M> {
+    protected final M master;
+    protected final S slave;
+
+    public ControllerGroup(M master, S slave) {
+        this.master = master;
+        this.slave = slave;
+    }
+
+    @Override
+    public void setNeutralMode(NeutralMode mode) {
+        master.setNeutralMode(mode);
+        slave.setNeutralMode(mode);
+    }
+
+    @Override
+    public void resetMotorConfig() {
+        master.resetMotorConfig();
+        slave.resetMotorConfig();
+    }
+
+    @Override
+    public void initSendable(SendableBuilder builder) {
+        master.initSendable(builder);
+        slave.initSendable(builder);
+    }
+
+    @Override
+    public void set(double speed) {
+        master.set(speed);
+    }
+
+    @Override
+    public double get() {
+        return master.get();
+    }
+
+    @Override
+    public M getController() {
+        return master;
+    }
+
+    @Override
+    public void setInverted(boolean isInverted) {
+        throw new UnsupportedOperationException("Cannot invert " + getClass().getSimpleName() + ". Invert individual controllers instead.");
+    }
+
+    @Override
+    public boolean getInverted() {
+        return false;
+    }
+
+    @Override
+    public void disable() {
+        master.disable();
+        slave.disable();
+    }
+
+    @Override
+    public void stopMotor() {
+        master.stopMotor();
+        slave.stopMotor();
+    }
+
+    public M getMaster() {
+        return master;
+    }
+
+    public S getSlave() {
+        return slave;
+    }
+}

src/main/java/org/hyperonline/hyperlib/controller/meta/groups/PhoenixControllerGroup.java

@@ -0,0 +1,25 @@
+package org.hyperonline.hyperlib.controller.meta.groups;
+
+import com.ctre.phoenix.motorcontrol.IMotorController;
+import org.hyperonline.hyperlib.controller.meta.MetaController;
+import org.hyperonline.hyperlib.controller.SendableMotorController;
+
+/**
+ * Class that groups together two Phoenix speed controllers (TalonSRX/VictorSPX) as master and slave.
+ * Useful to link controllers together to pass as one elsewhere.
+ *
+ * @param <M> type of master controller used
+ * @param <S> type of slave controller used
+ *
+ * @author Dheeraj Prakash
+ */
+public class PhoenixControllerGroup<
+        M extends SendableMotorController & IMotorController,
+        S extends SendableMotorController & IMotorController
+        > extends ControllerGroup<M, S> implements MetaController<M> {
+    public PhoenixControllerGroup(M master, S slave, boolean invertSlave) {
+        super(master, slave);
+        slave.follow(master);
+        slave.setInverted(invertSlave);
+    }
+}