DEV-574 improved throw bytes performance and console prints

ShimmerDriver/src/main/java/com/shimmerresearch/bluetooth/ShimmerBluetooth.java

@@ -715,7 +715,7 @@ else if((mCurrentCommand==GET_FW_VERSION_COMMAND)
 					}
 					threadSleep((int)((Math.random()+.1)*100.0));
 					writeBytes(insBytes);
-					printLogDataForDebugging("Command Transmitted: \t\t\t" + btCommandToString(mCurrentCommand) + " " + UtilShimmer.bytesToHexStringWithSpacesFormatted(insBytes));
+					printLogDataForDebugging("Command Transmitted: \t\t\t" + btCommandToString(mCurrentCommand) + ", len=" + insBytes.length + " " + UtilShimmer.bytesToHexStringWithSpacesFormatted(insBytes));
 
 					//TODO: are the two stops needed here? better to wait for ack from Shimmer
 					if(mCurrentCommand==STOP_STREAMING_COMMAND 
@@ -880,7 +880,7 @@ private void processNotStreamingWaitForResp() {
 						mWaitForResponse=false;
 						mTransactionCompleted=true;
 						setInstructionStackLock(false);
-						printLogDataForDebugging("Response Received:\t\t\t" + btCommandToString(responseCommand));
+						printLogDataForDebugging("Response Received:\t\t\t" + btCommandToString(responseCommand) + ", len=" + byteBuffer.length + " " + UtilShimmer.bytesToHexStringWithSpacesFormatted(byteBuffer));
 
 						// Special case for FW_VERSION_RESPONSE because it
 						// needs to initialize the Shimmer after releasing
@@ -939,7 +939,7 @@ protected void processPacket() {
 				&& bufferTemp[getPacketSizeWithCrc()+1]==DATA_PACKET){
 
 			if (mBtCommsCrcModeCurrent != BT_CRC_MODE.OFF && !checkCrc(bufferTemp, getPacketSize() + 1)) {
-				discardFirstBufferByte();
+				discardBufferBytesToNextPacket();
 				return;
 			}
 
@@ -1002,17 +1002,19 @@ else if(isSupportedInStreamCmds() && bufferTemp[getPacketSizeWithCrc()+2]==INSTR
 				}
 			} 
 			if(mByteArrayOutputStream.size()>getPacketSizeWithCrc()+2){
-				printLogDataForDebugging("Unknown packet error (check with JC):\tExpected: " + (getPacketSizeWithCrc()+2) + "bytes but buffer contains " + mByteArrayOutputStream.size() + "bytes");
-				discardFirstBufferByte(); //throw the first byte away
+				printLogDataForDebugging("Unknown packet error: \tExpected: " + (getPacketSizeWithCrc()+2) + "bytes but buffer contains " + mByteArrayOutputStream.size() + "bytes");
+				discardBufferBytesToNextPacket(); // Skip to start of next packet
 			}
 
 		} 
 		//TODO: ACK in bufferTemp[0] not handled
 		//else if
 		else {
-			printLogDataForDebugging("Packet syncing problem:\tExpected: " + (getPacketSizeWithCrc()+2) + "bytes. Buffer contains " + mByteArrayOutputStream.size() + "bytes" 
-									+ "\nBuffer = " + UtilShimmer.bytesToHexStringWithSpacesFormatted(mByteArrayOutputStream.toByteArray()));
-			discardFirstBufferByte(); //throw the first byte away
+			printLogDataForDebugging("Unexpected packet header bytes. Expected size: " + getPacketSizeWithCrc()
+					+ ", Packet 1 header: " + UtilShimmer.byteToHexStringFormatted(bufferTemp[0])
+					+ ", Packet 2 header: " + UtilShimmer.byteToHexStringFormatted(bufferTemp[getPacketSizeWithCrc() + 1]) 
+					+ "\nBuffer: " + UtilShimmer.bytesToHexStringWithSpacesFormatted(mByteArrayOutputStream.toByteArray()));
+			discardBufferBytesToNextPacket(); // Skip to start of next packet
 		}
 	}
 
