set up beginnings of parallelism

common-wpi-util/src/main/java/com/team2073/common/mediator/Mediator.java

@@ -4,6 +4,7 @@
 import com.team2073.common.assertion.Assert;
 import com.team2073.common.mediator.condition.Condition;
 import com.team2073.common.mediator.conflict.Conflict;
+import com.team2073.common.mediator.conflict.PositionBasedConflict;
 import com.team2073.common.mediator.request.Request;
 import com.team2073.common.mediator.subsys.ColleagueSubsystem;
 import com.team2073.common.periodic.PeriodicRunnable;
@@ -71,13 +72,15 @@ public void registerConflict(Conflict... conflicts) {
                 conflictMap.put(conflict.getOriginSubsystem(), originConflicts);
                 logger.debug("Creating conflict list for: [{}] and [{}]", conflict.getOriginSubsystem());
             }
-            if(conflictMap.get(conflict.getConflictingSubsystem()) == null){
+            if (conflictMap.get(conflict.getConflictingSubsystem()) == null) {
                 ArrayList<Conflict> inverseConflicts = new ArrayList<>();
                 conflictMap.put(conflict.getConflictingSubsystem(), inverseConflicts);
                 logger.debug("Creating conflict list for: [{}]", conflict.getConflictingSubsystem());
             }
             conflictMap.get(conflict.getOriginSubsystem()).add(conflict);
-            conflictMap.get(conflict.getConflictingSubsystem()).add(conflict.invert());
+            if (conflict.getCanInvert()) {
+                conflictMap.get(conflict.getConflictingSubsystem()).add(conflict.invert());
+            }
             logger.debug("Adding conflict: [{}] and [{}]", conflict.getName(), conflict.invert().getName());
 
         }
