// This functions processes a pattern, obtains the data and send it out to be written
private void Pattern(RawData data)
{
PeakQueueItem quI;
int ql;
long[] timestamps;
double fitLength = 15; //Amount of pixels to be used in fit of correlation
int halflength = (int)Math.Floor(fitLength / 2) + 1; // increased by 1 pixel to allow two fits
int length = patternLength; //Length of patterns
double[] crossCor = new double[(int)fitLength + 2]; // increased by 2 pixels to allow two fits
int startIndexRight = splitPixel; //Beginning of left pattern
int startIndexLeft = 0; //Beginning of left pattern
int endIndexRight = camWidth;
int pixshift = 1; // Direction of shift required to estimate slope of fit correction
float sum = 0;
double[] offset = new double[1];
double[] fit = new double[4];
double mu = 0;
double mu1 = 0;
double mu2 = 0;
double N = fitLength;
double y = 0;
double xy = 0;
double xxy = 0;
double b, c, D, Db, Dc;// a,b, and c are the values we solve for then derive mu,sigma, and A from them.
if (quittingBool)
{
return;
}
timestamps = new long[bufferSize];
newdata = new double[bufferSize, 2];
for (int frameNo = 0; frameNo < bufferSize; frameNo++)
{
try
{
frame = data.getData(frameNo);
if (firstFrame == true)
{
refFrame = frame;
firstFrame = false;
for (int j = splitPixel; j < frame.Length; j++)
{
if (frame[j] > threshold)
{
startIndexRightRef = j - pixelMargin;
break;
}
}
for (int j = 0; j < frame.Length; j++)
{
if (frame[j] > threshold)
{
startIndexLeftRef = j - pixelMargin;
break;
}
}
x = 0;
xSquar = 0;
xCube = 0;
xFourth = 0;
for (int j = 0; j < fitLength; j++)
{
x += j;
xSquar += j * j;
xCube += j * j * j;
xFourth += j * j * j * j;
}
}
//Finds beginning of pattern using a threshold
for (int j = splitPixel; j < frame.Length; j++)
{
if (frame[j] > threshold)
{
startIndexRight = j - pixelMargin;
break;
}
if (j == frame.Length - 1)
{
startIndexRight = 0;
}
}
Array.Copy(frame, flipFrame, frame.Length);
Array.Reverse(flipFrame);
for (int j = 0; j < flipFrame.Length; j++)
{
if (flipFrame[j] > threshold)
{
endIndexRight = flipFrame.Length - j + pixelMargin;
break;
}
if (j == frame.Length - 1)
{
endIndexRight = flipFrame.Length;
}
}
if (startIndexRight >= frame.Length || startIndexRight <= splitPixel || (endIndexRight - startIndexRight) < length)
{
timestamps[frameNo] = data.TimeStamp(frameNo);
newdata[frameNo, 0] = angleLastValue;
newdata[frameNo, 1] = refLastValue;
}
else
{
//Cuts length of pattern down if the pattern extends beyond the frame
while (length + startIndexRight + halflength + 1 >= frame.Length)
{
length = (int)Math.Round(length / 1.1);
}
//Calcualtes the crosscorrelation between the two patterns at shifts ; first time
for (int k = -halflength; k <= halflength; k++)
{
sum = 0;
for (int m = 0; m < length; m++)
{
if ((m + startIndexRight + k) > 0 && (m + startIndexRightRef) > 0)
{
if ((m + startIndexRight + k) < frame.Length && (m + startIndexRightRef) < refFrame.Length)
{
sum += frame[m + startIndexRight + k] * refFrame[m + startIndexRightRef];
}
}
}
crossCor[k + halflength] = sum;
}
//Sums x,x^2,x^3,x^4,ln(y),x ln(y),x^2 ln(y)
y = 0;
xy = 0;
xxy = 0;
for (int j = 0; j < fitLength; j++)
{
y += Math.Log(crossCor[j + 1]);
xy += j * Math.Log(crossCor[j + 1]);
xxy += j * j * Math.Log(crossCor[j + 1]);
}
//Solves system of equations using Cramer's rule
D = N * (xSquar * xFourth - xCube * xCube) - x * (x * xFourth - xCube * xSquar) + xSquar * (x * xCube - xSquar * xSquar);
//Da = y * (xSquar * xFourth - xCube * xCube) - x * (xy * xFourth - xCube * xxy) + xSquar * (xy * xCube - xSquar * xxy);
Db = N * (xy * xFourth - xCube * xxy) - y * (x * xFourth - xCube * xSquar) + xSquar * (x * xxy - xy * xSquar);
Dc = N * (xSquar * xxy - xy * xCube) - x * (x * xxy - xy * xSquar) + y * (x * xCube - xSquar * xSquar);
//a = Da / D;
b = Db / D;
c = Dc / D;
mu1 = -b / (2 * c);
// If fit-center is to left of center of crosscor pattern, shift cross-cor pattern by 1 pixel to right or vice versa
if (mu1 < (halflength - 1))
{
pixshift = -1;
}
else
{
pixshift = 1;
}
//Redo the fit
y = 0;
xy = 0;
xxy = 0;
for (int j = 0; j < fitLength; j++)
{
y += Math.Log(crossCor[j + 1 + pixshift]);
xy += j * Math.Log(crossCor[j + 1 + pixshift]);
xxy += j * j * Math.Log(crossCor[j + 1 + pixshift]);
}
D = N * (xSquar * xFourth - xCube * xCube) - x * (x * xFourth - xCube * xSquar) + xSquar * (x * xCube - xSquar * xSquar);
Db = N * (xy * xFourth - xCube * xxy) - y * (x * xFourth - xCube * xSquar) + xSquar * (x * xxy - xy * xSquar);
Dc = N * (xSquar * xxy - xy * xCube) - x * (x * xxy - xy * xSquar) + y * (x * xCube - xSquar * xSquar);
b = Db / D;
c = Dc / D;
mu2 = -b / (2 * c);
mu = (halflength - 1) - (mu1 - (halflength - 1)) * pixshift / (mu2 - mu1);
newdata[frameNo, 0] = mu + startIndexRight;
timestamps[frameNo] = data.TimeStamp(frameNo);
angleLastValue = mu + startIndexRight;
y = 0;
xy = 0;
xxy = 0;
//Finds beginning of pattern using a threshold
if (frameNo == 0)
{
for (int j = 0; j < frame.Length; j++)
{
if (frame[j] > threshold)
{
startIndexLeft = j - pixelMargin;
lightSourceStatus = 1;
break;
}
if (j == frame.Length - 1)
{
lightSourceStatus = 0;
}
}
if (startIndexLeft < 0)
{
startIndexLeft = 0;
}
}
//Calcualtes the crosscorrelation between the two patterns at shifts
for (int k = -halflength; k <= halflength; k++)
{
sum = 0;
for (int m = 0; m < length; m++)
{
if ((m + startIndexLeft + k) > 0 && (m + startIndexLeftRef) > 0)
{
sum += frame[m + startIndexLeft + k] * refFrame[m + startIndexLeftRef];
}
}
crossCor[k + halflength] = sum;
}
//Sums x,x^2,x^3,x^4,ln(y),x ln(y),x^2 ln(y)
for (int j = 0; j < fitLength; j++)
{
y += Math.Log(crossCor[j + 1]);
xy += j * Math.Log(crossCor[j + 1]);
xxy += j * j * Math.Log(crossCor[j + 1]);
}
//Solves system of equations using Cramer's rule
D = N * (xSquar * xFourth - xCube * xCube) - x * (x * xFourth - xCube * xSquar) + xSquar * (x * xCube - xSquar * xSquar);
//Da = y * (xSquar * xFourth - xCube * xCube) - x * (xy * xFourth - xCube * xxy) + xSquar * (xy * xCube - xSquar * xxy);
Db = N * (xy * xFourth - xCube * xxy) - y * (x * xFourth - xCube * xSquar) + xSquar * (x * xxy - xy * xSquar);
Dc = N * (xSquar * xxy - xy * xCube) - x * (x * xxy - xy * xSquar) + y * (x * xCube - xSquar * xSquar);
//a = Da / D;
b = Db / D;
c = Dc / D;
mu1 = -b / (2 * c);
// If fit-center is to left of center of crosscor pattern, shift cross-cor pattern by 1 pixel to right or vice versa
if (mu1 < (halflength - 1))
{
pixshift = -1;
}
else
{
pixshift = 1;
}
//Redo the fit
y = 0;
xy = 0;
xxy = 0;
for (int j = 0; j < fitLength; j++)
{
y += Math.Log(crossCor[j + 1 + pixshift]);
xy += j * Math.Log(crossCor[j + 1 + pixshift]);
xxy += j * j * Math.Log(crossCor[j + 1 + pixshift]);
}
D = N * (xSquar * xFourth - xCube * xCube) - x * (x * xFourth - xCube * xSquar) + xSquar * (x * xCube - xSquar * xSquar);
Db = N * (xy * xFourth - xCube * xxy) - y * (x * xFourth - xCube * xSquar) + xSquar * (x * xxy - xy * xSquar);
Dc = N * (xSquar * xxy - xy * xCube) - x * (x * xxy - xy * xSquar) + y * (x * xCube - xSquar * xSquar);
b = Db / D;
c = Dc / D;
mu2 = -b / (2 * c);
mu = (halflength - 1) - (mu1 - (halflength - 1)) * pixshift / (mu2 - mu1);
newdata[frameNo, 1] = mu + startIndexLeft;
refValue = mu + startIndexLeft;
newdata[frameNo, 0] = newdata[frameNo, 0] - refValue;
refLastValue = refValue;
}
}
catch (System.Threading.ThreadAbortException) { }
catch (Exception ex)
{
EmailError.emailAlert(ex);
timestamps[frameNo] = data.TimeStamp(frameNo);
newdata[frameNo, 0] = angleLastValue;
newdata[frameNo, 1] = refLastValue;
throw ex;
}
sum = frame.Select(x => (int)x).Sum();
Debug.WriteLine(sum);
if (sum == 0)
{
lightSourceStatus = 0;
}
else
{
lightSourceStatus = 1;
}
}
quI = new PeakQueueItem(timestamps, newdata);
lock (((ICollection)dataWritingQueue).SyncRoot)
{
while (dataWritingQueue.Count > 10)
{
dataWritingQueue.Dequeue();
}
dataWritingQueue.Enqueue(quI);
ql = dataWritingQueue.Count;
}
dataWritingSyncEvent.NewItemEvent.Set();
setTextBox2(data.QueueLen.ToString());
setTextBox3(ql.ToString());
if (quittingBool)
{
return;
}
}
// Local data writing
void dataWrite()
{
int f;
PeakQueueItem curItem;
long curTimeStamp;
double[] lowPassedData;
double[,] values;
values = new double[10000, 2];
curItem = new PeakQueueItem();
while (dataWritingThreadBool)
{
if (quittingBool)
{
return;
}
while ((WaitHandle.WaitAny(dataWritingSyncEvent.EventArray) != 1))
{
if (quittingBool)
{
return;
}
lock (((ICollection)dataWritingQueue).SyncRoot)
{
curItem = dataWritingQueue.Dequeue();
if (dataWritingQueue.Count >= 1)
{
dataWritingSyncEvent.NewItemEvent.Set();
}
}
curTimeStamp = 0;
for (f = 0; f < bufferSize; f++)
{
curTimeStamp = curItem.TimeStamp[f];
values[f, 0] = curItem.Data[f, 0]; //Diff
values[f, 1] = curItem.Data[f, 1]; //Ref
}
lowPassedData = lp(values, bufferSize);
dataSend(lowPassedData);
f = f - 1;
if (recordBool)
{
myDataWriter.Write(curTimeStamp / sampFreq / 3600 / 24 + dayFrameCo0, lowPassedData);
}
if (graphWindow != null && !graphWindow.IsDisposed)
{
int sumN = 0;
for (int i = 0; i < newdata.GetLength(0); i++)
{
if (newdata[i, 0] > 0)
{
graphSum += newdata[i, 0];
sumN++;
}
}
graphSum = graphSum / ((double)sumN);
graphData outData = new graphData(frame, graphSum);
lock (graphLock)
{
graphQueue.Enqueue(outData);
}
graphSignal.Set();
graphSum = 0;
}
}
}
}