@@ -1052,7 +1054,7 @@ public boolean checkCrc(byte[] bufferTemp, int length) {
 		if (mBtCommsCrcModeCurrent == BT_CRC_MODE.TWO_BYTE_CRC) {
 			// + 2 as this is the location of the CRC's MSB
 			if (bufferTemp[getPacketSize() + 2] != crcCalc[1]) {
-				discardFirstBufferByte();
+				discardBufferBytesToNextPacket();
 				return false;
 			}
 		}
@@ -1294,16 +1296,38 @@ protected void clearBuffers() {
 	}
 
 	/**
-	 * 
+	 * Next packet start should begin with DATA_PACKET or ACK_COMMAND_PROCESSED byte so skip to that point
 	 */
-	protected void discardFirstBufferByte(){
+	protected void discardBufferBytesToNextPacket(){
 		byte[] bTemp = mByteArrayOutputStream.toByteArray();
+
+		//Find index of first DATA_PACKET or ACK byte within the buffer
+		int offset = findOffsetOfNextZeroOrFF(bTemp);
+		//If not found, just skip one byte
+		offset = (offset == -1) ? 1 : offset;
+
 		mByteArrayOutputStream.reset();
-		mByteArrayOutputStream.write(bTemp, 1, bTemp.length-1); //this will throw the first byte away
+		mByteArrayOutputStream.write(bTemp, offset, bTemp.length-offset); // discard first 'offset' bytes
 		if(mEnablePCTimeStamps) {
-			mListofPCTimeStamps.remove(0);
+			// Remove first `offset` elements from the original list (destructive, modifies same list)
+			mListofPCTimeStamps.subList(0, Math.min(offset, mListofPCTimeStamps.size())).clear();
+		}
+		consolePrintLn("Throw Bytes " + UtilShimmer.bytesToHexStringWithSpacesFormatted(Arrays.copyOfRange(bTemp, 0, offset)));
+	}
+
+	/**
+	 * Finds the offset/index of the next DATA_PACKET (0x00) or ACK_COMMAND_PROCESSED (0xFF) byte.
+	 * 
+	 * @param buffer a byte array to search within
+	 * @return index of the first 0x00 or 0xFF byte found after position 0, or -1 if not found
+	 */
+	private static int findOffsetOfNextZeroOrFF(byte[] buffer) {
+		for (int i = 1; i < buffer.length; i++) {
+			byte b = buffer[i];
+			if (b == 0 || b == (byte) 0xFF)
+				return i;
 		}
-		consolePrintLn("Throw Byte" + UtilShimmer.bytesToHexStringWithSpacesFormatted(bTemp));
+		return -1;
 	}
 
 	/**

ShimmerDriver/src/main/java/com/shimmerresearch/comms/wiredProtocol/ShimmerCrc.java

@@ -9,7 +9,7 @@ public class ShimmerCrc {
 	 * @param b the byte to calculate the CRC on
 	 * @return the new CRC value
 	 */
-	protected static int shimmerUartCrcByte(int crc, byte b) {
+	public static int shimmerUartCrcByte(int crc, byte b) {
     	crc &= 0xFFFF;
         crc = ((crc & 0xFFFF) >>> 8) | ((crc & 0xFFFF) << 8);
         crc ^= (b&0xFF);

ShimmerDriverPC/src/test/java/com/shimmerresearch/shimmer3/communication/API_0000X_ByteCommunicationShimmer3_Streaming_Contiguous_Check.java

@@ -0,0 +1,114 @@
+package com.shimmerresearch.shimmer3.communication;
+import org.junit.Before;
+import org.junit.BeforeClass;
+import org.junit.FixMethodOrder;
+import org.junit.Test;
+import org.junit.runners.MethodSorters;
+
+import com.shimmerresearch.bluetooth.ShimmerBluetooth.BT_STATE;
+import com.shimmerresearch.driver.BasicProcessWithCallBack;
+import com.shimmerresearch.driver.CallbackObject;
+import com.shimmerresearch.driver.ObjectCluster;
+import com.shimmerresearch.driver.ShimmerMsg;
+import com.shimmerresearch.driver.Configuration.COMMUNICATION_TYPE;
+import com.shimmerresearch.driverUtilities.ChannelDetails;
+import com.shimmerresearch.driverUtilities.SensorDetails;
+import com.shimmerresearch.driverUtilities.ShimmerVerDetails.HW_ID;
+import com.shimmerresearch.exceptions.ShimmerException;
+import com.shimmerresearch.pcDriver.ShimmerPC;
+import com.shimmerresearch.sensors.AbstractSensor;
+import com.shimmerresearch.sensors.AbstractSensor.SENSORS;
+
+import bolts.TaskCompletionSource;
+
+import static org.junit.Assert.assertEquals;
+
+import java.util.ArrayList;
+import java.util.LinkedHashMap;
+import java.util.concurrent.TimeUnit;
+@FixMethodOrder(MethodSorters.NAME_ASCENDING) // Test methods will be run in alphabetical order
+public class API_0000X_ByteCommunicationShimmer3_Streaming_Contiguous_Check extends BasicProcessWithCallBack{
+	ShimmerPC mDevice;
+	TaskCompletionSource<Boolean> mCalibrationTask;
+	TaskCompletionSource<Boolean> mStreamingTask;
+	ByteCommunicationSimulatorS3 mByteCommunicationSimulatorS3;
+
+	@Before
+    public void setUp() {
+		mByteCommunicationSimulatorS3 = new ByteCommunicationSimulatorS3_streaming_timestamp_contiguous_check("COM99");
+		mDevice = new ShimmerPC("COM99");
+		mDevice.setTestRadio(mByteCommunicationSimulatorS3);
+		setWaitForData(mDevice);
+    }
+
+    ArrayList<ObjectCluster> mListOJC;
+    @Test
+    public void test001_testStreaming() {
+    	mListOJC = new ArrayList<ObjectCluster>();
+    	mByteCommunicationSimulatorS3.setIsNewBMPSupported(false);
+    	mCalibrationTask = new TaskCompletionSource<Boolean>();
+    	mDevice.connect("","");
+
+		mCalibrationTask = new TaskCompletionSource<>();
+		mStreamingTask = new TaskCompletionSource<Boolean>();
+		try {
+			boolean result = mCalibrationTask.getTask().waitForCompletion(5, TimeUnit.SECONDS);
+		} catch (InterruptedException e) {
+			// TODO Auto-generated catch block
+			e.printStackTrace();
+		}
+
+		try {
+			mDevice.startStreaming();
+		} catch (ShimmerException e) {
+			// TODO Auto-generated catch block
+			e.printStackTrace();
+		}
+		try {
+			mStreamingTask.getTask().waitForCompletion(2, TimeUnit.SECONDS);
+			if(mListOJC.size()!=2) {//only one is expected because only the first packet is followed  which meets ShimmerBluetooth.processPacket requirements
+	    		assert(false);
+			}
+		} catch (InterruptedException e) {
+			// TODO Auto-generated catch block
+			e.printStackTrace();
+		}
+
+
+
+    }
+
+
+	@Override
+	protected void processMsgFromCallback(ShimmerMsg shimmerMSG) {
+		// TODO Auto-generated method stub
+		  int ind = shimmerMSG.mIdentifier;
+
+		  Object object = (Object) shimmerMSG.mB;
+
+		if (ind == ShimmerPC.MSG_IDENTIFIER_STATE_CHANGE) {
+			if (ind == ShimmerPC.MSG_IDENTIFIER_STATE_CHANGE) {
+				CallbackObject callbackObject = (CallbackObject)object;
+
+				if (callbackObject.mState == BT_STATE.CONNECTED) {
+					if (mDevice.isInitialised()) {
+						try {
+							Thread.sleep(200);
+						} catch (InterruptedException e) {
+							// TODO Auto-generated catch block
+							e.printStackTrace();
+						}
+
+					}
+
+				}
+		}
+	} else if (ind == ShimmerPC.MSG_IDENTIFIER_DATA_PACKET) {
+		System.out.println("Shimmer MSG_IDENTIFIER_DATA_PACKET");
+		ObjectCluster objc = (ObjectCluster) shimmerMSG.mB;
+		mListOJC.add(objc);
+	} 
+
+}
+
+}

ShimmerDriverPC/src/test/java/com/shimmerresearch/shimmer3/communication/API_0000X_ByteCommunicationShimmer3_Streaming_Contiguous_Check_Fail.java

@@ -0,0 +1,116 @@
+package com.shimmerresearch.shimmer3.communication;
+import org.junit.Before;
+import org.junit.BeforeClass;
+import org.junit.FixMethodOrder;
+import org.junit.Test;
+import org.junit.runners.MethodSorters;
+
+import com.shimmerresearch.bluetooth.ShimmerBluetooth.BT_STATE;
+import com.shimmerresearch.driver.BasicProcessWithCallBack;
+import com.shimmerresearch.driver.CallbackObject;
+import com.shimmerresearch.driver.ObjectCluster;
+import com.shimmerresearch.driver.ShimmerMsg;
+import com.shimmerresearch.driver.ShimmerObject;
+import com.shimmerresearch.driver.Configuration.COMMUNICATION_TYPE;
+import com.shimmerresearch.driverUtilities.ChannelDetails;
+import com.shimmerresearch.driverUtilities.SensorDetails;
+import com.shimmerresearch.driverUtilities.ShimmerVerDetails.HW_ID;
+import com.shimmerresearch.exceptions.ShimmerException;
+import com.shimmerresearch.pcDriver.ShimmerPC;
+import com.shimmerresearch.sensors.AbstractSensor;
+import com.shimmerresearch.sensors.AbstractSensor.SENSORS;
+
+import bolts.TaskCompletionSource;
+
+import static org.junit.Assert.assertEquals;
+
+import java.util.ArrayList;
+import java.util.LinkedHashMap;
+import java.util.concurrent.TimeUnit;
+@FixMethodOrder(MethodSorters.NAME_ASCENDING) // Test methods will be run in alphabetical order
+public class API_0000X_ByteCommunicationShimmer3_Streaming_Contiguous_Check_Fail extends BasicProcessWithCallBack{
+	ShimmerPC mDevice;
+	TaskCompletionSource<Boolean> mCalibrationTask;
+	TaskCompletionSource<Boolean> mStreamingTask;
+	ByteCommunicationSimulatorS3 mByteCommunicationSimulatorS3;
+	public static int CONTIGUOUS_TIMESTAMP_TICKS_LIMIT = (10*32768);
+	@Before
+    public void setUp() {
+		mByteCommunicationSimulatorS3 = new ByteCommunicationSimulatorS3_streaming_timestamp_contiguous_check_fail("COM99");
+		mDevice = new ShimmerPC("COM99");
+		mDevice.setTestRadio(mByteCommunicationSimulatorS3);
+		setWaitForData(mDevice);
+    }
+
+    ArrayList<ObjectCluster> mListOJC;
+    @Test
+    public void test001_testStreaming() {
+    	mListOJC = new ArrayList<ObjectCluster>();
+    	mByteCommunicationSimulatorS3.setIsNewBMPSupported(false);
+    	mCalibrationTask = new TaskCompletionSource<Boolean>();
+    	mDevice.connect("","");
+
+		mCalibrationTask = new TaskCompletionSource<>();
+		mStreamingTask = new TaskCompletionSource<Boolean>();
+		try {
+			boolean result = mCalibrationTask.getTask().waitForCompletion(5, TimeUnit.SECONDS);
+		} catch (InterruptedException e) {
+			// TODO Auto-generated catch block
+			e.printStackTrace();
+		}
+
+		try {
+			mDevice.startStreaming();
+			System.out.println("Limit = " + CONTIGUOUS_TIMESTAMP_TICKS_LIMIT);
+		} catch (ShimmerException e) {
+			// TODO Auto-generated catch block
+			e.printStackTrace();
+		}
+		try {
+			mStreamingTask.getTask().waitForCompletion(2, TimeUnit.SECONDS);
+			if(mListOJC.size()!=2) {//only one is expected because only the first packet is followed  which meets ShimmerBluetooth.processPacket requirements
+	    		assert(false);
+			}
+		} catch (InterruptedException e) {
+			// TODO Auto-generated catch block
+			e.printStackTrace();
+		}
+
+
+
+    }
+
+
+	@Override
+	protected void processMsgFromCallback(ShimmerMsg shimmerMSG) {
+		// TODO Auto-generated method stub
+		  int ind = shimmerMSG.mIdentifier;
+
+		  Object object = (Object) shimmerMSG.mB;
+
+		if (ind == ShimmerPC.MSG_IDENTIFIER_STATE_CHANGE) {
+			if (ind == ShimmerPC.MSG_IDENTIFIER_STATE_CHANGE) {
+				CallbackObject callbackObject = (CallbackObject)object;
+
+				if (callbackObject.mState == BT_STATE.CONNECTED) {
+					if (mDevice.isInitialised()) {
+						try {
+							Thread.sleep(200);
+						} catch (InterruptedException e) {
+							// TODO Auto-generated catch block
+							e.printStackTrace();
+						}
+
+					}
+
+				}
+		}
+	} else if (ind == ShimmerPC.MSG_IDENTIFIER_DATA_PACKET) {
+		System.out.println("Shimmer MSG_IDENTIFIER_DATA_PACKET");
+		ObjectCluster objc = (ObjectCluster) shimmerMSG.mB;
+		mListOJC.add(objc);
+	} 
+
+}
+
+}