private void RotateSide(TubeM currentPos, int Angle)
{
var kondi = 1;
switch (Angle)
{
case 270:
kondi = -1;
break;
default:
kondi = 1;
break;
}
var centerRow = (SidePincer1.Row + SidePincer2.Row) / 2;
var centerColumn = (SidePincer1.Column + SidePincer2.Column) / 2;
var secondPincer1col = centerColumn + (12 + TranslationMainUp) * kondi * Math.Round(Math.Sin((RotationSide + 90) * Math.PI / 180));
var secondPincer2col = centerColumn - (12 + TranslationMainUp) * kondi * Math.Round(Math.Sin((RotationSide + 90) * Math.PI / 180));
var secondPincer1row = centerRow + (12 + TranslationMainUp) * kondi * Math.Round(Math.Cos((RotationSide + 90) * Math.PI / 180));
var secondPincer2row = centerRow - (12 + TranslationMainUp) * kondi * Math.Round(Math.Cos((RotationSide + 90) * Math.PI / 180));
if (Tubes.Where(x => x.Row == secondPincer1row).Where(y => y.Column == secondPincer1col).Where(x => x.ColorProp != "Black").FirstOrDefault() != null &&
Tubes.Where(x => x.Row == secondPincer2row).Where(y => y.Column == secondPincer2col).Where(x => x.ColorProp != "Black").FirstOrDefault() != null)
{
RotationSide = (short)(RotationSide + kondi * 90);
SidePincer1 = Tubes.Where(x => x.Row == secondPincer1row).Where(y => y.Column == secondPincer1col).FirstOrDefault();
SidePincer2 = Tubes.Where(x => x.Row == secondPincer2row).Where(y => y.Column == secondPincer2col).FirstOrDefault();
}
}
private void Pathfinder(TubeM currentPos, int row, int column, List <TubeM> pathList)
{
bool rS = false;
bool cS = false;
if ((EnNode.Row - currentPos.Row) > 0)
{
row += 1;
rS = true;
}
else if ((EnNode.Row - currentPos.Row) < 0)
{
row -= 1;
rS = true;
}
if ((EnNode.Column - currentPos.Column) > 0)
{
column += 1;
cS = true;
}
else if ((EnNode.Column - currentPos.Column) < 0)
{
column -= 1;
cS = true;
}
var currentPos1 = Tubes.Where(x => x.Row == row).Where(y => y.Column == column).Where(x => x.ColorProp != "Black").FirstOrDefault();
if (currentPos1 == null)
{
if (rS && cS)
{
currentPos1 = Tubes.Where(x => x.Row == currentPos.Row).Where(y => y.Column == column).Where(x => x.ColorProp != "Black").FirstOrDefault();
if (currentPos1 != null)
{
currentPos = currentPos1;
row = currentPos.Row;
}
else
{
currentPos = Tubes.Where(x => x.Row == row).Where(y => y.Column == currentPos.Column).Where(x => x.ColorProp != "Black").FirstOrDefault();
column = currentPos.Column;
}
}
//additional elses must be added
}
else
{
currentPos = currentPos1;
}
pathList.Add(currentPos);
if (currentPos != EnNode)
{
if (currentPos.ColorProp != "Red")
{
currentPos.ColorProp = "Blue";
}
Pathfinder(currentPos, row, column, pathList);
}
}
private void ResetColor()
{
var blueTubes = Tubes.Where(x => x.ColorProp == "Blue");
foreach (var item in blueTubes)
{
item.ColorProp = "Gray";
}
}
private TubeM ShiftMain(TubeM currentPos, List <int> coordinates)
{
var rows = coordinates.First();
var columns = coordinates.Last();
if ((rows * rows + columns * columns) > 98)
{//needs improvement
int i = 6 - TranslationMainUp;
while (i > 0)
{
var rowMain1 = MainPincer1.Row + Math.Round(Math.Cos(Math.PI * RotationMain / 180)) * i;
var columnMain1 = MainPincer1.Column + Math.Round(Math.Sin(Math.PI * RotationMain / 180)) * i;
var rowMain2 = MainPincer2.Row + Math.Round(Math.Cos(Math.PI * RotationMain / 180)) * i;
var columnMain2 = MainPincer2.Column + Math.Round(Math.Sin(Math.PI * RotationMain / 180)) * i;
if (Tubes.Where(x => x.Row == rowMain1).Where(y => y.Column == columnMain1).Where(x => x.ColorProp != "Black").FirstOrDefault() != null &&
Tubes.Where(x => x.Row == rowMain2).Where(y => y.Column == columnMain2).Where(x => x.ColorProp != "Black").FirstOrDefault() != null)
{
MainPincer1 = Tubes.Where(x => x.Row == rowMain1).Where(y => y.Column == columnMain1).FirstOrDefault();
MainPincer2 = Tubes.Where(x => x.Row == rowMain2).Where(y => y.Column == columnMain2).FirstOrDefault();
var currentPosrow = currentPos.Row + Math.Round(Math.Cos(Math.PI * RotationMain / 180)) * i;
var currentPoscol = currentPos.Column + Math.Round(Math.Sin(Math.PI * RotationMain / 180)) * i;
currentPos = Tubes.Where(x => x.Row == currentPosrow).Where(y => y.Column == currentPoscol).FirstOrDefault();
pathList.Add(currentPos);
break;
}
i = i - 1;
}
if (i == 0)
{
if (TranslationMainRight < 12)
{
TranslationMainRight += 1;
ShiftMain(currentPos, coordinates);
}
else
{
currentPos = TryToRotate(currentPos, 90);
ShiftMain(currentPos, coordinates);
}
}
TranslationMainUp = (short)(TranslationMainUp + i);
}
else
{
currentPos = EnNode;
}
return(currentPos);
}
private void SetStartPincers()
{
var mainPincer1row = -1 * Math.Round(Math.Cos(Math.PI * StartingAngle / 180)) + StNode.Row;
var mainPincer1col = -1 * Math.Round(Math.Sin(Math.PI * StartingAngle / 180)) + StNode.Column;
var mainPincer2row = -15 * Math.Round(Math.Cos(Math.PI * StartingAngle / 180)) + StNode.Row;
var mainPincer2col = -15 * Math.Round(Math.Sin(Math.PI * StartingAngle / 180)) + StNode.Column;
MainPincer1 = Tubes.Where(x => x.Row == mainPincer1row).Where(x => x.Column == mainPincer1col).Where(x => x.ColorProp != "Black").FirstOrDefault();
MainPincer2 = Tubes.Where(x => x.Row == mainPincer2row).Where(x => x.Column == mainPincer2col).Where(x => x.ColorProp != "Black").FirstOrDefault();
if (MainPincer1 == null || MainPincer2 == null)
{
ResetCoordinates();
return;
}
RotationMain = StartingAngle;
RotationSide = (short)(RotationMain + 45);
var centerRow = (mainPincer1row + mainPincer2row) / 2;
var centerColumn = (mainPincer1col + mainPincer2col) / 2;
var secondPincer1col = centerColumn + 12 * Math.Round(Math.Sin(RotationSide * Math.PI / 180));
var secondPincer2col = centerColumn - 12 * Math.Round(Math.Sin(RotationSide * Math.PI / 180));
var secondPincer1row = centerRow + 12 * Math.Round(Math.Cos(RotationSide * Math.PI / 180));
var secondPincer2row = centerRow - 12 * Math.Round(Math.Cos(RotationSide * Math.PI / 180));
SidePincer1 = Tubes.Where(x => x.Row == secondPincer1row).Where(x => x.Column == secondPincer1col).Where(x => x.ColorProp != "Black").FirstOrDefault();
SidePincer2 = Tubes.Where(x => x.Row == secondPincer2row).Where(x => x.Column == secondPincer2col).Where(x => x.ColorProp != "Black").FirstOrDefault();
if (SidePincer1 == null || SidePincer2 == null)
{
secondPincer1col = centerColumn + 12 * Math.Round(Math.Cos(RotationSide * Math.PI / 180));
secondPincer2col = centerColumn - 12 * Math.Round(Math.Cos(RotationSide * Math.PI / 180));
secondPincer1row = centerRow + 12 * Math.Round(Math.Sin(RotationSide * Math.PI / 180));
secondPincer2row = centerRow - 12 * Math.Round(Math.Sin(RotationSide * Math.PI / 180));
SidePincer1 = Tubes.Where(x => x.Row == secondPincer1row).Where(x => x.Column == secondPincer1col).Where(x => x.ColorProp != "Black").FirstOrDefault();
SidePincer2 = Tubes.Where(x => x.Row == secondPincer2row).Where(x => x.Column == secondPincer2col).Where(x => x.ColorProp != "Black").FirstOrDefault();
if (SidePincer1 == null || SidePincer2 == null)
{
ResetCoordinates();
return;
}
RotationSide = (short)(RotationMain + 135);
}
}
private void ShiftSide(TubeM currentPos, List <int> coordinates)
{
//revert translation to zero
var row1 = SidePincer1.Row;
var col1 = SidePincer1.Column;
var row2 = SidePincer2.Row;
var col2 = SidePincer2.Column;
int i = TranslationMainUp;
while (i > 0)
{
var rowSide1 = SidePincer1.Row + Math.Round(Math.Cos(Math.PI * RotationMain / 180)) * i;
var rowSide2 = SidePincer2.Row + Math.Round(Math.Cos(Math.PI * RotationMain / 180)) * i;
if (Tubes.Where(x => x.Row == rowSide1).Where(y => y.Column == SidePincer1.Column).Where(x => x.ColorProp != "Black").FirstOrDefault() != null &&
Tubes.Where(x => x.Row == rowSide2).Where(y => y.Column == SidePincer2.Column).Where(x => x.ColorProp != "Black").FirstOrDefault() != null)
{
SidePincer1 = Tubes.Where(x => x.Row == rowSide1).Where(y => y.Column == SidePincer1.Column).FirstOrDefault();
SidePincer2 = Tubes.Where(x => x.Row == rowSide2).Where(y => y.Column == SidePincer2.Column).FirstOrDefault();
break;
}
i = i - 1;
}
TranslationMainUp = (short)(TranslationMainUp - i);
int j = TranslationMainRight;
while (j > 0)
{
var colSide1 = SidePincer1.Column + Math.Round(Math.Sin(Math.PI * RotationMain / 180)) * i;
var colSide2 = SidePincer2.Column + Math.Round(Math.Sin(Math.PI * RotationMain / 180)) * i;
if (Tubes.Where(x => x.Row == SidePincer1.Row).Where(y => y.Column == colSide1).Where(x => x.ColorProp != "Black").FirstOrDefault() != null &&
Tubes.Where(x => x.Row == SidePincer2.Row).Where(y => y.Column == colSide2).Where(x => x.ColorProp != "Black").FirstOrDefault() != null)
{
SidePincer1 = Tubes.Where(x => x.Row == SidePincer1.Row).Where(y => y.Column == colSide1).FirstOrDefault();
SidePincer2 = Tubes.Where(x => x.Row == SidePincer2.Row).Where(y => y.Column == colSide2).FirstOrDefault();
break;
}
j = j - 1;
}
TranslationMainRight = (short)(TranslationMainRight - j);
}
private TubeM TryToRotate(TubeM currentPos, int Angle)
{
if (RotationMain > 360)
{
RotationMain = (short)(RotationMain - 360);
}
if (RotationSide > 360)
{
RotationSide = (short)(RotationSide - 360);
}
if (RotationMain < 0)
{
RotationMain = (short)(RotationMain + 360);
}
if (RotationSide < 0)
{
RotationSide = (short)(RotationSide + 360);
}
if (RotationMax == 30)
{
Debug.WriteLine("You are stuck");
throw new Exception();
}
if (Angle < 0)
{
Angle = 360 + Angle;
}
switch (Angle)
{
case 90:
var a = RotationSide;
if (RotationMain > RotationSide)
{
a = (short)(360 + RotationSide);
}
if (a - RotationMain == 135)
{
var rowMain1 = MainPincer1.Row + 14 * Math.Round(Math.Cos(Math.PI * RotationSide / 180));
var columnMain1 = MainPincer1.Column + 14 * Math.Round(Math.Sin(Math.PI * RotationSide / 180));
var rowMain2 = MainPincer2.Row - 14 * Math.Round(Math.Cos(Math.PI * RotationSide / 180));
var columnMain2 = MainPincer2.Column - 14 * Math.Round(Math.Sin(Math.PI * RotationSide / 180));
if (Tubes.Where(x => x.Row == rowMain1).Where(y => y.Column == columnMain1).Where(x => x.ColorProp != "Black").FirstOrDefault() != null &&
Tubes.Where(x => x.Row == rowMain2).Where(y => y.Column == columnMain2).Where(x => x.ColorProp != "Black").FirstOrDefault() != null)
{
RotationMain += 90;
MainPincer1 = Tubes.Where(x => x.Row == rowMain1).Where(y => y.Column == columnMain1).FirstOrDefault();
MainPincer2 = Tubes.Where(x => x.Row == rowMain2).Where(y => y.Column == columnMain2).FirstOrDefault();
var currentPosrow = currentPos.Row + 16 * Math.Cos(Math.PI * RotationSide / 180);
var currentPoscol = currentPos.Column + 16 * Math.Sin(Math.PI * RotationSide / 180);
currentPos = Tubes.Where(x => x.Row == currentPosrow).Where(y => y.Column == currentPoscol).FirstOrDefault();
pathList.Add(currentPos);
}
else
{
currentPos = TryToRotate(currentPos, 270);
RotationMax += 1;
}
}
else
{
RotationMax += 1;
RotateSide(currentPos, 90);
currentPos = TryToRotate(currentPos, 90);
}
return(currentPos);
case 180:
currentPos = TryToRotate(currentPos, 90);
currentPos = TryToRotate(currentPos, 90);
return(currentPos);
case 270:
var ab = RotationSide;
if (RotationMain > RotationSide)
{
ab = (short)(360 + RotationSide);
}
if ((ab - RotationMain) == 45)
{
var rowMain1 = MainPincer1.Row - 14 * Math.Round(Math.Cos(Math.PI * RotationSide / 180));
var columnMain1 = MainPincer1.Column - 14 * Math.Round(Math.Sin(Math.PI * RotationSide / 180));
var rowMain2 = MainPincer2.Row + 14 * Math.Round(Math.Cos(Math.PI * RotationSide / 180));
var columnMain2 = MainPincer2.Column + 14 * Math.Round(Math.Sin(Math.PI * RotationSide / 180));
if (Tubes.Where(x => x.Row == rowMain1).Where(y => y.Column == columnMain1).Where(x => x.ColorProp != "Black").FirstOrDefault() != null &&
Tubes.Where(x => x.Row == rowMain2).Where(y => y.Column == columnMain2).Where(x => x.ColorProp != "Black").FirstOrDefault() != null)
{
RotationMain -= 90;
MainPincer1 = Tubes.Where(x => x.Row == rowMain1).Where(y => y.Column == columnMain1).FirstOrDefault();
MainPincer2 = Tubes.Where(x => x.Row == rowMain2).Where(y => y.Column == columnMain2).FirstOrDefault();
var currentPosrow = currentPos.Row - 16 * Math.Round(Math.Cos(Math.PI * RotationSide / 180));
var currentPoscol = currentPos.Column - 16 * Math.Round(Math.Sin(Math.PI * RotationSide / 180));
currentPos = Tubes.Where(x => x.Row == currentPosrow).Where(y => y.Column == currentPoscol).FirstOrDefault();
pathList.Add(currentPos);
}
else
{
currentPos = TryToRotate(currentPos, 90);
RotationMax += 1;
}
}
else
{
RotateSide(currentPos, 270);
}
return(currentPos);
default:
return(currentPos);
}
}