iPerlHead : FLow filtering by FIRFilterFlow() corrected, ver. 2.18.1109

TBF/BenchControl/TestMethods/iPerlCommunication/iPerlHead/OptoTelegramRaw.cs

@@ -259,8 +259,9 @@ namespace TBF.BenchControl.TestMethods.iPerlCommunication.iPerlHead
 		///  7    ...
 		/// </summary>
 		/// <param name="optoData">array of OptoTelegramRaw objects</param>
-		/// <param name="optoDataCount">number of objects to process</param>
-		public static void FIRFilterFlow(OptoTelegramRaw[] optoData, int optoDataCount)
+        /// <param name="from">Index of the first optoData item to process</param>
+        /// <param name="to">Index of the last optoData item to process</param>
+        public static void FIRFilterFlow(OptoTelegramRaw[] optoData, int from, int to)
 		{
 			float[] kernel = new float[] { 0.1f, 0.2f, 0.4f, 0.2f, 0.1f };
 			int kSize = kernel.Length;
@@ -268,29 +269,52 @@ namespace TBF.BenchControl.TestMethods.iPerlCommunication.iPerlHead
 
 			float[] filtered = new float[kSize];
 
-			for (int i = 0; i < optoDataCount; i++)
+			for (int i = from; i <= to; i++)
 			{
-				int k = i - kSize;
-				if (k >= 0) optoData[k].RefFlow = filtered[i % kSize];
-
-				if (i < kSize2 || i >= optoDataCount - kSize2)
+                if ((i < from + kSize2) || (i > to - kSize2))
 				{
-					filtered[i % kSize] = optoData[i].RefFlow;
+                    /// Beginning or end of optoData buffer => Just copy data (=do not filter)
+					filtered[i % kSize] = optoData[BufferIdx(i)].RefFlow;
 				}
 				else
 				{
-					float weoightedSum = 0;
+                    /// Make a convolution of optoData and the kernel
+					float weightedSum = 0;
 					for (int j = -kSize2; j <= kSize2; j++)
-						weoightedSum += optoData[i + j].RefFlow * kernel[j + kSize2];
+						weightedSum += optoData[BufferIdx(i + j)].RefFlow * kernel[j + kSize2];
 
-					filtered[i % kSize] = weoightedSum;
+                    filtered[i % kSize] = weightedSum;
 				}
-			}
 
-			for (int k = optoDataCount - kSize; k < optoDataCount; k++)
+                if (i >= from + kSize)
+                {
+                    /// filtered[] buffer full => copy filtered data to optoData
+                    optoData[BufferIdx(i - kSize)].RefFlow = filtered[i % kSize];
+                }
+            }
+
+			for (int i = to - kSize + 1; i <= to; i++)
 			{
-				if (k >= 0) optoData[k].RefFlow = filtered[k % kSize];
+				if (i >= 0) optoData[BufferIdx(i)].RefFlow = filtered[i % kSize];
 			}
 		}
+
+        /// <summary>
+        /// Get index to optoData buffer
+        /// </summary>
+        /// <param name="index">Original unwrapped index</param>
+        /// <returns>Index to the buffer</returns>
+        public static int BufferIdx(int index)
+        {
+            if (index < IperlHead.MaxOptoDataCount)
+            {
+                return index;
+            }
+            else
+            {
+                return IperlHead.StartOptoDataCount + (index - IperlHead.MaxOptoDataCount) % IperlHead.EndOptoDataCount;
+            }
+        }
+
 	}
 }