void handleNodeStatus(const ReceivedDataStructure<protocol::NodeStatus>& msg)
{
Entry new_entry;
new_entry.time_since_last_update_ms100 = 0;
new_entry.status.health = msg.health & ((1 << protocol::NodeStatus::FieldTypes::health::BitLen) - 1);
new_entry.status.mode = msg.mode & ((1 << protocol::NodeStatus::FieldTypes::mode::BitLen) - 1);
new_entry.status.sub_mode = msg.sub_mode & ((1 << protocol::NodeStatus::FieldTypes::sub_mode::BitLen) - 1);
changeNodeStatus(msg.getSrcNodeID(), new_entry);
handleNodeStatusMessage(msg);
}
void handleTimerEvent(const TimerEvent&)
{
const int OfflineTimeoutMs100 = protocol::NodeStatus::OFFLINE_TIMEOUT_MS / 100;
for (uint8_t i = 1; i <= NodeID::Max; i++)
{
Entry& entry = getEntry(i);
if (entry.time_since_last_update_ms100 >= 0 &&
entry.status.mode != protocol::NodeStatus::MODE_OFFLINE)
{
entry.time_since_last_update_ms100 =
int8_t(entry.time_since_last_update_ms100 + int8_t(TimerPeriodMs100));
if (entry.time_since_last_update_ms100 > OfflineTimeoutMs100)
{
Entry new_entry_value = entry;
new_entry_value.time_since_last_update_ms100 = OfflineTimeoutMs100;
new_entry_value.status.mode = protocol::NodeStatus::MODE_OFFLINE;
changeNodeStatus(i, new_entry_value);
}
}
}
}
void shortestPath(struct maze* maze, int nodeStart, int nodeExit)
{
int empty = 0;
int rows = maze->numrows;
int cols = maze->numcols;
int maxsize = rows * cols;
int Queue[maxsize];
int Origin[maxsize];
int i;
for(i=0; i<maxsize;i++)
{
Queue[i] = 0;
Origin[i]=0;
}
int front = 0;
int rear= 0;
// create array to hold the status of each grid point we have checked
int** mazeStatus = make2DIntArray(rows, cols);
Queue[rear] = nodeStart;
Origin[rear] = -1;
rear++;
int current, left, right, top, down;
while(front != rear ) // while the queue is not empty
{
if (current == 157)
left = current;
if (Queue[front] == nodeExit)
break; // maze is solved
current = Queue[front];
left = current - 1;
if (left >=0 && left/cols==current/cols) //if left node exists
{
if (getNodeContent(maze, left) == MAZE_PATH )
{
if (getNodeStatusContent(mazeStatus, left, cols) == READY)
{
Queue[rear] = left;
Origin[rear] = current;
changeNodeStatus(mazeStatus, left, cols, WAITING);
rear++;
}
}
}
right = current + 1;
if (right < maxsize && (right / cols) == (current / cols) )
{
if (getNodeContent(maze, right) == MAZE_PATH)
{
if (getNodeStatusContent(mazeStatus, right, cols) == READY)
{
Queue[rear] = right;
Origin[rear] = current;
changeNodeStatus(mazeStatus, right, cols, WAITING);
rear++;
}
}
}
top = current - cols;
if (top >= 0 && top < maxsize)
{
if (getNodeContent(maze, top) == MAZE_PATH)
{
if (getNodeStatusContent(mazeStatus, top, cols) == READY)
{
Queue[rear] = top;
Origin[rear] = current;
changeNodeStatus(mazeStatus, top, cols, WAITING);
rear++;
}
}
}
down = current + cols;
if (down < maxsize && down > 0 )
{
if (getNodeContent(maze, down) == MAZE_PATH)
{
if (getNodeStatusContent(mazeStatus, down, cols) == READY)
{
Queue[rear] = down;
Origin[rear] = current;
changeNodeStatus(mazeStatus, down, cols, WAITING);
rear++;
}
}
}
changeNodeStatus(mazeStatus, current, cols, PROCESSED);
front++;
}
// Update the maze with the path
current = nodeExit;
changeNodeContent(maze, nodeExit, MAZE_TRAIL);
for(i=front; i>=0; i--)
{
if (Queue[i]==current)
{
current=Origin[i];
if (current==-1) // maze is solved
return;
changeNodeContent(maze, current, MAZE_TRAIL);
}
}
return;
}
