/**
* @brief Handles the case where there's redstone dust beside us
* @param component The component beside us
* @param direction Direction to us
*/
void Redstone::SolidBlock::_onRedstoneDustBeside(
const Redstone::Component * component,
const Redstone::Map::Direction & direction)
{
auto dust = dynamic_cast<const RedstoneDust *>(component);
if (dust->hasDirection(direction))
this->_powerLevel = std::max(this->_powerLevel, dust->getLevel());
}
signed char hasDirectionRelative(signed char direction)
{
return hasDirection(shiftBitsForDriving(direction,directionOffset), currentPosition[0],currentPosition[1]);
}
void getNextUnvisited()
{
checkIfAllVisited();
signed char x1 = currentPosition[0];
signed char y1 = currentPosition[1];
signed char cnt =0;
signed char colX=0;
signed char compare;
signed char rowY=0;
signed char foundOne=0;
//ecrobot_sound_tone(900,90,100);
if((allVisited)&&(getTokensFound()!=3))
{
addToPath(none);
return;
}
if(getTokensFound()==3)
{
getPathComplicated(x1,y1,6,6);
return;
}
signed char l,t,r,d;
for(colX=0; colX<=12;colX++){
for(rowY=0; rowY<=12;rowY++){
mazeVisited[colX][rowY]=visited(colX,rowY);
mazeDistance[colX][rowY]=0;
}
}
//now start calculatin'
mazeDistance[x1][y1]=50;
for(cnt=0; cnt<=35 ; cnt++)//nah, don't calculate how much: just do it ;)
{
for(colX=0;colX<=12;colX++)
{
for(rowY=12;rowY>=0;rowY--)
{
//if((mazeDistance[x1][y1]!=0)){foundOne !=0 ;break;};
l=r=t=d=0;
if((hasDirection(left,colX,rowY)==1) && (mazeDistance[colX-1][rowY] != 0))
{
if(colX>0)
l=mazeDistance[colX-1][rowY]-1;
}
if((hasDirection(right,colX,rowY)==1) && (mazeDistance[colX+1][rowY] != 0))
{
if(colX<12)
r=mazeDistance[colX+1][rowY]-1;
}
if((hasDirection(top,colX,rowY)==1) && (mazeDistance[colX][rowY+1] != 0))
{
if(rowY<12)
t=mazeDistance[colX][rowY+1]-1;
}
if((hasDirection(down,colX,rowY)==1) && (mazeDistance[colX][rowY-1] != 0))
{
if(rowY>0)
d=mazeDistance[colX][rowY-1]-1;
}
mazeDistance[colX][rowY]=getLargestValue(l,t,r,d,mazeDistance[colX][rowY]);
}
}
}
for(colX=0; colX<=12;colX++){
for(rowY=0; rowY<=12;rowY++){
mazeDistance[colX][rowY]=50-mazeDistance[colX][rowY];
}
}
for(compare=0; compare<=35;compare++)
{
if(foundOne){return;};
for(rowY=12;rowY>=0;rowY--)
{
if(foundOne){return;};
for(colX=0;colX<=12;colX++)
{
if((mazeVisited[colX][rowY]==0)&&(mazeDistance[colX][rowY]==compare))
{
getPathComplicated(x1,y1,colX,rowY);
foundOne =1;
return;
}
}
}
}
//alle besucht
ecrobot_sound_tone(400,250,100);
systick_wait_ms(250);
ecrobot_sound_tone(900,250,100);
systick_wait_ms(250);
if(!allVisited)
getPathComplicated(x1,y1,6,6);
setAllVisited(1);
}
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);
}
