void rotate(vector<vector<int>>& matrix) {
N = matrix.size();
bool isEven = (matrix.size()%2==0);
int rowLimit = isEven?matrix.size()/2-1: matrix.size()/2;
int colLimit = isEven?matrix.size()/2-1: matrix.size()/2-1;
for( int i=0; i<=rowLimit; ++i)
{
for( int j=0; j<=colLimit; ++j)
{
rotateCell(matrix, i, j);
}
}
}
/*******************************************************************
* Function: char setGateways(void)
* Input Variables: void
* Output Return: void
* Overview: Interpolates the map using the current
* orientation and reading
********************************************************************/
void setGateways(void)
{
// This will be the gatway the robot will look for
unsigned char curCell = currentGateway;
// Get the start location of the robot
unsigned char curRow = currentCellWorld >> 2;
unsigned char curCol = currentCellWorld & 0b0011;
// Git the start orientation of the robot
unsigned char curOrient = currentOrientation;
// Rotate the cell with reference to the robot
curCell = rotateCell(curCell,curOrient,TO_MAP_ROTATE);
// Set the current cell
ROBOT_WORLD[curRow][curCol] = curCell;
}
/*******************************************************************
* Function: void checkFire(void)
* Input Variables: none
* Output Return: unsigned char
* Overview: Checks world for Fires
********************************************************************/
unsigned char checkFire(void)
{
// Acquire the current gateway
unsigned char curGate = currentGateway;
// Acquire the current cell
unsigned char curRow = (currentCellWorld>>2);
unsigned char curCol = (currentCellWorld&0b0011);
unsigned char curCell = ROBOT_WORLD[curRow][curCol];
// Rotate the current gateway wrt to current orientation
curGate = rotateCell (curGate, currentOrientation, TO_MAP_ROTATE);
// Return the boolean value of the equality
if (curGate != curCell){
switch(curGate^curCell){
case 0b0001:
currentFireCell = (curRow<<2)+(curCol-1);
break;
case 0b0010:
currentFireCell = ((curRow+1)<<2)+(curCol);
return SUCCESS;
break;
case 0b0100:
currentFireCell = (curRow<<2)+(curCol+1);
return SUCCESS;
break;
case 0b1000:
currentFireCell = ((curRow-1)<<2)+(curCol);
return SUCCESS;
break;
default:
return FAIL;
break;
}
return SUCCESS;
}
return FAIL;
// return 0;
}
bool SakuraMatrix::perform(const InvocationInfo &info)
{
bool commandProcessed = false;
bool moveFocus = false;
int direction = -1;
switch(info.commandID)
{
case generateCommandId:
generateBranch();
repaint();
commandProcessed = true;
break;
case shuffleCommandId:
shuffleMatrix();
commandProcessed = true;
break;
case peekCommandId:
if(!info.isKeyDown)
{
setPeekMode(false);
commandProcessed = true;
}
else
{
if(!isAlreadySolved())
{
setPeekMode(true);
commandProcessed = true;
}
}
break;
case settingsCommandId:
toggleSettings();
break;
case keyboardMoveLeftCommandId:
if(_keyboardSupport && !_solved)
{
moveFocus = true;
direction = int(Direction::Left);
}
break;
case keyboardMoveUpCommandId:
if(_keyboardSupport && !_solved)
{
moveFocus = true;
direction = int(Direction::Top);
}
break;
case keyboardMoveRightCommandId:
if(_keyboardSupport && !_solved)
{
moveFocus = true;
direction = int(Direction::Right);
}
break;
case keyboardMoveDownCommandId:
if(_keyboardSupport && !_solved)
{
moveFocus = true;
direction = int(Direction::Bottom);
}
break;
case rotateLeftCommandId:
if(_keyboardSupport && !_solved)
{
rotateCell(_matrix[_y_focus][_x_focus], true);
}
break;
case rotateRightCommandId:
if(_keyboardSupport && !_solved)
{
rotateCell(_matrix[_y_focus][_x_focus], false);
}
break;
}
if(moveFocus && direction >= 0)
{
_matrix[_y_focus][_x_focus]->setDrawFocus(false);
_x_focus += __delta[direction].first;
_y_focus += __delta[direction].second;
if(_x_focus < 0)
_x_focus = _numberOfCellsX - 1;
else
if(_x_focus >= _numberOfCellsX)
_x_focus = 0;
else
if(_y_focus < 0)
_y_focus = _numberOfCellsY - 1;
else
if(_y_focus >= _numberOfCellsY)
_y_focus = 0;
_matrix[_y_focus][_x_focus]->setDrawFocus(true);
commandProcessed = true;
}
return commandProcessed;
}
/*******************************************************************
* Function: char matchBranch(unsigned char *ptROBOT_WORLD, unsigned char row, unsigned char col)
* Input Variables: char
* Output Return: unsigned char *, unsigned char, unsigned char
* Overview: Check to see if the branch is valid
* given the map and starting seed
********************************************************************/
char matchBranch( unsigned char row, unsigned char col)
{
// Local variables for comparing branch and gateway
unsigned char branch, curMove, curOrnt, gateway, i;
unsigned char curCell = 0;
// Local variables for nested for loops
unsigned char curRow = row;
unsigned char curCol = col;
// Then check for a matching brache
for(i = 0; i < currentBranch; i++){
// Check to see if we are still inside the map
// If we went outside, then return failure
if((curRow>WORLD_ROW_SIZE)||(curCol>WORLD_COLUMN_SIZE)){
return FAIL;
}
// Get current branch
branch = localizeGateways[0][i];
// Get the current move
curMove = localizeGateways[1][i];
// Get the current orientation
curOrnt = localizeGateways[2][i];
// Rotate the branch to reflect the map
branch = rotateCell (branch, curOrnt, TO_MAP_ROTATE);
// Get gateway from map
// This uses pointers to get the element
// gateway = *(ptROBOT_WORLD+curRow*WORLD_ROW_SIZE+curCol);
gateway = ROBOT_WORLD[curRow][curCol];
// If the matching pattern is broken
// stop matching and return failure
if(branch != gateway){
return FAIL;
}
// Set the new cell of the next branch
curCell = (curRow << 2) + curCol;
// If this is the last branch
// dont move the cell
// so we are left with our locilized position
// if((i == (currentBranch-2))&&){
// Prep for the gateway by moving with the next branch
curCell = shiftMap(curCell, curMove, curOrnt);
// }
// Get the currrent cell of the branch
curRow = curCell >> 2;
curCol = curCell & 0b0011;
}
// If we make it through all the branches
// then return success
currentCellWorld = curCell;
return SUCCESS;
}
/*******************************************************************
* Function: char getGateways(void)
* Input Variables: void
* Output Return: void
* Overview: Interpolates the list of gateways in the path by
using the map and initial conditions
********************************************************************/
void getGateways(void)
{
// Get the start location of the robot
// unsigned char curRow = (currentCellWorld>>2) & 0b1100;
// unsigned char curCol = currentCellWorld & 0b0011;
unsigned char curRow = currentCellWorld >> 2;
unsigned char curCol = currentCellWorld & 0b0011;
// Git the start orientation of the robot
unsigned char curOrient = currentOrientation;
// This will be the gatway the robot will look for
unsigned char curCell;
// This will be the move the robot will preform
unsigned char curMove;
// This is the index of the move we are looking at
unsigned char j;
for (j = 0; j<=MAX_MOVE_SIZE; j++)
{ // Get the current move
curMove = moveCommands[j];
// Get the current cell
curCell = ROBOT_WORLD[curRow][curCol];
// Rotate the cell with reference to the robot
curCell = rotateCell(curCell,curOrient,1);
// Store the cell as a searchable gateway
moveGateways[j] = curCell;
// If we are moving forward
// move to the next cell with respect to our orientation
if (curMove == MOVE_FORWARD){
switch(curOrient){
case NORTH:
curRow -= 1;
break;
case EAST:
curCol += 1;
break;
case SOUTH:
curRow += 1;
break;
case WEST:
curCol -= 1;
break;
default:
break;
}
}
// If we are turning right
// then rotate our map orientation appropriately
else if (curMove == MOVE_RIGHT){
curOrient++;
curOrient = curOrient&0b11;
// LCD_clear();
// LCD_printf("Num:\n%i\curOrient:\n"BYTETOBINARYPATTERN,j,BYTETOBINARY(curOrient));
// TMRSRVC_delay(500);//wait 1/2 seconds
}
// If we are turning left
// then rotate our map orientation appropriately
else if (curMove == MOVE_LEFT){
// if(curOrient == 0){
// curOrient = 0b0011;
// }
curOrient--;
curOrient = curOrient&0b11;
}
}
for (j = 0; j<=MAX_MOVE_SIZE; j++)
{
curCell = moveGateways[j];
LCD_clear();
LCD_printf("Num:\n%i\nCurCell:\n"BYTETOBINARYPATTERN,j,BYTETOBINARY(curCell));
TMRSRVC_delay(1000);//wait 1 second
}
}