@@ -101,7 +104,6 @@ public void add(Request request) {
 
         requestList.add(request);
         executeList.add(requestList);
-
     }
 
     /**
@@ -112,8 +114,6 @@ public void add(Request request) {
      */
     @Override
     public void onPeriodic() {
-//        Iterator<Deque<Request>> exeItr = executeList.iterator();
-
         Deque<Request> currentRequestList = executeList.peekFirst();
         if (currentRequestList != null) {
             logger.trace("Iterating through requestList [{}]", Arrays.toString(currentRequestList.toArray()));
@@ -159,7 +159,12 @@ private void execute(Request request) {
 
         if (conflicts.isEmpty()) {
             logger.debug("Executing request [{}]. Conflicts: [{}]", request.getName(), conflicts.size());
+
             subsystemMap.get(subsystem).set(condition.getConditionValue());
+            if (request.getParallelRequest() != null) {
+                logger.debug("Executing parallel Request [{}]", request.getParallelRequest().getName());
+                subsystemMap.get(request.getParallelRequest().getSubsystem()).set(request.getParallelRequest().getCondition().getConditionValue());
+            }
         } else {
             for (Conflict conflict : conflicts) {
                 logger.debug("Conflict [{}]", conflict.toString());
@@ -214,9 +219,16 @@ private ArrayList<Conflict> findRequestConflicts(Request request) {
      * @return a {@link Request} that resolves the {@link Conflict}
      */
     private Request createConflictRequest(Conflict conflict) {
-        return new Request<>(conflict.getConflictingSubsystem(),
+        Request request = new Request<>(conflict.getConflictingSubsystem(),
                 conflict.getResolution(conflict.getConflictingCondition(),
                         subsystemMap.get(conflict.getConflictingSubsystem())));
+        Request conflictingRequest = new Request<>(conflict.getOriginSubsystem(), conflict.getOriginInterimResolution(subsystemMap.get(conflict.getOriginSubsystem()),
+                subsystemMap.get(conflict.getConflictingSubsystem())));
+
+        if (conflict instanceof PositionBasedConflict) {
+            request.setParallelRequest(conflictingRequest);
+        }
+        return request;
     }
 
     @VisibleForTesting

common-wpi-util/src/main/java/com/team2073/common/mediator/conflict/Conflict.kt

@@ -8,7 +8,8 @@ abstract class Conflict<OT, CT>(
         val originSubsystem: Class<out ColleagueSubsystem<OT>>,
         val originCondition: Condition<OT>,
         val conflictingSubsystem: Class<out ColleagueSubsystem<CT>>,
-        val conflictingCondition: Condition<CT>) {
+        val conflictingCondition: Condition<CT>,
+        val canInvert: Boolean) {
 
     abstract fun invert(): Conflict<CT, OT>
 
@@ -21,4 +22,6 @@ abstract class Conflict<OT, CT>(
     fun getName(): String {
         return "ORIGIN SUBSYSTEM: ${originSubsystem.simpleName} IN $originCondition conflicts with ${conflictingSubsystem.simpleName} IN $conflictingCondition"
     }
+
+    abstract fun getOriginInterimResolution(originSubsystem: ColleagueSubsystem<OT>, conflictingSubsystem: ColleagueSubsystem<CT>): Condition<OT>
 }

common-wpi-util/src/main/java/com/team2073/common/mediator/conflict/PositionBasedConflict.kt

@@ -6,13 +6,15 @@ import com.team2073.common.mediator.request.Request
 import com.team2073.common.mediator.subsys.ColleagueSubsystem
 import com.team2073.common.mediator.subsys.PositionBasedSubsystem
 import org.apache.commons.lang3.Range
+import org.apache.commons.math3.geometry.euclidean.twod.Vector2D
 
 class PositionBasedConflict(
         val originSubsystemP: Class<out ColleagueSubsystem<Double>>,
         val originConditionP: Condition<Double>,
         val conflictingSubsystemP: Class<out ColleagueSubsystem<Double>>,
-        val conflictingConditionP: Condition<Double>) :
-        Conflict<Double, Double>(originSubsystemP, originConditionP, conflictingSubsystemP, conflictingConditionP) {
+        val conflictingConditionP: Condition<Double>,
+        val canInvertP: Boolean) :
+        Conflict<Double, Double>(originSubsystemP, originConditionP, conflictingSubsystemP, conflictingConditionP, canInvertP) {
 
     override fun isConditionConflicting(originCondition: Condition<Double>, conflictingCondition: Condition<Double>): Boolean {
         return originCondition == originConditionP && conflictingCondition == conflictingConditionP
@@ -26,6 +28,40 @@ class PositionBasedConflict(
 
     }
 
+    override fun getOriginInterimResolution(originSubsystem: ColleagueSubsystem<Double>, conflictingSubsystem: ColleagueSubsystem<Double>): Condition<Double> {
+        val originPoint: Vector2D = (originSubsystem as PositionBasedSubsystem).positionToPoint(originSubsystem.getCurrentCondition().getConditionValue())
+        val conflictingPoint: Vector2D = (conflictingSubsystem as PositionBasedSubsystem).positionToPoint(conflictingSubsystem.getCurrentCondition().getConditionValue())
+        val nearestOriginSafeY: Double
+        val nearestConflictSafeY: Double
+        val nearestOriginSafeX: Double
+        val nearestConflictSafeX: Double
+        val originSafetyRange = originSubsystem.getSafetyRange()
+
+        if (originPoint.y > conflictingPoint.y) {
+            nearestOriginSafeY = originPoint.y - originSubsystem.getSafetyRange()
+            nearestConflictSafeY = conflictingPoint.y + conflictingSubsystem.getSafetyRange()
+        } else {
+            nearestOriginSafeY = originPoint.y + originSubsystem.getSafetyRange()
+            nearestConflictSafeY = conflictingPoint.y - conflictingSubsystem.getSafetyRange()
+        }
+
+        if(originPoint.x > conflictingPoint.x){
+            nearestOriginSafeX = originPoint.x - originSubsystem.getSafetyRange()
+            nearestConflictSafeX = conflictingPoint.x + conflictingSubsystem.getSafetyRange()
+        }else{
+            nearestOriginSafeX = originPoint.x + originSubsystem.getSafetyRange()
+            nearestConflictSafeX = conflictingPoint.x - conflictingSubsystem.getSafetyRange()
+        }
+
+        val nearestOriginSafePoint = Vector2D(nearestOriginSafeX, nearestOriginSafeY)
+        val nearestConflictSafePoint = Vector2D(nearestConflictSafeX, nearestConflictSafeY)
+
+        val nearestOriginSafePosition = originSubsystem.pointToPosition(nearestOriginSafePoint);
+
+        return PositionBasedCondition(nearestOriginSafePosition,
+                Range.between(nearestOriginSafePosition - originSafetyRange, nearestOriginSafePosition + originSafetyRange))
+    }
+
     override fun isRequestConflicting(request: Request<Double>, currentConflictingCondition: Condition<Double>, currentOriginCondition: Condition<Double>): Boolean {
         val range: Range<Double> = Range.between(currentOriginCondition.getConditionValue(), request.condition.getConditionValue())
         val conflictingRange: Range<Double> = Range.between((currentConflictingCondition as PositionBasedCondition).range.minimum,
@@ -35,6 +71,6 @@ class PositionBasedConflict(
     }
 
     override fun invert(): Conflict<Double, Double> {
-        return PositionBasedConflict(conflictingSubsystemP, conflictingConditionP, originSubsystemP, originConditionP)
+        return PositionBasedConflict(conflictingSubsystemP, conflictingConditionP, originSubsystemP, originConditionP, canInvertP)
     }
 }

common-wpi-util/src/main/java/com/team2073/common/mediator/conflict/PositionStateBasedConflict.kt

@@ -1,9 +1,11 @@
 package com.team2073.common.mediator.conflict
 
 import com.team2073.common.mediator.condition.Condition
+import com.team2073.common.mediator.condition.PositionBasedCondition
 import com.team2073.common.mediator.condition.StateBasedCondition
 import com.team2073.common.mediator.request.Request
 import com.team2073.common.mediator.subsys.ColleagueSubsystem
+import com.team2073.common.mediator.subsys.PositionBasedSubsystem
 import com.team2073.common.mediator.subsys.SubsystemStateCondition
 import org.apache.commons.lang3.Range
 
@@ -12,8 +14,15 @@ class PositionStateBasedConflict<T : Enum<T>>(
         val originConditionPS: Condition<Double>,
         val conflictingSubsystemPS: Class<out ColleagueSubsystem<SubsystemStateCondition<T>>>,
         val conflictingConditionPS: Condition<SubsystemStateCondition<T>>,
-        val resolveState: SubsystemStateCondition<T>?) :
-        Conflict<Double, SubsystemStateCondition<T>>(originSubsystemPS, originConditionPS, conflictingSubsystemPS, conflictingConditionPS) {
+        val resolveState: SubsystemStateCondition<T>?,
+        val canInvertPS: Boolean) :
+        Conflict<Double, SubsystemStateCondition<T>>(originSubsystemPS, originConditionPS, conflictingSubsystemPS, conflictingConditionPS, canInvertPS) {
+
+    override fun getOriginInterimResolution(originSubsystem: ColleagueSubsystem<Double>, conflictingSubsystem: ColleagueSubsystem<SubsystemStateCondition<T>>): Condition<Double> {
+        val originSafetyRange: Double = (originSubsystem as PositionBasedSubsystem).getSafetyRange()
+        return PositionBasedCondition(originSubsystem.getCurrentCondition().getConditionValue(),
+                Range.between(originSubsystem.getCurrentCondition().getConditionValue() - originSafetyRange, originSubsystem.getCurrentCondition().getConditionValue() + originSafetyRange))
+    }
 
     override fun isConditionConflicting(originCondition: Condition<Double>, conflictingCondition: Condition<SubsystemStateCondition<T>>): Boolean {
         return originCondition == originConditionPS && conflictingCondition == conflictingConditionPS
@@ -30,6 +39,6 @@ class PositionStateBasedConflict<T : Enum<T>>(
     }
 
     override fun invert(): Conflict<SubsystemStateCondition<T>, Double> {
-        return StatePositionBasedConflict(conflictingSubsystemPS, conflictingConditionPS, originSubsystemPS, originConditionPS, null)
+        return StatePositionBasedConflict(conflictingSubsystemPS, conflictingConditionPS, originSubsystemPS, originConditionPS, null, canInvertPS)
     }
 }

common-wpi-util/src/main/java/com/team2073/common/mediator/conflict/StateBasedConflict.kt

@@ -12,11 +12,16 @@ class StateBasedConflict<OT : Enum<OT>, CT : Enum<CT>>(
         val conflictingSubsystemS: Class<ColleagueSubsystem<SubsystemStateCondition<CT>>>,
         val conflictingConditionS: Condition<SubsystemStateCondition<CT>>,
         val resolveState: SubsystemStateCondition<CT>?,
-        val inverseResolveState: SubsystemStateCondition<OT>?) :
-        Conflict<SubsystemStateCondition<OT>, SubsystemStateCondition<CT>>(originSubsystemS, originConditionS, conflictingSubsystemS, conflictingConditionS) {
+        val inverseResolveState: SubsystemStateCondition<OT>?,
+        val canInvertS: Boolean) :
+        Conflict<SubsystemStateCondition<OT>, SubsystemStateCondition<CT>>(originSubsystemS, originConditionS, conflictingSubsystemS, conflictingConditionS, canInvertS) {
+
+    override fun getOriginInterimResolution(originSubsystem: ColleagueSubsystem<SubsystemStateCondition<OT>>, conflictingSubsystem: ColleagueSubsystem<SubsystemStateCondition<CT>>): Condition<SubsystemStateCondition<OT>> {
+        return StateBasedCondition(originSubsystem.getCurrentCondition().getConditionValue())
+    }
 
     override fun invert(): Conflict<SubsystemStateCondition<CT>, SubsystemStateCondition<OT>> {
-        return StateBasedConflict(conflictingSubsystemS, conflictingConditionS, originSubsystemS, originConditionS, inverseResolveState, resolveState)
+        return StateBasedConflict(conflictingSubsystemS, conflictingConditionS, originSubsystemS, originConditionS, inverseResolveState, resolveState, canInvertS)
     }
 
     override fun isRequestConflicting(request: Request<SubsystemStateCondition<OT>>, currentConflictingCondition: Condition<SubsystemStateCondition<CT>>, currentOriginCondition: Condition<SubsystemStateCondition<OT>>): Boolean {

common-wpi-util/src/main/java/com/team2073/common/mediator/conflict/StatePositionBasedConflict.kt

@@ -2,6 +2,7 @@ package com.team2073.common.mediator.conflict
 
 import com.team2073.common.mediator.condition.Condition
 import com.team2073.common.mediator.condition.PositionBasedCondition
+import com.team2073.common.mediator.condition.StateBasedCondition
 import com.team2073.common.mediator.request.Request
 import com.team2073.common.mediator.subsys.ColleagueSubsystem
 import com.team2073.common.mediator.subsys.PositionBasedSubsystem
@@ -12,8 +13,13 @@ class StatePositionBasedConflict<T : Enum<T>>(val originSubsystemPS: Class<out C
                                               val originConditionPS: Condition<SubsystemStateCondition<T>>,
                                               val conflictingSubsystemPS: Class<out ColleagueSubsystem<Double>>,
                                               val conflictingConditionPS: Condition<Double>,
-                                              val inverseResolveState: SubsystemStateCondition<T>?) :
-        Conflict<SubsystemStateCondition<T>, Double>(originSubsystemPS, originConditionPS, conflictingSubsystemPS, conflictingConditionPS){
+                                              val inverseResolveState: SubsystemStateCondition<T>?,
+                                              val canInvertPS: Boolean) :
+        Conflict<SubsystemStateCondition<T>, Double>(originSubsystemPS, originConditionPS, conflictingSubsystemPS, conflictingConditionPS, canInvertPS){
+
+    override fun getOriginInterimResolution(originSubsystem: ColleagueSubsystem<SubsystemStateCondition<T>>, conflictingSubsystem: ColleagueSubsystem<Double>): Condition<SubsystemStateCondition<T>> {
+        return StateBasedCondition(originSubsystem.getCurrentCondition().getConditionValue())
+    }
 
     override fun isConditionConflicting(originCondition: Condition<SubsystemStateCondition<T>>, conflictingCondition: Condition<Double>): Boolean {
         return originCondition == originConditionPS && conflictingCondition == conflictingConditionPS
@@ -32,6 +38,6 @@ class StatePositionBasedConflict<T : Enum<T>>(val originSubsystemPS: Class<out C
     }
 
     override fun invert(): Conflict<Double, SubsystemStateCondition<T>> {
-        return PositionStateBasedConflict(conflictingSubsystemPS, conflictingConditionPS, originSubsystemPS, originConditionPS, inverseResolveState)
+        return PositionStateBasedConflict(conflictingSubsystemPS, conflictingConditionPS, originSubsystemPS, originConditionPS, inverseResolveState, canInvertPS)
     }
 }

common-wpi-util/src/main/java/com/team2073/common/mediator/request/Request.kt

@@ -2,10 +2,13 @@ package com.team2073.common.mediator.request
 
 import com.team2073.common.mediator.condition.Condition
 import com.team2073.common.mediator.subsys.ColleagueSubsystem
+import org.jetbrains.annotations.Nullable
 
 data class Request<T>(val subsystem: Class<out ColleagueSubsystem<T>>,
                       val condition: Condition<T>) {
 
+    var parallelRequest: Request<T>? = null
+
     fun getName(): String {
         return "REQUEST| SUBSYSTEM: ${subsystem.simpleName} CONDITION: $condition"
     }

common-wpi-util/src/main/java/com/team2073/common/mediator/subsys/PositionBasedSubsystem.kt

@@ -1,6 +1,12 @@
 package com.team2073.common.mediator.subsys
 
+import org.apache.commons.math3.geometry.euclidean.twod.Vector2D
+
 interface PositionBasedSubsystem : ColleagueSubsystem<Double> {
 
     fun getSafetyRange(): Double
+
+    fun positionToPoint(position: Double): Vector2D
+
+    fun pointToPosition(point: Vector2D): Double
 }