void __ts(uint32_t *ip, uint32_t *cp, term_t *sp, term_t *sbot)
{
//term_t *sbot = proc_stack_bottom(proc);
uint32_t *fi = backstep_to_func_info(ip);
if (fi == 0)
{
printk("No current function\n");
return;
}
else
printk("* %pt:%pt/%d +%ld\n", T(fi[1]), T(fi[2]), fi[3], ip-fi);
if (cp == 0) // after allocate, before call/apply
{
cp = demasquerade_pointer(sp[0]);
while (++sp < sbot)
if (is_boxed(*sp) && is_cp(peel_boxed(*sp)))
break;
}
do {
if (cp[0] == shrink_ptr(int_code_end_label))
break;
uint32_t *fi = backstep_to_func_info(cp);
printk(" %pt:%pt/%d\n", T(fi[1]), T(fi[2]), fi[3]);
cp = demasquerade_pointer(sp[0]);
while (++sp < sbot)
if (is_boxed(*sp) && is_cp(peel_boxed(*sp)))
break;
} while (sp < sbot);
}
void update_map() {
static U8 ready=FALSE;
static int last_pos_x = 0;
static int last_pos_y = 0;
//Stores the average value of walls
static int count_front_walls = 0;
static int count_left_walls = 0;
static int count_right_walls = 0;
// If not ready (not enough inside the cell), return
if(!ready) {
if(is_inside_square(get_realX(),get_realY(),MAPPING_RES)) ready=TRUE;
return;
}
int pos_x = get_x();
int pos_y = get_y();
// If out of the map, return
if (is_out_of_map(pos_x,pos_y)) return;
U8 data;
// If just enter in a new cell
if ( ( last_pos_y != pos_y ) || ( last_pos_x != pos_x ) ) {
count_front_walls = 0;
count_left_walls = 0;
count_right_walls = 0;
ready=FALSE;
if(pos_x<__min_x) __min_x=pos_x;
else if(pos_x>__max_x) __max_x=pos_x;
if(pos_y<__min_y) __min_y=pos_y;
else if(pos_y>__max_y) __max_y=pos_y;
int direction = direction_of_next_cell(pos_x,pos_y,last_pos_x,last_pos_y);
last_pos_x = pos_x;
last_pos_y = pos_y;
coord_to_table_index(&pos_x,&pos_y);
data = _map[pos_x][pos_y];
if(direction != NO_CARD) {
set_wall_state(&data, direction, NO_WALL);
_map[pos_x][pos_y]=data;
}
return;
}
else {
coord_to_table_index(&pos_x,&pos_y);
data = _map[pos_x][pos_y];
}
int cardinal_point = get_cardinal_point();
// Do measurements only if in a good direction
if(is_cp(cardinal_point)) {
int left_wall = detect_wall(get_distanceL());
int right_wall = detect_wall(get_distanceR());
int front_wall = detect_wall(get_distanceF());
//Add plus one for is wall and substract one for no wall
count_front_walls = front_wall ? count_front_walls + 1 : count_front_walls - 1;
count_left_walls = left_wall ? count_left_walls + 1 : count_left_walls - 1;
count_right_walls = right_wall ? count_right_walls + 1 : count_right_walls - 1;
if(count_front_walls>0) set_wall_state(&data, cardinal_point, IS_WALL);
else set_wall_state(&data, cardinal_point, NO_WALL);
if(count_right_walls>0) set_wall_state(&data, next_cp(cardinal_point), IS_WALL);
else set_wall_state(&data, next_cp(cardinal_point), NO_WALL);
if(count_left_walls>0) set_wall_state(&data, previous_cp(cardinal_point), IS_WALL);
else set_wall_state(&data, previous_cp(cardinal_point), NO_WALL);
}
_map[pos_x][pos_y]=data;
}
