Fixed fit method of ImmutableByteSequence

Also added unit tests.

Change-Id: I9ce7ef25ae25afb35b3315d38aeca4c19c55882c
diff --git a/core/net/src/test/java/org/onosproject/net/pi/impl/PiCriterionTranslatorsTest.java b/core/net/src/test/java/org/onosproject/net/pi/impl/PiCriterionTranslatorsTest.java
index de0ff3f..156be3a 100644
--- a/core/net/src/test/java/org/onosproject/net/pi/impl/PiCriterionTranslatorsTest.java
+++ b/core/net/src/test/java/org/onosproject/net/pi/impl/PiCriterionTranslatorsTest.java
@@ -17,22 +17,24 @@
 package org.onosproject.net.pi.impl;
 
 import org.junit.Test;
-import org.onlab.packet.MacAddress;
 import org.onlab.packet.EthType;
-import org.onlab.packet.VlanId;
-import org.onlab.packet.Ip6Address;
-import org.onlab.packet.TpPort;
-import org.onlab.packet.IpPrefix;
-import org.onlab.packet.MplsLabel;
 import org.onlab.packet.Ip4Address;
+import org.onlab.packet.Ip6Address;
+import org.onlab.packet.IpPrefix;
+import org.onlab.packet.MacAddress;
+import org.onlab.packet.MplsLabel;
+import org.onlab.packet.TpPort;
+import org.onlab.packet.VlanId;
 import org.onosproject.net.PortNumber;
+import org.onosproject.net.flow.criteria.ArpHaCriterion;
+import org.onosproject.net.flow.criteria.ArpOpCriterion;
+import org.onosproject.net.flow.criteria.ArpPaCriterion;
 import org.onosproject.net.flow.criteria.Criteria;
 import org.onosproject.net.flow.criteria.EthCriterion;
 import org.onosproject.net.flow.criteria.EthTypeCriterion;
 import org.onosproject.net.flow.criteria.IPCriterion;
-import org.onosproject.net.flow.criteria.PortCriterion;
-import org.onosproject.net.flow.criteria.UdpPortCriterion;
 import org.onosproject.net.flow.criteria.IPDscpCriterion;
+import org.onosproject.net.flow.criteria.IPEcnCriterion;
 import org.onosproject.net.flow.criteria.IPProtocolCriterion;
 import org.onosproject.net.flow.criteria.IPv6ExthdrFlagsCriterion;
 import org.onosproject.net.flow.criteria.IPv6FlowLabelCriterion;
@@ -47,27 +49,25 @@
 import org.onosproject.net.flow.criteria.MplsCriterion;
 import org.onosproject.net.flow.criteria.MplsTcCriterion;
 import org.onosproject.net.flow.criteria.PbbIsidCriterion;
+import org.onosproject.net.flow.criteria.PortCriterion;
 import org.onosproject.net.flow.criteria.SctpPortCriterion;
 import org.onosproject.net.flow.criteria.TcpFlagsCriterion;
 import org.onosproject.net.flow.criteria.TcpPortCriterion;
 import org.onosproject.net.flow.criteria.TunnelIdCriterion;
-import org.onosproject.net.flow.criteria.VlanPcpCriterion;
-import org.onosproject.net.flow.criteria.ArpHaCriterion;
-import org.onosproject.net.flow.criteria.ArpOpCriterion;
-import org.onosproject.net.flow.criteria.ArpPaCriterion;
-import org.onosproject.net.flow.criteria.IPEcnCriterion;
+import org.onosproject.net.flow.criteria.UdpPortCriterion;
 import org.onosproject.net.flow.criteria.VlanIdCriterion;
+import org.onosproject.net.flow.criteria.VlanPcpCriterion;
 import org.onosproject.net.pi.runtime.PiExactFieldMatch;
 import org.onosproject.net.pi.runtime.PiHeaderFieldId;
 import org.onosproject.net.pi.runtime.PiLpmFieldMatch;
 import org.onosproject.net.pi.runtime.PiTernaryFieldMatch;
+
 import java.util.Random;
+
 import static org.hamcrest.CoreMatchers.is;
 import static org.hamcrest.MatcherAssert.assertThat;
 import static org.onosproject.net.pi.impl.CriterionTranslatorHelper.translateCriterion;
