void getPathComplicated(signed char x1, signed char y1, signed char xDestination, signed char yDestination)
{
if((x1==xDestination)&&(y1==yDestination))
{
stopBoth();
ecrobot_sound_tone(100,500,100);
systick_wait_ms(2000);
endAll();
addToPath(none);
return;
}
//display_currentNode();
//systick_wait_ms(1000);
/*
display_clear(1);
display_goto_xy(0,0);
display_string("gPC called");
display_goto_xy(0,1);
display_int((int)x1,1);
display_goto_xy(3,1);
display_int((int)y1,1);
display_goto_xy(0,2);
display_int((int)xDestination,1);
display_goto_xy(3,2);
display_int((int)yDestination,1);
display_update();
*/
//systick_wait_ms(1000);
signed char xCurrent;
signed char yCurrent;
signed char startReached = 0;
signed char endReached = 0;
signed char xDistance= xDestination - x1;
signed char yDistance= yDestination - y1;
signed char yD2=0;
signed char xD2=0;
if(yDistance<0){yD2=-yDistance;}else{yD2=yDistance;}
if(xDistance<0){xD2=-xDistance;}else{xD2=xDistance;}
char maxIterations = (xD2+yD2)/2+40;//40 is safety padding
char cntLoops;
if(maxIterations<0)
maxIterations=-maxIterations;
//************************************** MAIN BODY ******
int x,y;
for(x=0;x<=12;x++)
{
for(y=0;y<=12;y++)
{
mazeStart[x][y]=0;
mazeEnd[x][y]=0;
}
}
mazeStart[x1][y1]=50;
int colX=0;
int rowY=0;
mazeEnd[xDestination][yDestination]=50;
signed char l,r,t,d;
for(cntLoops=0; cntLoops<=maxIterations*2; cntLoops++)
{
if((startReached==1) && (endReached==1))
{
cntLoops = (maxIterations*2)+1;
}
//***************************set mazeStart's values checks now if values overflow
for(colX=0;colX<=12;colX++)
{
for(rowY=12;rowY>=0;rowY--)
{
if(mazeStart[xDestination][yDestination]!=0){endReached = 1;break;};
//check if the field has the path to the adjecant field
l=r=t=d=0;
if((hasDirection(left,colX,rowY)==1) && (mazeStart[colX-1][rowY] != 0))
{
if(colX>0)
l=mazeStart[colX-1][rowY]-1;
}
if((hasDirection(right,colX,rowY)==1) && (mazeStart[colX+1][rowY] != 0))
{
if(colX<12)
r=mazeStart[colX+1][rowY]-1;
}
if((hasDirection(top,colX,rowY)==1) && (mazeStart[colX][rowY+1] != 0))
{
if(rowY<12)
t=mazeStart[colX][rowY+1]-1;
}
if((hasDirection(down,colX,rowY)==1) && (mazeStart[colX][rowY-1] != 0))
{
if(rowY>0)
d=mazeStart[colX][rowY-1]-1;
}
mazeStart[colX][rowY]=getLargestValue(l,t,r,d,mazeStart[colX][rowY]);
}
}
//*************************** set mazeEnd's values
for(colX=0;colX<=12;colX++)
{
for(rowY=12;rowY>=0;rowY--)
{
if((mazeEnd[x1][y1]!=0)){startReached = 1;break;};
l=r=t=d=0;
if((hasDirection(left,colX,rowY)==1) && (mazeEnd[colX-1][rowY] != 0))
{
if(colX>0)
l=mazeEnd[colX-1][rowY]-1;
}
if((hasDirection(right,colX,rowY)==1) && (mazeEnd[colX+1][rowY] != 0))
{
if(colX<12)
r=mazeEnd[colX+1][rowY]-1;
}
if((hasDirection(top,colX,rowY)==1) && (mazeEnd[colX][rowY+1] != 0))
{
if(rowY<12)
t=mazeEnd[colX][rowY+1]-1;
}
if((hasDirection(down,colX,rowY)==1) && (mazeEnd[colX][rowY-1] != 0))
{
if(rowY>0)
d=mazeEnd[colX][rowY-1]-1;
}
mazeEnd[colX][rowY]=getLargestValue(l,t,r,d,mazeEnd[colX][rowY]);
}
}
}
//prnt values both matrixes now added into mazeStart
int ty, tx;
tx=ty=0;
for(ty=12; ty>=0; ty--)
{
for(tx=0; tx<=12; tx++)
{
mazeStart[tx][ty]=mazeStart[tx][ty]+mazeEnd[tx][ty];
mazeEnd[tx][ty]=0;
}
}
//now add way of highest numbers to path so it can be printed or driven
xCurrent = x1;
yCurrent = y1;
signed char xDisLeft = xDistance;
signed char yDisLeft = yDistance;
signed char pathValue = mazeStart[x1][y1];
mazeEnd[xCurrent][yCurrent]=1; //repurposing mazeE to store already visited Nodes
if(endReached==1)
{
while((xCurrent!=xDestination) || (yCurrent != yDestination))
{
if((xDisLeft>0)||(((mazeEnd[xCurrent-1][yCurrent]==1)||(mazeStart[xCurrent-1][yCurrent]!=pathValue||(hasDirection(left,xCurrent,yCurrent)==0)))&&((mazeEnd[xCurrent][yCurrent+1]==1)||(mazeStart[xCurrent][yCurrent+1]!=pathValue)||(hasDirection(top,xCurrent,yCurrent)==0))&&((mazeEnd[xCurrent][yCurrent-1]==1)||(mazeStart[xCurrent][yCurrent-1]!=pathValue)||(hasDirection(down,xCurrent,yCurrent)==0))))
{
if(hasDirection(right,xCurrent,yCurrent) && (mazeStart[xCurrent+1][yCurrent]==pathValue) && (mazeEnd[xCurrent+1][yCurrent]==0))
{
addToPath(right);
mazeEnd[xCurrent][yCurrent]=1;
xCurrent++;
xDisLeft--;
}
}
if((xCurrent==xDestination) && (yCurrent == yDestination))
break;
if(xDisLeft<0||(((mazeEnd[xCurrent+1][yCurrent]==1)||(mazeStart[xCurrent+1][yCurrent]!=pathValue)||(hasDirection(right,xCurrent,yCurrent)==0))&&((mazeEnd[xCurrent][yCurrent+1]==1)||(mazeStart[xCurrent][yCurrent+1]!=pathValue)||(hasDirection(top,xCurrent,yCurrent)==0))&&((mazeEnd[xCurrent][yCurrent-1]==1)||(mazeStart[xCurrent][yCurrent-1]!=pathValue)||(hasDirection(down,xCurrent,yCurrent)==0))))
{
if(hasDirection(left,xCurrent,yCurrent) && (mazeStart[xCurrent-1][yCurrent]==pathValue) && (mazeEnd[xCurrent-1][yCurrent]==0))
{
addToPath(left);
mazeEnd[xCurrent][yCurrent]=1;
xCurrent--;
xDisLeft++;
}
}
if((xCurrent==xDestination) && (yCurrent == yDestination))
break;
if(yDisLeft>0||(((mazeEnd[xCurrent+1][yCurrent]==1)||(mazeStart[xCurrent+1][yCurrent]!=pathValue)||(hasDirection(right,xCurrent,yCurrent)==0))&&((mazeEnd[xCurrent][yCurrent-1]==1)||(mazeStart[xCurrent][yCurrent-1]!=pathValue)||(hasDirection(down,xCurrent,yCurrent)==0))&&((mazeEnd[xCurrent-1][yCurrent]==1)||(mazeStart[xCurrent-1][yCurrent]!=pathValue)||(hasDirection(left,xCurrent,yCurrent)==0))))
{
if(hasDirection(top,xCurrent,yCurrent) && (mazeStart[xCurrent][yCurrent+1]==pathValue) && (mazeEnd[xCurrent][yCurrent+1]==0))
{
addToPath(top);
mazeEnd[xCurrent][yCurrent]=1;
yCurrent++;
yDisLeft--;
}
}
if((xCurrent==xDestination) && (yCurrent == yDestination))
break;
if(yDisLeft<0||(((mazeEnd[xCurrent+1][yCurrent]==1)||(mazeStart[xCurrent+1][yCurrent]!=pathValue)||(hasDirection(right,xCurrent,yCurrent)==0))&&((mazeEnd[xCurrent][yCurrent+1]==1)||(mazeStart[xCurrent][yCurrent+1]!=pathValue)||(hasDirection(top,xCurrent,yCurrent)==0))&&((mazeEnd[xCurrent-1][yCurrent]==1)||(mazeStart[xCurrent-1][yCurrent]!=pathValue)||(hasDirection(left,xCurrent,yCurrent)==0))))
{
if(hasDirection(down,xCurrent,yCurrent) && (mazeStart[xCurrent][yCurrent-1]==pathValue) && (mazeEnd[xCurrent][yCurrent-1]==0))
{
addToPath(down);
mazeEnd[xCurrent][yCurrent]=1;
yCurrent--;
yDisLeft++;
}
}
if((xCurrent==xDestination) && (yCurrent == yDestination))
break;
}
}
/*
display_goto_xy(0,3);
display_string("ret:");
display_goto_xy(4,3);
display_int(pathStorage[1],1);
display_update();*/
//systick_wait_ms(2000);
}
/*
* Stops the robot, whatever direction it is moving.
*/
void halt(){
stopBoth();
stopBothBackward();
}
