public static void triangulatePosition(Node[] nodes)
{
//List<DateTime> date, List< Int32 > node, List<Int32> x, List< Int32 > y, List<Int32> senOne, List< Int32 > senTwo, List<Int32> senThree, List< Int32 > senFour, List<Int32> senFive, List< Int32 > senSix
////////////////////////
/*
TODO:
-fix node degree accuracy - values were not returning correctly
-find base voltages of sensors? average values that did not trip the sensor?
-
*/
////////////////////////
bool isTripped = false;
int length = nodes.Length;
float[] nodeDegrees = new float[length];
float[] highSens = new float[length];
float[] secSens = new float[length];
float[] highSensIndex = new float[length];
float[] secSensIndex = new float[length];
float[] data;
/*
if (length == 1)
{
try
{
float degree = determineAngle(senOne[0], senTwo[0], senThree[0], senFour[0], senFive[0], senSix[0]);
//Console.WriteLine("Data: " + degree);
float[] tempArr = { senOne[0], senTwo[0], senThree[0], senFour[0], senFive[0], senSix[0] };
highSens[0] = tempArr.Max();//first highest sensor value
if (highSens[0] > 1)
{
Console.WriteLine("Data: " + degree + " sensor val: " + senOne[0] + " " + senTwo[0] + " " + senThree[0] + " " + senFour[0] + " " + senFive[0] + " " + senSix[0]);
}
}
catch { }
}
*/
if (length >= 3)
{
//calculate degree of each node
for (int i = 0; i < length; i++)
{
float[] tempArr = { nodes[i].SenOne, nodes[i].SenTwo, nodes[i].SenThr, nodes[i].SenFou, nodes[i].SenFiv, nodes[i].SenSix };
nodeDegrees[i] = determineAngle(nodes[i].SenOne, nodes[i].SenTwo, nodes[i].SenThr, nodes[i].SenFou, nodes[i].SenFiv, nodes[i].SenSix);
highSens[i] = tempArr.Max();//first highest sensor value
secSens[i] = (from num in tempArr orderby num descending select num).Skip(1).First();//second highest sensor value
highSensIndex[i] = Array.IndexOf(tempArr, highSens[i]);
secSensIndex[i] = Array.IndexOf(tempArr, secSens[i]);
if (highSens[i] > 40)
{
isTripped = true;
}
else
{
float tempVal = 0;
foreach (float element in tempArr)
{
tempVal += element;
}
switch (i)
{
case 0:
avgSensVal1 = (avgSensVal1 + tempVal / 6) / 2;
break;
case 1:
avgSensVal2 = (avgSensVal2 + tempVal / 6) / 2;
break;
case 2:
avgSensVal3 = (avgSensVal3 + tempVal / 6) / 2;
break;
}
}
}
if (isTripped)
{
data = triangulate((float)nodes[0].X, (float)nodes[1].X, (float)nodes[2].X, (float)nodes[0].Y, (float)nodes[1].Y, (float)nodes[2].Y, highSens[0], secSens[0], highSens[1], secSens[1], highSens[2], secSens[2], highSensIndex[0], secSensIndex[0], highSensIndex[1], secSensIndex[1], highSensIndex[2], secSensIndex[2], avgSensVal1, avgSensVal2, avgSensVal3);
double[] position = new double[2];
position = convToLatLon(data[0], data[1]);
//just for organizing the console a bit better
if (wasTripped)
{
//Console.WriteLine();
wasTripped = false;
}
if (position[0].ToString() == "NaN" || position[0] != Double.NaN)
{
if (position[0] > 41 && position[0] < 43 && position[1] > -81 && position[1] < -79)
{
string tempString = position[0] + "," + position[1];
Tuple<string, DateTime, bool> tempData = Tuple.Create(tempString, DateTime.Now, false);
//Console.WriteLine(tempString);
PipeThread.data.Add(tempData);
}
}
}
else
{
wasTripped = true;
//Console.Write("\rNo sensor tripped.");
}
/*
data[0] = x_est;
data[1] = y_est;
data[2] = I1_SE;
data[3] = I1_S;
data[4] = I2_SE;
data[5] = I2_S;
data[6] = I3_SE;
data[7] = I3_S;
*/
/* old code
float[] slope = new float[length];
for (int i = 0; i < length; i++)
{
slope[i] = calcSlope(nodeDegrees[i], x[i], y[i]);
}
float[] interceptX = new float[length * length];
float[] interceptY = new float[length * length];
int tempLength = 0;
for (int i = 0; i < length; i++)
{
for (int j = 0; j < length; j++)
{
if (i != j)
{
float _x;
float _y;
findIntercept(slope[i], slope[j], x[i], y[i], x[j], y[j], out _x, out _y);
interceptX[tempLength] = _x;
interceptY[tempLength] = _y;
if (senOne[i] >= 150 || senTwo[i] >= 150 || senThree[i] >= 150 || senFour[i] >= 150 || senFive[i] >= 150 || senSix[i] >= 150)
{
//Console.WriteLine(i + " " + nodeDegrees[i]);
}
tempLength++;
}
}
}
float avgX;
float avgY;
avgPoints(interceptX, interceptY, out avgX, out avgY);
for (int i = 0; i < length; i++)
{
if (senOne[i] >= 150 || senTwo[i] >= 150 || senThree[i] >= 150 || senFour[i] >= 150 || senFive[i] >= 150 || senSix[i] >= 150)
{
Console.WriteLine(avgX + " " + avgY);
}
}
*/
}
else
{
//Console.Write("\rConnect more nodes to begin. ");
}
}
//split string first via ':', then split via space
public static void parsePipeString(string _readLine)
{
bool dataIntact = true;
string[] dataSplit = _readLine.Split(' ');
try
{
if (dataSplit[0] == "Event")
{
//request audio for 1 minute interval 30 before 30 after
//request sensor data for that time
//"Event 1 |-32768 -> 32768| yyyy-MM-dd_HH-mm-ss.ffff"
//int nodeNum = Convert.ToInt32(dataSplit[1]);
//int amplitude = Math.Abs(Convert.ToInt32(dataSplit[2]));
DateTime dt = DateTime.ParseExact(dataSplit[1], "yyyy-MM-dd_HH-mm-ss.ffff", CultureInfo.InvariantCulture);
PipeThread.messages.Add("Request " + DateTime.Now.ToString("yyyy-MM-dd_HH-mm-ss.ffff"), DateTime.Now);
PipeThread.messages.Add("RequestAudio " + DateTime.Now.ToString("yyyy-MM-dd_HH-mm-ss.ffff") + " " + 20.ToString(), DateTime.Now);
Console.WriteLine("Added message: Event" + _readLine);
}
else if (dataSplit[0] == "Node")
{
if (!isInitialized)
{
nodes[0] = new Node();
nodes[1] = new Node();
nodes[2] = new Node();
}
DateTime dateT;
int nodeT, senOneT, senTwoT, senThreeT, senFourT, senFiveT, senSixT;
double xT, yT;
dateT = new DateTime();
nodeT = -1;
senOneT = -1;
senTwoT = -1;
senThreeT = -1;
senFourT = -1;
senFiveT = -1;
senSixT = -1;
Int32.TryParse(dataSplit[1], out nodeT);
double.TryParse(dataSplit[2], out xT);
double.TryParse(dataSplit[3], out yT);
Int32.TryParse(dataSplit[4], out senOneT);
Int32.TryParse(dataSplit[5], out senTwoT);
Int32.TryParse(dataSplit[6], out senThreeT);
Int32.TryParse(dataSplit[7], out senFourT);
Int32.TryParse(dataSplit[8], out senFiveT);
Int32.TryParse(dataSplit[9], out senSixT);
DateTime.TryParseExact(dataSplit[10], "yyyy-MM-dd_HH-mm-ss.ffff", CultureInfo.InvariantCulture, DateTimeStyles.None, out dateT);
//Console.WriteLine(dataSplit[0] + " " + dataSplit[1] + " " + dataSplit[2] + " " + dataSplit[3] + " " + dataSplit[4]);
//Console.WriteLine(dateT + " " + nodeT + " " + xT + " " + yT + " " + senOneT + " " + senTwoT + " " + senThreeT + " " + senFourT + " " + senFiveT + " " + senSixT);
if (dateT == new DateTime()) { dataIntact = false; };
if (nodeT == -1) { dataIntact = false; };
if (xT == 0) { dataIntact = false; };
if (yT == 0) { dataIntact = false; };
if (senOneT == -1) { dataIntact = false; };
if (senTwoT == -1) { dataIntact = false; };
if (senThreeT == -1) { dataIntact = false; };
if (senFourT == -1) { dataIntact = false; };
if (senFiveT == -1) { dataIntact = false; };
if (senSixT == -1) { dataIntact = false; };
//create node object once triangulation code can be scaled i.e. assume infinite nodes
if (nodeT > 3 || nodeT < 0)
{
dataIntact = false;
}
else
{
Console.WriteLine(xT + " " + yT);
nodes[nodeT].Date = dateT;
nodes[nodeT].NodeNumber = nodeT;
nodes[nodeT].X = xT;
nodes[nodeT].Y = yT;
nodes[nodeT].SenOne = senOneT;
nodes[nodeT].SenTwo = senTwoT;
nodes[nodeT].SenThr = senThreeT;
nodes[nodeT].SenFou = senFourT;
nodes[nodeT].SenFiv = senFiveT;
nodes[nodeT].SenSix = senSixT;
nodes[nodeT].Iterator++;
if (nodes[nodeT].Activated == false)
{
string tempString = "Node," + nodeT + "," + nodes[nodeT].X + "," + nodes[nodeT].Y;
Tuple<string, DateTime, bool> tempData = Tuple.Create(tempString, DateTime.Now, false);
//position = Calculation.convToLatLon(xT, yT);
PipeThread.data.Add(tempData);
//Console.WriteLine(tempString);
nodes[nodeT].Activated = true;
}
}
foreach (Node node in nodes)
{
if (node.Iterator > 50)
{
Console.WriteLine("Stagnated Data, check sensor connectivity");
}
}
if (dataIntact && nodes.Any(o => o.Activated == false))
{
Calculation.triangulatePosition(nodes);
foreach (Node node in nodes)
{
node.Iterator = 0;
}
}
else
{
//PipeReadThread.data.Add("Data not intact.");
}
//read backwards, take only one value,kill rest
/*
if (senI1 > 0 && senI2 > 0 && senI3 > 0)
{
if (senI1 > 1)
{
int pres = 0;
for (int i = node.ToArray().Length; i > 0; i--)
{
if (node[i - 1] == 0)
{
if (pres == 0)
{
pres++;//increment only
}
else
{
date.RemoveAt(i);
node.RemoveAt(i);
x.RemoveAt(i);
y.RemoveAt(i);
senOne.RemoveAt(i);
senTwo.RemoveAt(i);
senThree.RemoveAt(i);
senFour.RemoveAt(i);
senFive.RemoveAt(i);
senSix.RemoveAt(i);
}
}
}
}
if (senI2 > 1)
{
int pres = 0;
for (int i = node.ToArray().Length; i > 0; i--)
{
if (node[i - 1] == 1)
{
if (pres == 0)
{
pres++;//increment only
}
else
{
date.RemoveAt(i);
node.RemoveAt(i);
x.RemoveAt(i);
y.RemoveAt(i);
senOne.RemoveAt(i);
senTwo.RemoveAt(i);
senThree.RemoveAt(i);
senFour.RemoveAt(i);
senFive.RemoveAt(i);
senSix.RemoveAt(i);
}
}
}
}
if (senI3 > 1)
{
int pres = 0;
for (int i = node.ToArray().Length; i > 0; i--)
{
if (node[i - 1] == 2)
{
if (pres == 0)
{
pres++;//increment only
}
else
{
date.RemoveAt(i);
node.RemoveAt(i);
x.RemoveAt(i);
y.RemoveAt(i);
senOne.RemoveAt(i);
senTwo.RemoveAt(i);
senThree.RemoveAt(i);
senFour.RemoveAt(i);
senFive.RemoveAt(i);
senSix.RemoveAt(i);
}
}
}
}
Console.WriteLine(node.ToArray().Length);
Calculation.triangulatePosition(date, node, x, y, senOne, senTwo, senThree, senFour, senFive, senSix);
date.Clear();
node.Clear();
x.Clear();
y.Clear();
senOne.Clear();
senTwo.Clear();
senThree.Clear();
senFour.Clear();
senFive.Clear();
senSix.Clear();
sen1 = false;
sen2 = false;
sen3 = false;
senI1 = 0;
senI2 = 0;
senI3 = 0;
}
*/
}
}
catch (Exception exception)
{
Console.WriteLine("PipeParser: " + exception);
}
}