-import static org.onosproject.net.pi.model.PiMatchType.EXACT;
-import static org.onosproject.net.pi.model.PiMatchType.TERNARY;
-import static org.onosproject.net.pi.model.PiMatchType.LPM;
+import static org.onosproject.net.pi.model.PiMatchType.*;
 
 /**
  * Tests for CriterionTranslators.
@@ -337,7 +337,7 @@
 
         PiExactFieldMatch exactMatch = (PiExactFieldMatch) translateCriterion(criterion, fieldId, EXACT, bitWidth);
 
-        assertThat(exactMatch.value().asReadOnlyBuffer().get(), is(criterion.tc()));
+        assertThat(exactMatch.value().asReadOnlyBuffer().get(1), is(criterion.tc()));
     }
 
     @Test
diff --git a/utils/misc/src/main/java/org/onlab/util/ImmutableByteSequence.java b/utils/misc/src/main/java/org/onlab/util/ImmutableByteSequence.java
index 07dda85..2f98cfb 100644
--- a/utils/misc/src/main/java/org/onlab/util/ImmutableByteSequence.java
+++ b/utils/misc/src/main/java/org/onlab/util/ImmutableByteSequence.java
@@ -78,8 +78,8 @@
      * the passed byte array, from/to the given indexes (inclusive).
      *
      * @param original a byte array value
-     * @param fromIdx starting index
-     * @param toIdx ending index
+     * @param fromIdx  starting index
+     * @param toIdx    ending index
      * @return a new immutable byte sequence
      */
     public static ImmutableByteSequence copyFrom(byte[] original, int fromIdx, int toIdx) {
@@ -190,9 +190,9 @@
      * Creates a new byte sequence that is prefixed with specified number of
      * zeros if val = 0 or ones if val = 0xff.
      *
-     * @param size number of total bytes
+     * @param size       number of total bytes
      * @param prefixBits number of bits in prefix
-     * @param val 0 for prefix of zeros; 0xff for prefix of ones
+     * @param val        0 for prefix of zeros; 0xff for prefix of ones
      * @return new immutable byte sequence
      */
     static ImmutableByteSequence prefix(int size, long prefixBits, byte val) {
@@ -212,7 +212,7 @@
     /**
      * Creates a new byte sequence that is prefixed with specified number of zeros.
      *
-     * @param size number of total bytes
+     * @param size       number of total bytes
      * @param prefixBits number of bits in prefix
      * @return new immutable byte sequence
      */
@@ -223,7 +223,7 @@
     /**
      * Creates a new byte sequence that is prefixed with specified number of ones.
      *
-     * @param size number of total bytes
+     * @param size       number of total bytes
      * @param prefixBits number of bits in prefix
      * @return new immutable byte sequence
      */
@@ -283,15 +283,51 @@
         return Objects.equal(this.value, other.value);
     }
 
+    /**
+     * Returns the index of the most significant bit (MSB), assuming a bit numbering scheme of type "LSB 0", i.e. the
+     * bit numbering starts at zero for the least significant bit (LSB). The MSB index of a byte sequence of zeros will
+     * be -1.
+     * <p>
+     * As an example, the following conditions always hold true:
+     * {@code
+     * ImmutableByteSequence.copyFrom(0).msbIndex() == -1
+     * ImmutableByteSequence.copyFrom(1).msbIndex() == 0
+     * ImmutableByteSequence.copyFrom(2).msbIndex() == 1
+     * ImmutableByteSequence.copyFrom(3).msbIndex() == 1
+     * ImmutableByteSequence.copyFrom(4).msbIndex() == 2
+     * ImmutableByteSequence.copyFrom(512).msbIndex() == 9
+     * }
+     *
+     * @return index of the MSB, -1 if the sequence has all bytes set to 0
+     */
+    public int msbIndex() {
+        int index = (size() * 8) - 1;
+        byteLoop:
+        for (int i = 0; i < size(); i++) {
+            byte b = value.get(i);
+            if (b != 0) {
+                for (int j = 7; j >= 0; j--) {
+                    byte mask = (byte) ((1 << j) - 1);
+                    if ((b & ~mask) != 0) {
+                        break byteLoop;
+                    }
+                    index--;
+                }
+            }
+            index -= 8;
+        }
+        return index;
+    }
+
     @Override
     public String toString() {
         return HexString.toHexString(value.array());
     }
 
     /**
-     * Trims or expands the given byte sequence so to fit a given bit-width. When trimming, the
-     * operations is deemed to be safe only if the trimmed bits are zero, otherwise an exception
-     * will be thrown. When expanding, the sequence will be padded with zeros. The returned byte
+     * Trims or expands the given byte sequence so to fit a given bit-width. When trimming, the operations is deemed to
+     * be safe only if the trimmed bits are zero, i.e. it is safe to trim only when {@code bitWidth > msbIndex()},
+     * otherwise an exception will be thrown. When expanding, the sequence will be padded with zeros. The returned byte
      * sequence will have minimum size to contain the given bit-width.
      *
      * @param original a byte sequence
@@ -307,51 +343,43 @@
 
         int newByteWidth = (int) Math.ceil((double) bitWidth / 8);
 
-        byte[] originalBytes = original.asArray();
+        if (bitWidth == original.size() * 8) {
+            // No need to fit.
+            return original;
+        }
+
+        ByteBuffer newBuffer = ByteBuffer.allocate(newByteWidth);
 
         if (newByteWidth > original.size()) {
-            // pad missing bytes with zeros
-            return ImmutableByteSequence.copyFrom(Arrays.copyOf(originalBytes, newByteWidth));
-        }
-
-        byte[] newBytes = new byte[newByteWidth];
-        // ImmutableByteSequence is always big-endian, hence check the array in reverse order
-        int diff = originalBytes.length - newByteWidth;
-        for (int i = originalBytes.length - 1; i >= 0; i--) {
-            byte ob = originalBytes[i]; // original byte
-            byte nb; // new byte
-            if (i > diff) {
-                // no need to truncate, copy as is
-                nb = ob;
-            } else if (i == diff) {
-                // truncate this byte, check if we're loosing something
-                byte mask = (byte) ((1 >> ((bitWidth % 8) + 1)) - 1);
-                if ((ob & ~mask) != 0) {
-                    throw new ByteSequenceTrimException(originalBytes, bitWidth);
-                } else {
-                    nb = (byte) (ob & mask);
+            // Pad extra bytes with 0's.
+            int numPadBytes = newByteWidth - original.size();
+            for (int i = 0; i < numPadBytes; i++) {
+                newBuffer.put((byte) 0x00);
+            }
+            newBuffer.put(original.asReadOnlyBuffer());
+        } else {
+            // Trim sequence.
+            if (bitWidth > original.msbIndex()) {
+                int diff = original.size() - newByteWidth;
+                ByteBuffer originalBuffer = original.asReadOnlyBuffer();
+                for (int i = diff; i < original.size(); i++) {
+                    newBuffer.put(originalBuffer.get(i));
                 }
             } else {
-                // drop this byte, check if we're loosing something
-                if (originalBytes[i] != 0) {
-                    throw new ByteSequenceTrimException(originalBytes, bitWidth);
-                } else {
-                    continue;
-                }
+                throw new ByteSequenceTrimException(original, bitWidth);
             }
-            newBytes[i - diff] = nb;
         }
 
-        return ImmutableByteSequence.copyFrom(newBytes);
+        return new ImmutableByteSequence(newBuffer);
     }
 
     /**
      * Signals that a byte sequence cannot be trimmed.
      */
     public static class ByteSequenceTrimException extends Exception {
-        ByteSequenceTrimException(byte[] bytes, int bitWidth) {
-            super(format("cannot trim %s into a %d long bits value",
-                         HexString.toHexString(bytes), bitWidth));
+        ByteSequenceTrimException(ImmutableByteSequence original, int bitWidth) {
+            super(format("cannot trim %s into a %d bits long value",
+                         original, bitWidth));
         }
     }
 }
diff --git a/utils/misc/src/test/java/org/onlab/util/ImmutableByteSequenceTest.java b/utils/misc/src/test/java/org/onlab/util/ImmutableByteSequenceTest.java
index 07aa40e..b297f6e 100644
--- a/utils/misc/src/test/java/org/onlab/util/ImmutableByteSequenceTest.java
+++ b/utils/misc/src/test/java/org/onlab/util/ImmutableByteSequenceTest.java
@@ -17,6 +17,7 @@
 package org.onlab.util;
 
 import com.google.common.testing.EqualsTester;
+import org.junit.Assert;
 import org.junit.Rule;
 import org.junit.Test;
 import org.junit.rules.ExpectedException;
@@ -25,6 +26,7 @@
 import java.nio.ByteOrder;
 import java.util.Random;
 
+import static java.lang.String.format;
 import static org.hamcrest.MatcherAssert.assertThat;
 import static org.hamcrest.Matchers.equalTo;
 import static org.hamcrest.Matchers.is;
@@ -124,59 +126,59 @@
     public void testBitSetMethods() throws Exception {
         // All zeros tests
         assertThat("3 bytes, all 0's",
-                ImmutableByteSequence.ofZeros(3),
-                is(equalTo(ImmutableByteSequence.copyFrom(
-                        new byte[]{0, 0, 0}))));
+                   ImmutableByteSequence.ofZeros(3),
+                   is(equalTo(ImmutableByteSequence.copyFrom(
+                           new byte[]{0, 0, 0}))));
         assertThat("3 bytes, all 0's via prefix",
-                ImmutableByteSequence.prefixZeros(3, 3 * Byte.SIZE),
-                is(equalTo(ImmutableByteSequence.copyFrom(
-                        new byte[]{0, 0, 0}))));
+                   ImmutableByteSequence.prefixZeros(3, 3 * Byte.SIZE),
+                   is(equalTo(ImmutableByteSequence.copyFrom(
+                           new byte[]{0, 0, 0}))));
 
         // All ones tests
         assertThat("3 bytes, all 1's",
-                ImmutableByteSequence.ofZeros(3),
-                is(equalTo(ImmutableByteSequence.copyFrom(
-                        new byte[]{0, 0, 0}))));
+                   ImmutableByteSequence.ofZeros(3),
+                   is(equalTo(ImmutableByteSequence.copyFrom(
+                           new byte[]{0, 0, 0}))));
         assertThat("3 bytes, all 1's via prefix",
-                ImmutableByteSequence.prefixOnes(3, 3 * Byte.SIZE),
-                is(equalTo(ImmutableByteSequence.copyFrom(
-                        new byte[]{(byte) 0xff, (byte) 0xff, (byte) 0xff}))));
+                   ImmutableByteSequence.prefixOnes(3, 3 * Byte.SIZE),
+                   is(equalTo(ImmutableByteSequence.copyFrom(
+                           new byte[]{(byte) 0xff, (byte) 0xff, (byte) 0xff}))));
 
         // Zero prefix tests
         assertThat("2 bytes, prefixed with 5 0's",
-                ImmutableByteSequence.prefix(2, 5, (byte) 0),
-                is(equalTo(ImmutableByteSequence.copyFrom(
-                        new byte[]{(byte) 0x7, (byte) 0xff}))));
+                   ImmutableByteSequence.prefix(2, 5, (byte) 0),
+                   is(equalTo(ImmutableByteSequence.copyFrom(
+                           new byte[]{(byte) 0x7, (byte) 0xff}))));
         assertThat("4 bytes, prefixed with 16 0's",
-                ImmutableByteSequence.prefix(4, 16, (byte) 0),
-                is(equalTo(ImmutableByteSequence.copyFrom(
-                        new byte[]{0, 0, (byte) 0xff, (byte) 0xff}))));
+                   ImmutableByteSequence.prefix(4, 16, (byte) 0),
+                   is(equalTo(ImmutableByteSequence.copyFrom(
+                           new byte[]{0, 0, (byte) 0xff, (byte) 0xff}))));
         assertThat("4 bytes, prefixed with 20 0's",
-                ImmutableByteSequence.prefix(4, 20, (byte) 0),
-                is(equalTo(ImmutableByteSequence.copyFrom(
-                        new byte[]{0, 0, (byte) 0x0f, (byte) 0xff}))));
+                   ImmutableByteSequence.prefix(4, 20, (byte) 0),
+                   is(equalTo(ImmutableByteSequence.copyFrom(
+                           new byte[]{0, 0, (byte) 0x0f, (byte) 0xff}))));
         assertThat("8 bytes, prefixed with 36 0's",
-                ImmutableByteSequence.prefixZeros(8, 38),
-                is(equalTo(ImmutableByteSequence.copyFrom(
-                        new byte[]{0, 0, 0, 0, (byte) 0x03, (byte) 0xff, (byte) 0xff, (byte) 0xff}))));
+                   ImmutableByteSequence.prefixZeros(8, 38),
+                   is(equalTo(ImmutableByteSequence.copyFrom(
+                           new byte[]{0, 0, 0, 0, (byte) 0x03, (byte) 0xff, (byte) 0xff, (byte) 0xff}))));
 
         // Ones prefix tests
         assertThat("2 bytes, prefixed with 5 1's",
-                ImmutableByteSequence.prefix(2, 5, (byte) 0xff),
-                is(equalTo(ImmutableByteSequence.copyFrom(
-                        new byte[]{(byte) 0xf8, 0}))));
+                   ImmutableByteSequence.prefix(2, 5, (byte) 0xff),
+                   is(equalTo(ImmutableByteSequence.copyFrom(
+                           new byte[]{(byte) 0xf8, 0}))));
         assertThat("4 bytes, prefixed with 16 1's",
-                ImmutableByteSequence.prefix(4, 16, (byte) 0xff),
-                is(equalTo(ImmutableByteSequence.copyFrom(
-                        new byte[]{(byte) 0xff, (byte) 0xff, 0, 0}))));
+                   ImmutableByteSequence.prefix(4, 16, (byte) 0xff),
+                   is(equalTo(ImmutableByteSequence.copyFrom(
+                           new byte[]{(byte) 0xff, (byte) 0xff, 0, 0}))));
         assertThat("4 bytes, prefixed with 20 1's",
-                ImmutableByteSequence.prefix(4, 20, (byte) 0xff),
-                is(equalTo(ImmutableByteSequence.copyFrom(
-                        new byte[]{(byte) 0xff, (byte) 0xff, (byte) 0xf0, 0}))));
+                   ImmutableByteSequence.prefix(4, 20, (byte) 0xff),
+                   is(equalTo(ImmutableByteSequence.copyFrom(
+                           new byte[]{(byte) 0xff, (byte) 0xff, (byte) 0xf0, 0}))));
         assertThat("8 bytes, prefixed with 10 1's",
-                ImmutableByteSequence.prefixOnes(8, 10),
-                is(equalTo(ImmutableByteSequence.copyFrom(
-                        new byte[]{(byte) 0xff, (byte) 0xc0, 0, 0, 0, 0, 0, 0}))));
+                   ImmutableByteSequence.prefixOnes(8, 10),
+                   is(equalTo(ImmutableByteSequence.copyFrom(
+                           new byte[]{(byte) 0xff, (byte) 0xc0, 0, 0, 0, 0, 0, 0}))));
     }
 
     @Test
