diff --git a/constraints/src/main/java/gov/nasa/jpl/aerie/constraints/model/DiscreteProfile.java b/constraints/src/main/java/gov/nasa/jpl/aerie/constraints/model/DiscreteProfile.java index 55a424dc1b..57e0b0abb9 100644 --- a/constraints/src/main/java/gov/nasa/jpl/aerie/constraints/model/DiscreteProfile.java +++ b/constraints/src/main/java/gov/nasa/jpl/aerie/constraints/model/DiscreteProfile.java @@ -60,15 +60,14 @@ public Windows notEqualTo(final DiscreteProfile other) { @Override public Windows changePoints() { final var result = IntervalMap.builder().set(this.profilePieces.map($ -> false)); - for (int i = 0; i < this.profilePieces.size(); i++) { - final var segment = this.profilePieces.get(i); - if (i == 0) { + for (final var segment : profilePieces) { + if (segment == profilePieces.first()) { if (!segment.interval().contains(Duration.MIN_VALUE)) { result.unset(Interval.at(segment.interval().start)); } } else { - final var previousSegment = this.profilePieces.get(i-1); - if (Interval.meets(previousSegment.interval(), segment.interval())) { + final var previousSegment = this.profilePieces.segments().lower(segment); + if (previousSegment != null && Interval.meets(previousSegment.interval(), segment.interval())) { if (!previousSegment.value().equals(segment.value())) { result.set(Interval.at(segment.interval().start), true); } @@ -83,15 +82,16 @@ public Windows changePoints() { public Windows transitions(final SerializedValue oldState, final SerializedValue newState) { final var result = IntervalMap.builder().set(this.profilePieces.map($ -> false)); - for (int i = 0; i < this.profilePieces.size(); i++) { - final var segment = this.profilePieces.get(i); - if (i == 0) { + for (final var segment : profilePieces) { + //for (int i = 0; i < this.profilePieces.size(); i++) { + //final var segment = this.profilePieces.get(i); + if (segment == profilePieces.first()) { if (segment.value().equals(newState) && !segment.interval().contains(Duration.MIN_VALUE)) { result.unset(Interval.at(segment.interval().start)); } } else { - final var previousSegment = this.profilePieces.get(i-1); - if (Interval.meets(previousSegment.interval(), segment.interval())) { + final var previousSegment = this.profilePieces.segments().lower(segment); + if (previousSegment != null && Interval.meets(previousSegment.interval(), segment.interval())) { if (previousSegment.value().equals(oldState) && segment.value().equals(newState)) { result.set(Interval.at(segment.interval().start), true); } diff --git a/constraints/src/main/java/gov/nasa/jpl/aerie/constraints/model/LinearEquation.java b/constraints/src/main/java/gov/nasa/jpl/aerie/constraints/model/LinearEquation.java index c08d4f5d23..c53c5f2717 100644 --- a/constraints/src/main/java/gov/nasa/jpl/aerie/constraints/model/LinearEquation.java +++ b/constraints/src/main/java/gov/nasa/jpl/aerie/constraints/model/LinearEquation.java @@ -15,7 +15,7 @@ /** * A linear equation in point-slope form. */ -public final class LinearEquation { +public final class LinearEquation implements Comparable { public final Duration initialTime; public final double initialValue; public final double rate; @@ -185,4 +185,12 @@ public boolean equals(final Object obj) { public int hashCode() { return Objects.hash(this.initialValue, this.initialTime, this.rate); } + + @Override + public int compareTo(final LinearEquation o) { + int c = Double.compare(this.valueAt(Duration.ZERO), o.valueAt(Duration.ZERO)); + if (c != 0) return c; + c = Double.compare(this.valueAt(Duration.MINUTE), o.valueAt(Duration.MINUTE)); + return c; + } } diff --git a/constraints/src/main/java/gov/nasa/jpl/aerie/constraints/model/LinearProfile.java b/constraints/src/main/java/gov/nasa/jpl/aerie/constraints/model/LinearProfile.java index e9f9469151..fb5b75e7d5 100644 --- a/constraints/src/main/java/gov/nasa/jpl/aerie/constraints/model/LinearProfile.java +++ b/constraints/src/main/java/gov/nasa/jpl/aerie/constraints/model/LinearProfile.java @@ -106,17 +106,16 @@ private Windows getWindowsSatisfying(final LinearProfile other, final BiFunction @Override public Windows changePoints() { final var result = IntervalMap.builder().set(this.profilePieces.map(LinearEquation::changing)); - for (int i = 0; i < this.profilePieces.size(); i++) { - final var segment = this.profilePieces.get(i); + for (final var segment : profilePieces) { final var startTime = segment.interval().start; - if (i == 0) { + if (segment == profilePieces.first()) { if (!segment.interval().contains(Duration.MIN_VALUE)) { result.unset(Interval.at(startTime)); } } else { - final var previousSegment = this.profilePieces.get(i-1); + final var previousSegment = this.profilePieces.segments().lower(segment); - if (Interval.meets(previousSegment.interval(), segment.interval())) { + if (previousSegment != null && Interval.meets(previousSegment.interval(), segment.interval())) { if (previousSegment.value().valueAt(startTime) != segment.value().valueAt(startTime)) { result.set(Interval.at(startTime), true); } @@ -133,7 +132,7 @@ public Windows changePoints() { @Override public boolean isConstant() { return profilePieces.isEmpty() || - (profilePieces.size() == 1 && !profilePieces.get(0).value().changing()); + (profilePieces.size() == 1 && !profilePieces.first().value().changing()); } /** Assigns a default value to all gaps in the profile. */ diff --git a/constraints/src/main/java/gov/nasa/jpl/aerie/constraints/time/Interval.java b/constraints/src/main/java/gov/nasa/jpl/aerie/constraints/time/Interval.java index 489d072b02..b3197c20f2 100644 --- a/constraints/src/main/java/gov/nasa/jpl/aerie/constraints/time/Interval.java +++ b/constraints/src/main/java/gov/nasa/jpl/aerie/constraints/time/Interval.java @@ -3,6 +3,7 @@ import gov.nasa.jpl.aerie.merlin.protocol.types.Duration; import org.apache.commons.lang3.tuple.Pair; +import java.util.Comparator; import java.util.Objects; import static gov.nasa.jpl.aerie.constraints.time.Interval.Inclusivity.Exclusive; @@ -277,7 +278,10 @@ public boolean adjacent(Interval x){ @Override public int compareTo(final Interval o) { - return start.compareTo(o.start); + int c = compareStarts(o); + if (c != 0) return c; + c = compareEnds(o); + return c; } public static int compareStartToStart(final Interval x, final Interval y) { diff --git a/constraints/src/main/java/gov/nasa/jpl/aerie/constraints/time/IntervalAlgebra.java b/constraints/src/main/java/gov/nasa/jpl/aerie/constraints/time/IntervalAlgebra.java index 6e2a7dcfac..ca41c5d1ae 100644 --- a/constraints/src/main/java/gov/nasa/jpl/aerie/constraints/time/IntervalAlgebra.java +++ b/constraints/src/main/java/gov/nasa/jpl/aerie/constraints/time/IntervalAlgebra.java @@ -102,6 +102,54 @@ public static Interval strictUpperBoundsOf(final Interval x) { ); } + /** + * Whether the start of one interval is before the start of another. This assumes that the intervals are both + * non-empty but does not check. + * @param x the first interval + * @param y the second interval + * @return whether the start of x is before the start of y + */ + public static boolean startBeforeStart(Interval x, Interval y) { + return x.start.shorterThan(y.start) || + (x.start.isEqualTo(y.start) && (x.includesStart() && !y.includesStart())); + } + + /** + * Whether the end of one interval is before the start of another. This assumes that the intervals are both + * non-empty but does not check. + * @param x the first interval + * @param y the second interval + * @return whether the end of x is before the start of y + */ + public static boolean endBeforeStart(Interval x, Interval y) { + return x.end.shorterThan(y.start) || + (x.end.isEqualTo(y.start) && (!x.includesEnd() || !y.includesStart())); + } + + /** + * Whether the end of one interval is before the end of another. This assumes that the intervals are both + * non-empty but does not check. + * @param x the first interval + * @param y the second interval + * @return whether the end of x is before the end of y + */ + public static boolean endBeforeEnd(Interval x, Interval y) { + return x.end.shorterThan(y.end) || + (x.end.isEqualTo(y.end) && (!x.includesEnd() && y.includesEnd())); + } + + /** + * Whether the start of one interval is before the end of another. This assumes that the intervals are both + * non-empty but does not check. + * @param x the first interval + * @param y the second interval + * @return whether the start of x is before the end of y + */ + public static boolean startBeforeEnd(Interval x, Interval y) { + return x.start.shorterThan(y.end); + } + + /** * Whether any point is contained in both operands. * @@ -110,7 +158,8 @@ public static Interval strictUpperBoundsOf(final Interval x) { * @return whether the operands overlap */ static boolean overlaps(Interval x, Interval y) { - return !isEmpty(intersect(x, y)); + if (x.isEmpty() || y.isEmpty()) return false; + return !endBeforeStart(x, y) && !endBeforeStart(y, x); } /** @@ -121,9 +170,8 @@ static boolean overlaps(Interval x, Interval y) { * @return whether `outer` contains every point in `inner` */ static boolean contains(Interval outer, Interval inner) { - // If `inner` doesn't overlap with the complement of `outer`, - // then `inner` must exist entirely within `outer`. - return !(overlaps(inner, strictUpperBoundsOf(outer)) || overlaps(inner, strictLowerBoundsOf(outer))); + if (outer.isEmpty() || inner.isEmpty()) return false; + return !startBeforeStart(inner, outer) && !endBeforeEnd(outer, inner); } /** @@ -158,7 +206,8 @@ static boolean equals(Interval x, Interval y) { * @return whether the start point of x is before all points in y */ static boolean startsBefore(Interval x, Interval y) { - return strictlyContains(strictLowerBoundsOf(y), strictLowerBoundsOf(x)); + if (x.isEmpty() || y.isEmpty()) return false; + return startBeforeStart(x, y); } /** @@ -169,7 +218,8 @@ static boolean startsBefore(Interval x, Interval y) { * @return whether the end point of x is after all points in y */ static boolean endsAfter(Interval x, Interval y) { - return strictlyContains(strictUpperBoundsOf(y), strictUpperBoundsOf(x)); + if (x.isEmpty() || y.isEmpty()) return false; + return endBeforeEnd(y, x); } /** @@ -213,7 +263,9 @@ static boolean startsStrictlyAfter(Interval x, Interval y) { * @return whether the end point of x is strictly before all points in y */ static boolean endsStrictlyBefore(Interval x, Interval y) { - return !isEmpty(intersect(strictUpperBoundsOf(x), strictLowerBoundsOf(y))); + if (x.isEmpty() || y.isEmpty()) return false; + return x.end.shorterThan(y.start) || + (x.end.isEqualTo(y.start) && (!x.includesEnd() && !y.includesStart())); } /** @@ -224,7 +276,8 @@ static boolean endsStrictlyBefore(Interval x, Interval y) { * @return whether x ends when y begins, with no overlap and no gap */ static boolean meets(Interval x, Interval y) { - return equals(strictUpperBoundsOf(x), strictUpperBoundsOf(strictLowerBoundsOf(y))); + if (x.isEmpty() || y.isEmpty()) return false; + return x.end.isEqualTo(y.start) && (x.endInclusivity != y.startInclusivity); } /** diff --git a/constraints/src/main/java/gov/nasa/jpl/aerie/constraints/time/IntervalMap.java b/constraints/src/main/java/gov/nasa/jpl/aerie/constraints/time/IntervalMap.java index d8c9e2b95c..3934ef93a5 100644 --- a/constraints/src/main/java/gov/nasa/jpl/aerie/constraints/time/IntervalMap.java +++ b/constraints/src/main/java/gov/nasa/jpl/aerie/constraints/time/IntervalMap.java @@ -5,17 +5,19 @@ import java.util.ArrayList; import java.util.Arrays; -import java.util.Collections; +import java.util.Collection; import java.util.Iterator; import java.util.List; import java.util.Objects; import java.util.Optional; +import java.util.TreeSet; import java.util.function.BiFunction; import java.util.function.Function; import java.util.stream.Stream; import static gov.nasa.jpl.aerie.constraints.time.Interval.Inclusivity.Exclusive; import static gov.nasa.jpl.aerie.constraints.time.Interval.Inclusivity.Inclusive; +import static gov.nasa.jpl.aerie.constraints.time.IntervalAlgebra.endBeforeStart; /** * A generic container that maps non-overlapping intervals on the timeline to values. @@ -34,11 +36,11 @@ public final class IntervalMap implements Iterable> { // INVARIANT: `segments` is list of non-empty, non-overlapping segments in ascending order. // INVARIANT: If two adjacent segments abut exactly (e.g. [0, 3), [3, 5]), their values are non-equal. - private final List> segments; + private final TreeSet> segments; // PRECONDITION: The list of `segments` meets the invariants of the class. - private IntervalMap(final List> segments) { - this.segments = Collections.unmodifiableList(segments); + private IntervalMap(final Collection> segments) { + this.segments = new TreeSet<>(segments); } /** Creates an IntervalMap builder */ @@ -53,7 +55,11 @@ public static Builder builder() { * overwrites the former. */ public static IntervalMap of(final List> segments) { - final var builder = new Builder(segments.size()); + final var builder = new Builder(); + + if (invariantsMet(segments)) { + return new IntervalMap<>(segments); + } for (final var segment : segments) { builder.set(segment.interval(), segment.value()); } @@ -61,6 +67,32 @@ public static IntervalMap of(final List> segments) { return builder.build(); } + /** + * Check if invariants are met on a list of segments.

+ * INVARIANT: `segments` is list of non-empty, non-overlapping segments in ascending order.
+ * INVARIANT: If two adjacent segments abut exactly (e.g. [0, 3), [3, 5]), their values are non-equal. + * + * @param segments + * @return + * @param + */ + private static boolean invariantsMet(Iterable> segments) { + // check if segments meets preconditions + boolean segmentsOkay = true; + Segment oldSegment = null; + for (final var segment : segments) { + if (segment.interval().isEmpty() || + (oldSegment != null && + (!endBeforeStart(oldSegment.interval(), segment.interval()) || + (segment.interval().start.isEqualTo(oldSegment.interval().end) && Objects.equals(segment.value(), oldSegment.value()))))) { + segmentsOkay = false; + break; + } + oldSegment = segment; + } + return segmentsOkay; + } + /** Creates an IntervalMap with a single segment. */ public static IntervalMap of(final Interval interval, final V value) { return IntervalMap.of(List.of(Segment.of(interval, value))); @@ -263,17 +295,15 @@ IntervalMap map2( final IntervalMap right, final TriFunction, Optional, Optional> transform ) { - final var result = new ArrayList>(); + final var result = new TreeSet>();//new ArrayList>(); var startTime = Duration.MIN_VALUE; var startInclusivity = Inclusive; Duration endTime; Interval.Inclusivity endInclusivity; - var leftIndex = 0; - var rightIndex = 0; - var nextLeftIndex = 0; - var nextRightIndex = 0; + final Iterator> leftIter = left.segments.iterator(); + final Iterator> rightIter = right.segments.iterator(); Interval leftInterval; Interval rightInterval; @@ -282,13 +312,22 @@ IntervalMap map2( Optional previousValue = Optional.empty(); + boolean leftGetNext = true; + boolean rightGetNext = true; + boolean leftDone = false; + boolean rightDone = false; + Segment leftNextDefinedSegment = null; + Segment rightNextDefinedSegment = null; + Segment lastSegmentAdded = null; + while (startTime.shorterThan(Duration.MAX_VALUE) || startInclusivity == Inclusive) { - if (leftIndex < left.size()) { - var leftNextDefinedSegment = left.get(leftIndex); + if (!leftDone && (!leftGetNext || leftIter.hasNext())) { + if (leftGetNext) leftNextDefinedSegment = leftIter.next(); + leftGetNext = false; if (leftNextDefinedSegment.interval().start.shorterThan(startTime) || (leftNextDefinedSegment.interval().start.isEqualTo(startTime) && !leftNextDefinedSegment.interval().startInclusivity.moreRestrictiveThan(startInclusivity))) { leftInterval = leftNextDefinedSegment.interval(); leftValue = Optional.of(leftNextDefinedSegment.value()); - nextLeftIndex = leftIndex + 1; + leftGetNext = true; } else { leftInterval = Interval.between( Duration.MIN_VALUE, @@ -298,16 +337,18 @@ IntervalMap map2( leftValue = Optional.empty(); } } else { + leftDone = true; leftInterval = Interval.FOREVER; leftValue = Optional.empty(); } - if (rightIndex < right.size()) { - var rightNextDefinedSegment = right.get(rightIndex); + if (!rightDone && (!rightGetNext || rightIter.hasNext())) { + if (rightGetNext) rightNextDefinedSegment = rightIter.next(); + rightGetNext = false; if (rightNextDefinedSegment.interval().start.shorterThan(startTime) || (rightNextDefinedSegment.interval().start.isEqualTo(startTime) && !rightNextDefinedSegment.interval().startInclusivity.moreRestrictiveThan(startInclusivity))) { rightInterval = rightNextDefinedSegment.interval(); rightValue = Optional.of(rightNextDefinedSegment.value()); - nextRightIndex = rightIndex + 1; + rightGetNext = true; } else { rightInterval = Interval.between( Duration.MIN_VALUE, @@ -317,6 +358,7 @@ IntervalMap map2( rightValue = Optional.empty(); } } else { + rightDone = true; rightInterval = Interval.FOREVER; rightValue = Optional.empty(); } @@ -325,26 +367,24 @@ IntervalMap map2( endTime = leftInterval.end; if (leftInterval.includesEnd() && rightInterval.includesEnd()) { endInclusivity = Inclusive; - leftIndex = nextLeftIndex; - rightIndex = nextRightIndex; } else if (leftInterval.includesEnd()) { endInclusivity = Exclusive; - rightIndex = nextRightIndex; + leftGetNext = false; } else if (rightInterval.includesEnd()) { endInclusivity = Exclusive; - leftIndex = nextLeftIndex; + rightGetNext = false; } else { endInclusivity = Exclusive; - rightIndex = nextRightIndex; + leftGetNext = false; } } else if (leftInterval.end.shorterThan(rightInterval.end)) { endTime = leftInterval.end; endInclusivity = leftInterval.endInclusivity; - leftIndex = nextLeftIndex; + rightGetNext = false; } else { endTime = rightInterval.end; endInclusivity = rightInterval.endInclusivity; - rightIndex = nextRightIndex; + leftGetNext = false; } var finalInterval = Interval.between(startTime, startInclusivity, endTime, endInclusivity); if (finalInterval.isEmpty()) continue; @@ -352,15 +392,17 @@ IntervalMap map2( var newValue = transform.apply(finalInterval, leftValue, rightValue); if (newValue.isPresent()) { if (!newValue.equals(previousValue)) { - result.add(Segment.of(finalInterval, newValue.get())); + lastSegmentAdded = Segment.of(finalInterval, newValue.get()); + result.add(lastSegmentAdded); } else { - var previousInterval = result.remove(result.size() - 1).interval(); - result.add( + var previousInterval = lastSegmentAdded.interval(); + result.remove(lastSegmentAdded); + lastSegmentAdded = Segment.of( Interval.unify(previousInterval, finalInterval), newValue.get() - ) - ); + ); + result.add(lastSegmentAdded); } } previousValue = newValue; @@ -390,12 +432,6 @@ IntervalMap flatMap2( return result.build(); } - /** Gets the segment at a given index */ - public Segment get(final int index) { - final var i = (index >= 0) ? index : this.segments.size() + index; - return this.segments.get(i); - } - /** The number of defined intervals in this. */ public int size() { return this.segments.size(); @@ -420,6 +456,10 @@ public Iterable iterateEqualTo(final V value) { .iterator(); } + public TreeSet> segments() { + return this.segments; + } + public Stream> stream() { return this.segments.stream(); } @@ -435,20 +475,21 @@ public String toString() { return this.segments.toString(); } + public Segment first() { + if (segments == null) return null; + return segments.first(); + } + /** A builder for IntervalMap */ public static final class Builder { // INVARIANT: `segments` is list of non-empty, non-overlapping segments in ascending order. // INVARIANT: If two adjacent segments abut exactly (e.g. [0, 3), [3, 5]), their values are non-equal. - private List> segments; + private TreeSet> segments; private boolean built = false; public Builder() { - this.segments = new ArrayList<>(); - } - - public Builder(int initialCapacity) { - this.segments = new ArrayList<>(initialCapacity); + this.segments = new TreeSet<>(); } public Builder set(final IntervalMap map) { @@ -469,50 +510,67 @@ public Builder set(Interval interval, final V value) { // <> is `interval`, the interval to apply; [] is the currently-indexed interval in the map. // Cases: --[---]---<--->-- - int index = 0; - while (index < this.segments.size() && IntervalAlgebra.endsStrictlyBefore(this.getInterval(index), interval)) { - index += 1; + Segment s = null; + var originalS = Segment.of(interval, value); + var iter = this.segments.headSet(originalS, true).descendingIterator(); + Segment lowerS = null; + while (iter.hasNext()) { + lowerS = iter.next(); + if (IntervalAlgebra.endsStrictlyBefore(lowerS.interval(), originalS.interval())) { + break; + } else { + s = lowerS; + } + } + if (s == null) { // there are no elements that start before `interval` + s = this.segments.higher(originalS); } // Cases: --[---<---]--->-- and --[---<--->---]-- - if (index < this.segments.size() && IntervalAlgebra.startsBefore(this.getInterval(index), interval)) { + if (s != null && IntervalAlgebra.startsBefore(s.interval(), interval)) { // If the intervals agree on their value, we can unify the old interval with the new one. // Otherwise, we'll snip the old one. - if (Objects.equals(this.getValue(index), value)) { - interval = IntervalAlgebra.unify(this.segments.remove(index).interval(), interval); + if (Objects.equals(s.value(), value)) { + segments.remove(s); + interval = IntervalAlgebra.unify(s.interval(), interval); } else { - final var prefix = IntervalAlgebra.intersect(this.getInterval(index), IntervalAlgebra.strictLowerBoundsOf(interval)); - final var suffix = IntervalAlgebra.intersect(this.getInterval(index), IntervalAlgebra.strictUpperBoundsOf(interval)); - - this.segments.set(index, Segment.of(prefix, this.getValue(index))); - if (!IntervalAlgebra.isEmpty(suffix)) this.segments.add(index + 1, Segment.of(suffix, this.getValue(index))); - - index += 1; + final var prefix = IntervalAlgebra.intersect(s.interval(), IntervalAlgebra.strictLowerBoundsOf(interval)); + final var suffix = IntervalAlgebra.intersect(s.interval(), IntervalAlgebra.strictUpperBoundsOf(interval)); + this.segments.remove(s); + s = Segment.of(prefix, s.value()); + this.segments.add(s); + if (!IntervalAlgebra.isEmpty(suffix)) { + s = Segment.of(suffix, s.value()); + this.segments.add(s); + } else { + s = this.segments.higher(s); + } } } // Cases: --<---[---]--->-- - while (index < this.segments.size() && !IntervalAlgebra.endsAfter(this.getInterval(index), interval)) { - this.segments.remove(index); + while (s != null && !IntervalAlgebra.endsAfter(s.interval(), interval)) { + this.segments.remove(s); + s = this.segments.higher(s); } // Cases: --<---[--->---]-- - if (index < this.segments.size() && !IntervalAlgebra.startsStrictlyAfter(this.getInterval(index), interval)) { + if (s != null && !IntervalAlgebra.startsStrictlyAfter(s.interval(), interval)) { // If the intervals agree on their value, we can unify the old interval with the new one. // Otherwise, we'll snip the old one. - if (Objects.equals(this.getValue(index), value)) { - interval = IntervalAlgebra.unify(this.segments.remove(index).interval(), interval); + this.segments.remove(s); + if (Objects.equals(s.value(), value)) { + interval = IntervalAlgebra.unify(s.interval(), interval); } else { - final var suffix = IntervalAlgebra.intersect(this.getInterval(index), IntervalAlgebra.strictUpperBoundsOf(interval)); - - this.segments.set(index, Segment.of(suffix, this.getValue(index))); + final var suffix = IntervalAlgebra.intersect(s.interval(), IntervalAlgebra.strictUpperBoundsOf(interval)); + this.segments.add(Segment.of(suffix, s.value())); } } - // now, everything left of `index` is strictly left of `interval`, - // and everything right of `index` is strictly right of `interval`, + // now, everything left of s is strictly left of `interval`, + // and everything right of s is strictly right of `interval`, // so adding this interval to the list is trivial. - this.segments.add(index, Segment.of(interval, value)); + this.segments.add(Segment.of(interval, value)); return this; } @@ -522,42 +580,50 @@ public Builder unset(Interval interval) { if (interval.isEmpty()) return this; - for (int i = 0; i < this.segments.size(); i++) { - final var existingInterval = this.segments.get(i).interval(); - - if (IntervalAlgebra.endsStrictlyBefore(existingInterval, interval)) continue; - else if (IntervalAlgebra.startsStrictlyAfter(existingInterval, interval)) break; - + Segment s = Segment.of(interval, null); + s = segments.ceiling(s); + while (s != null && !IntervalAlgebra.startsStrictlyAfter(s.interval(), interval)) { + // TODO -- This might be cleaner and more efficient with a headset, but the part at the end with + // ceiling() and first() doesn't fit. + // Actually, it might be better to rewrite since it was written based on segments being a list, + // and now segments is an ordered set. + final var existingInterval = s.interval(); if (IntervalAlgebra.startsBefore(interval, existingInterval)) { - final var segment = this.segments.remove(i); + segments.remove(s); if (IntervalAlgebra.contains(interval, existingInterval)) { - i--; + s = segments.lower(s); } else { - final var newInterval = Interval.between(interval.end, interval.endInclusivity.opposite(), existingInterval.end, existingInterval.endInclusivity); + final var newInterval = Interval.between(interval.end, interval.endInclusivity.opposite(), + existingInterval.end, existingInterval.endInclusivity); if (!newInterval.isEmpty()) { - this.segments.add(i, Segment.of(newInterval, segment.value())); + this.segments.add(Segment.of(newInterval, s.value())); } else { - i--; + s = segments.lower(s); } } } else { - final var segment = this.segments.remove(i); - final var leftInterval = Interval.between(existingInterval.start, existingInterval.startInclusivity, interval.start, interval.startInclusivity.opposite()); + this.segments.remove(s); + var value = s.value(); + final var leftInterval = Interval.between(existingInterval.start, existingInterval.startInclusivity, + interval.start, interval.startInclusivity.opposite()); if (!leftInterval.isEmpty()) { - this.segments.add(i, Segment.of(leftInterval, segment.value())); + this.segments.add(Segment.of(leftInterval, value)); } else { - i--; + s = segments.lower(s); } if (IntervalAlgebra.endsAfter(existingInterval, interval)) { - final var rightInterval = Interval.between(interval.end, interval.endInclusivity.opposite(), existingInterval.end, existingInterval.endInclusivity); + final var rightInterval = Interval.between(interval.end, interval.endInclusivity.opposite(), + existingInterval.end, existingInterval.endInclusivity); if (!rightInterval.isEmpty()) { - this.segments.add(i+1, Segment.of(rightInterval, segment.value())); + this.segments.add(Segment.of(rightInterval, value)); } else { - i--; + s = segments.lower(s); } - i++; + if (s == null && !segments.isEmpty()) s = segments.first(); + else s = segments.higher(s); } } + if (s != null) s = segments.higher(s); } return this; @@ -573,13 +639,5 @@ public IntervalMap build() { // SAFETY: `segments` meets the same invariants as required by `IntervalMap`. return new IntervalMap<>(segments); } - - private Interval getInterval(final int index) { - return this.segments.get(index).interval(); - } - - private V getValue(final int index) { - return this.segments.get(index).value(); - } } } diff --git a/constraints/src/main/java/gov/nasa/jpl/aerie/constraints/time/Segment.java b/constraints/src/main/java/gov/nasa/jpl/aerie/constraints/time/Segment.java index c5663133ea..505125bc92 100644 --- a/constraints/src/main/java/gov/nasa/jpl/aerie/constraints/time/Segment.java +++ b/constraints/src/main/java/gov/nasa/jpl/aerie/constraints/time/Segment.java @@ -1,8 +1,20 @@ package gov.nasa.jpl.aerie.constraints.time; +import gov.nasa.jpl.aerie.merlin.protocol.types.ObjectComparator; + +import java.util.Comparator; + /** A basic container used by {@link IntervalMap} that associates an interval with a value */ -public record Segment(Interval interval, V value) { +public record Segment(Interval interval, V value) implements Comparable> { public static Segment of(final Interval interval, final V value) { return new Segment<>(interval, value); } + + @Override + public int compareTo(final Segment o) { + final var comparator = + Comparator.comparing(Segment::interval).thenComparing(Segment::value, ObjectComparator.getInstance()); + return comparator.compare(this, o); + } + } diff --git a/constraints/src/main/java/gov/nasa/jpl/aerie/constraints/time/Windows.java b/constraints/src/main/java/gov/nasa/jpl/aerie/constraints/time/Windows.java index 6f5f99e15b..e2cab009e3 100644 --- a/constraints/src/main/java/gov/nasa/jpl/aerie/constraints/time/Windows.java +++ b/constraints/src/main/java/gov/nasa/jpl/aerie/constraints/time/Windows.java @@ -185,8 +185,7 @@ public Optional> minTrueTimePoint(){ /** Gets the time and inclusivity of the trailing edge of the last true segment */ public Optional> maxTrueTimePoint(){ - for (int i = this.segments.size() - 1; i >= 0; i--) { - final var segment = this.segments.get(i); + for (var segment : segments.segments().reversed()) { if (segment.value()) { final var window = segment.interval(); return Optional.of(Pair.of(window.end, window.endInclusivity)); @@ -230,8 +229,7 @@ public Windows removeTrueSegment(final int indexToRemove) { } } else { int index = -1; - for (int i = this.segments.size() - 1; i >= 0; i--) { - final var segment = this.segments.get(i); + for (var segment : segments.segments().reversed()) { if (segment.value()) { if (index == indexToRemove) { return new Windows(this.segments.set(segment.interval(), false)); @@ -264,8 +262,7 @@ public Windows keepTrueSegment(final int indexToKeep) { } } else { int index = -1; - for (int i = this.segments.size() - 1; i >= 0; i--) { - final var segment = this.segments.get(i); + for (var segment : segments.segments().reversed()) { if (segment.value()) { if (index != indexToKeep) { builder.set(Segment.of(segment.interval(), false)); @@ -419,14 +416,14 @@ public LinearProfile accumulatedDuration(final Duration unit) { */ public Windows starts() { var result = IntervalMap.builder().set(this.segments).build(); - for (int i = 0; i < result.size(); i++) { - final var segment = result.get(i); + for (final var segment : result.segments()) { if (segment.value()) { final boolean meetsFalse; - if (i == 0) { + if (segment == result.first()) { meetsFalse = false; } else { - meetsFalse = Interval.meets(this.segments.get(i - 1).interval(), segment.interval()); + var s = this.segments.segments().lower(segment); + meetsFalse = s != null && Interval.meets(s.interval(), segment.interval()); } if (meetsFalse) { result = result.set(Interval.at(segment.interval().start), true); @@ -461,14 +458,14 @@ public Windows starts() { @Override public Windows ends() { var result = IntervalMap.builder().set(this.segments).build(); - for (int i = 0; i < this.segments.size(); i++) { - final var segment = this.segments.get(i); + for (final var segment : this.segments.segments()) { if (segment.value()) { final boolean meetsFalse; - if (i == this.segments.size()-1) { + if (segment == this.segments.segments().last()) { meetsFalse = false; } else { - meetsFalse = Interval.meets(segment.interval(), this.segments.get(i + 1).interval()); + var s = this.segments.segments().higher(segment); + meetsFalse = s != null && Interval.meets(segment.interval(), s.interval()); } if (meetsFalse) { result = result.set(Interval.between( @@ -497,14 +494,19 @@ public Spans intoSpans(final Interval bounds) { boolean boundsStartContained = false; boolean boundsEndContained = false; if(this.segments.size() == 1){ - if (segments.get(0).interval().contains(bounds.start) || - Interval.hasSameStart(segments.get(0).interval(), bounds)) boundsStartContained = true; - if (segments.get(0).interval().contains(bounds.end) || - Interval.hasSameEnd(segments.get(0).interval(), bounds)) boundsEndContained = true; + if (segments.first().interval().contains(bounds.start) || + Interval.hasSameStart(segments.first().interval(), bounds)) boundsStartContained = true; + if (segments.first().interval().contains(bounds.end) || + Interval.hasSameEnd(segments.first().interval(), bounds)) boundsEndContained = true; } - for (int i = 0; i < this.segments.size() - 1; i++) { - final var leftInterval = this.segments.get(i).interval(); - final var rightInterval = this.segments.get(i+1).interval(); + Interval leftInterval = null; + Interval rightInterval = null; + for (final var segment : this.segments.segments()) { + rightInterval = segment.interval(); + if (leftInterval == null) { + leftInterval = rightInterval; + continue; + } if((leftInterval.contains(bounds.start) || rightInterval.contains(bounds.start)) || Interval.hasSameStart(leftInterval, bounds) || Interval.hasSameStart(rightInterval, bounds)) boundsStartContained = true; if((leftInterval.contains(bounds.end) || rightInterval.contains(bounds.end)) || @@ -518,6 +520,7 @@ public Spans intoSpans(final Interval bounds) { message.append(")."); throw new InvalidGapsException(message.toString()); } + leftInterval = rightInterval; } if (!boundsStartContained) throw new InvalidGapsException("cannot convert Windows with gaps into Spans (gap detected at plan bounds start)"); if (!boundsEndContained) throw new InvalidGapsException("cannot convert Windows with gaps into Spans (gap detected at plan bounds end)"); @@ -573,15 +576,14 @@ public Windows notEqualTo(final Windows other) { @Override public Windows changePoints() { + Segment previousSegment = null; final var result = IntervalMap.builder().set(this.segments.map($ -> false)); - for (int i = 0; i < this.segments.size(); i++) { - final var segment = this.segments.get(i); - if (i == 0) { + for (final var segment : this.segments.segments()) { + if (segment == this.segments.first()) { if (!segment.interval().contains(Duration.MIN_VALUE)) { result.unset(Interval.at(segment.interval().start)); } } else { - final var previousSegment = this.segments.get(i-1); if (Interval.meets(previousSegment.interval(), segment.interval())) { if (!previousSegment.value().equals(segment.value())) { result.set(Interval.at(segment.interval().start), true); @@ -590,6 +592,7 @@ public Windows changePoints() { result.unset(Interval.at(segment.interval().start)); } } + previousSegment = segment; } return new Windows(result.build()); @@ -633,11 +636,6 @@ public Windows select(final List intervals) { return new Windows(segments.select(intervals)); } - /** Delegated to {@link IntervalMap#get(int)} */ - public Segment get(final int index) { - return segments.get(index); - } - /** Delegated to {@link IntervalMap#size()} */ public int size() { return segments.size(); diff --git a/merlin-sdk/src/main/java/gov/nasa/jpl/aerie/merlin/protocol/types/ObjectComparator.java b/merlin-sdk/src/main/java/gov/nasa/jpl/aerie/merlin/protocol/types/ObjectComparator.java new file mode 100644 index 0000000000..02d9e6f4a2 --- /dev/null +++ b/merlin-sdk/src/main/java/gov/nasa/jpl/aerie/merlin/protocol/types/ObjectComparator.java @@ -0,0 +1,75 @@ +package gov.nasa.jpl.aerie.merlin.protocol.types; + +import java.util.Comparator; +import java.util.Map; +import java.util.Set; +import java.util.SortedSet; + +/** + * A generic comparator to use when you don't have one and need one. This handles some common Collection types. + * There may be some others out there that could simplify or improve on this. + */ +public class ObjectComparator implements Comparator { + private static gov.nasa.jpl.aerie.merlin.protocol.types.ObjectComparator INSTANCE = null; + + public static gov.nasa.jpl.aerie.merlin.protocol.types.ObjectComparator getInstance() { + if (INSTANCE == null) { + INSTANCE = new gov.nasa.jpl.aerie.merlin.protocol.types.ObjectComparator(); + } + return INSTANCE; + } + + @SuppressWarnings("unchecked") + @Override + public int compare(Object o1, Object o2) { + if (o1 == o2) return 0; + if (o1 == null) return -1; + if (o2 == null) return 1; + + // Compare using Comparable if applicable. + // o1's comparator may assume the type of o2, causing a ClassCastException. + // This could result in poor performance if the exception is thrown regularly. + try { + if (o1 instanceof Comparable && o2 instanceof Comparable) { + return ((Comparable) o1).compareTo(o2); + } + } catch (ClassCastException t) { + } + + if (o1 instanceof Set && !(o1 instanceof SortedSet) && o2 instanceof Set && !(o2 instanceof SortedSet)) { + return Integer.compare(o1.hashCode(), o2.hashCode()); // this is sometimes sum of element hashcodes + } + + if (o1 instanceof Iterable && o2 instanceof Iterable) { + var i1 = ((Iterable) o1).iterator(); + var i2 = ((Iterable) o2).iterator(); + while (i1.hasNext() && i2.hasNext()) { + int c = compare(i1.next(), i2.next()); + if (c != 0) return c; + } + if (i1.hasNext()) return 1; + if (i2.hasNext()) return -1; + return 0; + } + + if (o1 instanceof Map && o2 instanceof Map) { + return compare(((Map) o1).entrySet(), ((Map) o2).entrySet()); + } + + if (o1 instanceof Map.Entry && o2 instanceof Map.Entry) { + int c = compare(((Map.Entry) o1).getKey(), ((Map.Entry) o2).getKey()); + if (c != 0) return c; + c = compare(((Map.Entry) o1).getValue(), ((Map.Entry) o2).getValue()); + return c; + } + + // Fallback comparison + int classComparison = o1.getClass().getName().compareTo(o2.getClass().getName()); + if (classComparison != 0) { + return classComparison; + } + + return Integer.compare(o1.hashCode(), o2.hashCode()); +// return Integer.compare(System.identityHashCode(o1), System.identityHashCode(o2)); + } +} diff --git a/merlin-sdk/src/main/java/gov/nasa/jpl/aerie/merlin/protocol/types/SerializedValue.java b/merlin-sdk/src/main/java/gov/nasa/jpl/aerie/merlin/protocol/types/SerializedValue.java index 3187d8051f..9c8dba185e 100644 --- a/merlin-sdk/src/main/java/gov/nasa/jpl/aerie/merlin/protocol/types/SerializedValue.java +++ b/merlin-sdk/src/main/java/gov/nasa/jpl/aerie/merlin/protocol/types/SerializedValue.java @@ -1,6 +1,7 @@ package gov.nasa.jpl.aerie.merlin.protocol.types; import java.math.BigDecimal; +import java.util.ArrayList; import java.util.List; import java.util.Map; import java.util.Objects; @@ -25,7 +26,7 @@ * code would need to know about all possible subclasses for deserialization). The Visitor * pattern on a class closed to extension allows us to guarantee that no ambiguity occurs. */ -public sealed interface SerializedValue { +public sealed interface SerializedValue extends Comparable { SerializedValue NULL = SerializedValue.ofNull(); /** @@ -39,6 +40,8 @@ public sealed interface SerializedValue { */ T match(Visitor visitor); + Object getValue(); + /** * An operation to be performed on the data contained in a {@link SerializedValue}. * @@ -60,11 +63,28 @@ interface Visitor { T onList(List value); } + @Override + default int compareTo(final SerializedValue o) { + return gov.nasa.jpl.aerie.merlin.protocol.types.ObjectComparator.getInstance().compare(this.getValue(), o.getValue()); + } + + record NullValue() implements SerializedValue { @Override public T match(final Visitor visitor) { return visitor.onNull(); } + + @Override + public Object getValue() { + return null; + } + + @Override + public int compareTo(final SerializedValue o) { + if (o instanceof NullValue) return 0; + return -1; + } } record NumericValue(BigDecimal value) implements SerializedValue { @@ -73,6 +93,11 @@ public T match(final Visitor visitor) { return visitor.onNumeric(value); } + @Override + public BigDecimal getValue() { + return value; + } + // `BigDecimal#equals` is too strict -- values differing only in representation need to be considered the same. @Override public boolean equals(final Object obj) { @@ -91,6 +116,10 @@ record BooleanValue(boolean value) implements SerializedValue { public T match(final Visitor visitor) { return visitor.onBoolean(value); } + @Override + public Boolean getValue() { + return value; + } } record StringValue(String value) implements SerializedValue { @@ -98,6 +127,10 @@ record StringValue(String value) implements SerializedValue { public T match(final Visitor visitor) { return visitor.onString(value); } + @Override + public String getValue() { + return value; + } } record MapValue(Map map) implements SerializedValue { @@ -105,6 +138,10 @@ record MapValue(Map map) implements SerializedValue { public T match(final Visitor visitor) { return visitor.onMap(map); } + @Override + public Map getValue() { + return map; + } } record ListValue(List list) implements SerializedValue { @@ -112,6 +149,10 @@ record ListValue(List list) implements SerializedValue { public T match(final Visitor visitor) { return visitor.onList(list); } + @Override + public List getValue() { + return list; + } } /**