void chk_walls(unsigned int c_pos, unsigned char m_dir)
{
unsigned int i;
unsigned char cnt_L0,cnt_R0,cnt_L45,cnt_R45;
// Initiate.
cnt_L0 = cnt_R0 = cnt_L45 = cnt_R45 = 0;
// Count.
for(i=0;i<SS_HISTORY_SIZE;i++) {
if (ss_history[SS_L45][i] > threshold_L45 ) cnt_L45++;
if (ss_history[SS_R45][i] > threshold_R45 ) cnt_R45++;
if (ss_history[SS_L0 ][i] > threshold_L0 ) cnt_L0++;
if (ss_history[SS_R0 ][i] > threshold_R0 ) cnt_R0++;
}
// Decide the existance of walls.
if ( cnt_L45 > 2 ) maze[c_pos] |= wall_bit(get_absolute_dir(L,m_dir));
if ( cnt_R45 > 2 ) maze[c_pos] |= wall_bit(get_absolute_dir(R,m_dir));
//if ( cnt_L0 > 2 && cnt_R0 > 2 ) maze[c_pos] |= wall_bit(get_absolute_dir(F,m_dir));
if ( cnt_L0 > 1 && cnt_R0 > 1 ) maze[c_pos] |= wall_bit(get_absolute_dir(F,m_dir));
// Process walls of blocks around.
unsigned char x,y;
x = pos_x( c_pos ); y = pos_y( c_pos );
if ( (maze[c_pos] & NORTH) && y < 15 ) maze[ c_pos + dxy[N] ] |= SOUTH;
if ( (maze[c_pos] & EAST ) && x < 15 ) maze[ c_pos + dxy[E] ] |= WEST;
if ( (maze[c_pos] & SOUTH) && y > 0 ) maze[ c_pos + dxy[S] ] |= NORTH;
if ( (maze[c_pos] & WEST ) && x > 0 ) maze[ c_pos + dxy[W] ] |= EAST;
// Check the block read.
maze_info[ c_pos ] |= VALID_BLK;
}
void run_algol()
{
// ALGOL /////////////////////////////////////////////////////////
if ((maze_info[mouse_pos] & VALID_BLK) == 0) {
chk_walls(mouse_pos,mouse_dir);
ss_pulse_slow();
#ifdef DEBUG_ALGOL
unsigned int steps_checker = steps_L;
#endif
unsigned int v_dir;
if ( run_fptr < run_rptr ) {
if ( run_queue[ run_fptr ] == run_sFX ) v_dir = get_absolute_dir(F,mouse_dir);
else if ( run_queue[ run_fptr ] == run_sRX ) v_dir = get_absolute_dir(R,mouse_dir);
else if ( run_queue[ run_fptr ] == run_sLX ) v_dir = get_absolute_dir(L,mouse_dir);
else goto RUN_ALGOL;
// Conditional running algorithm
if ( (maze[mouse_pos] & wall_bit(v_dir)) == 0 ) {
eyes_on();
goto END_ALGOL;
}
}
RUN_ALGOL:
//ss_pulse_slow();
eyes_off();
disable_adj();
scan_maze(goal_pos);
make_path();
path_cvrt();
refresh_ss_history();
enable_adj();
//ss_pulse_fast();
END_ALGOL:
ss_pulse_fast();
#ifdef DEBUG_ALGOL
led_num((steps_L - steps_checker)/10);
//led_num((steps_L - steps_checker));
#endif
}
else eyes_on();
//////////////////////////////////////////////////////////////////
}
static int gen_bin_tree_tail(unsigned char *maze, unsigned char *map,
struct s_link **head)
{
struct btree_node *bt_new_node, *bt_node;
struct s_link *sl_node;
struct s_link *sl_new_node, *tail_new_list = NULL;
unsigned char i, index, abs_dir;
struct btree_node bt_node_backup;
char dir, is_goal = 0, found_node;
#ifdef CONFIG_PATH_LIMIT
int path_limit = 0;
#endif
if (!maze || !map || !head || !*head) {
print_error("NULL pointer error! %X %X %X %X\n",
(unsigned int)maze, (unsigned int)map,
(unsigned int)head, (unsigned int)*head);
}
print_dbg(DEBUG_S_LINK, "Check input linked list: %08X\n",
(unsigned int)*head);
debug_sl_node(*head);
print_dbg(DEBUG_S_LINK, "Find next lower contour level\n");
/* Find next lower contour cube arond current location */
for (sl_node = *head; sl_node; sl_node = sl_node->node) {
index = sl_node->bt_node->pos;
abs_dir = sl_node->bt_node->abs_dir;
bt_node = sl_node->bt_node;
/* when it's set, found turn. on the unkonwn blocks,
* there may be too many pathes to calculate.
**/
found_node = 0;
bt_node_backup.time = 0; /* use to check FD detection */
for (i = NI; i <= WI; i++) {
if (!(maze[index] & wall_bit(i)) &&
(map[index] == map[index + maze_dxy[i]] + 1)) {
dir = relative_direction(abs_dir, i);
/* if block is unknown, then take turn prior
* to straight path. It's to save memory
* and calculation time.
*/
if (dir == FD && ((maze[index]&0xF0) != 0xF0)) {
bt_node_backup.pos = index+maze_dxy[i];
bt_node_backup.dir = dir;
bt_node_backup.abs_dir = i;
/* to mark block has meaningful data */
bt_node_backup.time = 1;
continue;
}
found_node = 1;
/* create new bt_node and save next mouse index
* and absolute direction of mouse at next block
*/
bt_new_node =
bt_node_alloc(index+maze_dxy[i], i);
/* Next bt_node->dir is to save how mouse made
* a turn to come the block.
*/
bt_new_node->dir = dir;
/* there can be 3rd child, only take 2 child at
* a time
*/
if (!add_bt_node(bt_node, bt_new_node)) {
sl_new_node = s_link_alloc(bt_new_node);
add_sl_node(&tail_new_list, sl_new_node);
#ifdef CONFIG_PATH_LIMIT
path_limit++;
#endif
} else {
bt_node_free(bt_new_node);
}
/*
* debug_sl_node(tail_new_list);
*/
}
}
/* if forward is the only one in known block,
* then use it
*/
if (!found_node && bt_node_backup.time) {
bt_new_node =
bt_node_alloc(bt_node_backup.pos,
bt_node_backup.abs_dir);
bt_new_node->dir = bt_node_backup.dir;
add_bt_node(bt_node, bt_new_node);
sl_new_node = s_link_alloc(bt_new_node);
add_sl_node(&tail_new_list, sl_new_node);
#ifdef CONFIG_PATH_LIMIT
path_limit++;
#endif
}
if (map[index] == 1 && !is_goal)
is_goal = 1;
/* Remove bt_node if there is no child, it'll search
* parent node and remove the node if it had no child
*/
if (!is_goal)
clean_bin_tree(bt_node, 80);
#ifdef CONFIG_PATH_LIMIT
/* 10 pathes : max 20KB
* 20 pathes : max 30 KB
* 40 pathes : max 57 KB
*/
if (path_limit >= 20)
break;
#endif
}
if (tail_new_list)
debug_sl_node(tail_new_list);
#ifdef DEBUG
if (is_goal) {
i = 0;
print_dbg(DEBUG_BINTREE,
"Found maze goal!!!\n");
print_dbg(DEBUG_BINTREE,
"tail_new_list for tail: 0x%08X\n",
(unsigned int)tail_new_list);
print_dbg(DEBUG_BINTREE,
"*head pointer: %08X\n",
(unsigned int)*head);
if (tail_new_list)
print_error("tail_new_list must be NULL!\n");
for (sl_node = *head; sl_node; sl_node = sl_node->node) {
index = sl_node->bt_node->pos;
bt_node = sl_node->bt_node;
i++;
}
print_dbg(DEBUG_BINTREE, "Total path: %d\n", i);
}
#endif
/* critical error to find next contour maps */
if (!is_goal && tail_new_list == NULL) {
printf("contour map\n");
print_full(map);
printf("maze map\n");
print_map(maze);
for (sl_node = *head; sl_node; sl_node = sl_node->node) {
index = sl_node->bt_node->pos;
abs_dir = sl_node->bt_node->abs_dir;
bt_node = sl_node->bt_node;
printf("mouse:(%d,%d),abs_dir:%d\n", pos_x(index),
pos_y(index), abs_dir);
}
}
/* free s_link nodes */
if (!is_goal) {
sl_node_free(*head);
*head = tail_new_list;
}
return is_goal;
}
/* function name: draw_contour
* Input Parameter
* maze : maze array wich has maze information
* map : array to draw contour map
* type : maze type to run mouse
* pos : current mouse location in the maze
*/
void draw_contour(unsigned char *maze, unsigned char *map,
unsigned int type, unsigned char pos)
{
int i, contour_lvl = 1;
int index;
unsigned int item;
char found_mouse = 0;
struct circular_buffer *cb, contour_buffer;
cb = &contour_buffer;
circular_buffer_init(cb, contour_cb_buffer,
CONTOUR_CB_BUFFER_MAX);
/* Uninitialized contour map */
memset(map, 0, MAZEMAX);
/* Seed value 1 as a goal */
switch (type) {
case TO_GOAL_4X4:
map[get_index(3, 3)] = contour_lvl;
circular_buffer_write(cb, gen_contour_pos(contour_lvl, 0x33));
break;
case TO_GOAL_8X8:
map[get_index(7, 7)] = contour_lvl;
circular_buffer_write(cb, gen_contour_pos(contour_lvl, 0x77));
break;
case TO_GOAL_16X16:
map[get_index(7, 7)] = contour_lvl;
map[get_index(8, 7)] = contour_lvl;
map[get_index(7, 8)] = contour_lvl;
map[get_index(8, 8)] = contour_lvl;
/* Add list of same level contour value */
circular_buffer_write(cb, gen_contour_pos(contour_lvl, 0x77));
circular_buffer_write(cb, gen_contour_pos(contour_lvl, 0x78));
circular_buffer_write(cb, gen_contour_pos(contour_lvl, 0x87));
circular_buffer_write(cb, gen_contour_pos(contour_lvl, 0x88));
break;
case TO_START_4X4:
case TO_START_8X8:
case TO_START_16X16:
map[get_index(0, 0)] = contour_lvl;
circular_buffer_write(cb, gen_contour_pos(contour_lvl, 0x00));
break;
default:
if (type < MAZEMAX) {
map[type] = contour_lvl;
circular_buffer_write(cb, gen_contour_pos(contour_lvl, type));
} else {
print_exit("Invalid target goal index!\n");
}
break;
}
/* Get one contour number from circular buffer.
* Put next higher value in next block if there is
* no wall to there and save it inti circular buffer.
* If contour map reaches to current mouse or
* circular buffer is empty then it's done.
*/
while (!circular_buffer_empty(cb) && !found_mouse) {
circular_buffer_read(cb, &item);
index = get_contour_index(item);
contour_lvl = get_contour_lvl(item) + 1;
/* Calculate contour lvl around current cube */
for (i = NI; i <= WI; i++) {
if (!(maze[index] & wall_bit(i)) &&
(map[index + maze_dxy[i]] == 0)) {
map[index + maze_dxy[i]] = contour_lvl;
circular_buffer_write(cb,
gen_contour_pos(contour_lvl,
index + maze_dxy[i]));
if (index + maze_dxy[i] == pos)
found_mouse = 1;
}
}
#ifdef DEBUG
if (debug_flag & DEBUG_CONTOUR) {
print_map(map);
usleep(20000);
}
#endif
}
if (!found_mouse) {
/* Mouse alorighm should never hit this location */
print_map(map);
print_exit("%s couldn't find mouse location\n",
__func__);
}
}