@@ -186,4 +188,61 @@
                 "Expect IllegalArgumentException due to val = 0x7");
         ImmutableByteSequence.prefix(5, 10, (byte) 0x7);
     }
+
+    @Test
+    public void testMsbIndex() {
+        assertThat("Value 0 should have MSB index -1",
+                   ImmutableByteSequence.copyFrom(0).msbIndex(), is(-1));
+        for (int i = 0; i < 63; i++) {
+            long value = (long) Math.pow(2, i);
+            assertThat(format("Value %d should have MSB index %d", value, i),
+                       ImmutableByteSequence.copyFrom(value).msbIndex(), is(i));
+        }
+    }
+
+    private void checkIllegalFit(ImmutableByteSequence bytes, int bitWidth) {
+        try {
+            ImmutableByteSequence.fit(bytes, bitWidth);
+            Assert.fail(format("Except ByteSequenceTrimException due to value = %s and bitWidth %d",
+                               bytes.toString(), bitWidth));
+        } catch (ImmutableByteSequence.ByteSequenceTrimException e) {
+            // We expect this.
+        }
+    }
+
+    private void checkLegalFit(ImmutableByteSequence bytes, int bitWidth)
+            throws ImmutableByteSequence.ByteSequenceTrimException {
+        ImmutableByteSequence fitBytes = ImmutableByteSequence.fit(bytes, bitWidth);
+        ImmutableByteSequence sameBytes = ImmutableByteSequence.fit(fitBytes, bytes.size() * 8);
+        assertThat(format("Fitted value %s (re-extended to %s) not equal to original value %s",
+                          fitBytes, sameBytes, bytes),
+                   sameBytes,
+                   is(equalTo(bytes)));
+    }
+
+    @Test
+    public void testFit() throws ImmutableByteSequence.ByteSequenceTrimException {
+        // Test fit by forcing a given MSB index.
+        for (int msbIndex = 0; msbIndex < 32; msbIndex++) {
+            long value = (long) Math.pow(2, msbIndex);
+            ImmutableByteSequence bytes = ImmutableByteSequence.copyFrom(value);
+            checkLegalFit(bytes, msbIndex + 1);
+            if (msbIndex != 0) {
+                checkIllegalFit(bytes, msbIndex);
+            }
+        }
+    }
+
+    @Test
+    public void testRandomFit() throws ImmutableByteSequence.ByteSequenceTrimException {
+        // Test fit against the computed MSB index.
+        Random random = new Random();
+        for (int i = 0; i < 1000; i++) {
+            ImmutableByteSequence bytes = ImmutableByteSequence.copyFrom((long) Math.abs(random.nextInt()));
+            int msbIndex = bytes.msbIndex();
+            checkIllegalFit(bytes, msbIndex - random.nextInt(16));
+            checkLegalFit(bytes, msbIndex + 2 + random.nextInt(16));
+            checkLegalFit(bytes, msbIndex + 1);
+        }
+    }
 }
\ No newline at end of file