File content as of revision 25:7155cb993870:

#include "maze_solver.h"
#include "drivecontrol.h"
DriveControl * driver = new DriveControl ();

//==============================================================

Maze:: Maze(){
    int goal1 = MAZE_SIZE / 2;
    int goal2 = (MAZE_SIZE - 1) / 2;
    for (int i = 0; i < MAZE_SIZE; i++) {
        for (int j = 0; j < MAZE_SIZE; j++) {
            int min = min4(manhattan_dist(i, goal1, j, goal1),
                           manhattan_dist(i, goal1, j, goal2),
                           manhattan_dist(i, goal2, j, goal1),
                           manhattan_dist(i, goal2, j, goal2));
            maze[i][j] = new Cell(i, j, min);
            if (i == MAZE_SIZE - 1) {
                maze[i][j]->top_wall = true;
            }
            if (j == MAZE_SIZE - 1) {
                maze[i][j]->right_wall = true;
            }
        }
    }

}

int Maze:: manhattan_dist(int x1, int x2, int y1, int y2) {
    return abs(x1 - x2) + abs(y1 - y2);
}


// Function that takes the minimum of the four given distances

int Maze:: min4(int a, int b, int c, int d) {
    int min;
    (a < b) ? min = a : min = b;
    if (c < min) min = c;
    if (d < min) min = d;
    return min;
}

bool Maze:: is_center(Cell *cell) {
    int x = cell->x;
    int y = cell->y;
    int goal1 = MAZE_SIZE / 2;
    int goal2 = (MAZE_SIZE - 1) / 2;
    if (manhattan_dist(y, goal1, x, goal1) == 0 ||
        manhattan_dist(y, goal1, x, goal2) == 0 ||
        manhattan_dist(y, goal2, x, goal1) == 0 ||
        manhattan_dist(y, goal2, x, goal2) == 0) {
        return true;
    }
    return false;
}

bool Maze:: is_center(int x, int y) {
    int goal1 = MAZE_SIZE / 2;
    int goal2 = (MAZE_SIZE - 1) / 2;
    if (manhattan_dist(y, goal1, x, goal1) == 0 ||
        manhattan_dist(y, goal1, x, goal2) == 0 ||
        manhattan_dist(y, goal2, x, goal1) == 0 ||
        manhattan_dist(y, goal2, x, goal2) == 0) {
        return true;
    }
    return false;
}

//============================================================

int Mouse:: get_direction(){
    return direction;
}

void Mouse:: set_direction(int dir){
    direction = dir;
}

bool Mouse:: get_front_sensor_value(){
    bool has_wall;
    if(direction == NORTH){
        if(mouse_x == MAZE_SIZE-1){ //if the mouse is at the topest row;
            return 1;
        }
        // TODO: change to check front sensor value.
        // TODO: has_wall = reference_maze->maze[mouse_x][mouse_y]->top_wall;
        has_wall = driver->has_front_wall();
        detected_maze->maze[mouse_x][mouse_y]->top_wall = has_wall;
    }
    else if(direction == SOUTH){
        if(mouse_x == 0){
            return 1;
        }
        // TODO: has_wall = reference_maze->maze[mouse_x-1][mouse_y]->top_wall; //its bottom cell's upper wall
        has_wall = driver->has_front_wall();
        detected_maze->maze[mouse_x-1][mouse_y]->top_wall = has_wall;
    }
    else if(direction == EAST){
        if(mouse_y == MAZE_SIZE-1)
            return 1;
        // TODO: has_wall = reference_maze->maze[mouse_x][mouse_y]->right_wall;
        has_wall = driver->has_front_wall();
        detected_maze->maze[mouse_x][mouse_y]->right_wall = has_wall;
    }       
    else{ //WEST
        if(mouse_y == 0)
            return 1;
        // TODO: has_wall = reference_maze->maze[mouse_x][mouse_y-1]->right_wall;
        has_wall = driver->has_front_wall();
        detected_maze->maze[mouse_x][mouse_y-1]->right_wall = has_wall;
    }
    return has_wall;
}

bool Mouse:: get_left_sensor_value(){
    bool has_wall;

    if(direction == NORTH){
        if(mouse_y == 0) //if the mouse is at the left_most column
            return 1;
        
        // TODO: has_wall = reference_maze->maze[mouse_x][mouse_y-1]->right_wall;
        has_wall = driver->has_left_wall();
        detected_maze->maze[mouse_x][mouse_y-1]->right_wall = has_wall;

    }
    else if(direction == SOUTH){
        if(mouse_y == MAZE_SIZE-1)
            return 1;

        // TODO: has_wall = reference_maze->maze[mouse_x][mouse_y]->right_wall;
        has_wall = driver->has_left_wall();
        detected_maze->maze[mouse_x][mouse_y]->right_wall = has_wall;

    }
    else if(direction == EAST){
        if(mouse_x == MAZE_SIZE-1)
            return 1;

        // TODO: has_wall = reference_maze->maze[mouse_x][mouse_y]->top_wall;
        has_wall = driver->has_left_wall();
        detected_maze->maze[mouse_x][mouse_y]->top_wall = has_wall;

    }
    else{
        if(mouse_x == 0)
            return 1;
        // TODO: has_wall = reference_maze->maze[mouse_x-1][mouse_y]->top_wall;
        has_wall = driver->has_left_wall();
        detected_maze->maze[mouse_x-1][mouse_y]->top_wall = has_wall;
    }

    return has_wall;
}

bool Mouse:: get_right_sensor_value(){

    bool has_wall;

    if(direction == NORTH){
        if(mouse_y == MAZE_SIZE-1) //if the mouse is at the right_most column
            return 1;

        // TODO: has_wall = reference_maze->maze[mouse_x][mouse_y]->right_wall;
        has_wall = driver->has_right_wall();
        detected_maze->maze[mouse_x][mouse_y]->right_wall = has_wall;

    }
    else if(direction == SOUTH){
        if(mouse_y == 0)
            return 1;

        // TODO: has_wall = reference_maze->maze[mouse_x][mouse_y-1]->right_wall;
        has_wall = driver->has_right_wall();
        detected_maze->maze[mouse_x][mouse_y-1]->right_wall = has_wall;

    }
    else if(direction == EAST){
        if(mouse_x == 0)
            return 1;

        // TODO: has_wall = reference_maze->maze[mouse_x-1][mouse_y]->top_wall;
        has_wall = driver->has_right_wall();
        detected_maze->maze[mouse_x-1][mouse_y]->top_wall = has_wall;
    }
    else{
        if(mouse_x == MAZE_SIZE-1)
            return 1;

        // TODO: has_wall = reference_maze->maze[mouse_x][mouse_y]->top_wall;
        has_wall = driver->has_right_wall();
        detected_maze->maze[mouse_x][mouse_y]->top_wall = has_wall;

    }

    return has_wall;

}



// the front sensor is 0, then the mouse can move in this direction
bool Mouse:: can_move(){
    //in the real world, instead of using 0, we need to set some constant
    //as a threshold for the sensor value; 
    return (get_front_sensor_value()==0);
}


//Assumption: the maze[0][y] and maze[x][0] has left wall and bottom wall
void Mouse:: check_open_neighbor(){

    right_sensor = get_right_sensor_value();
    front_sensor = get_front_sensor_value();
    left_sensor = get_left_sensor_value();



    if(direction == NORTH){

        if(prev == SOUTH){
            south_open = 1;
        }

        north_open = !front_sensor;
        east_open = !right_sensor;
        west_open = !left_sensor;


    } else if(direction == SOUTH){

        if(prev == NORTH){
            north_open = 1;
        }

        south_open = !front_sensor;
        east_open = !left_sensor;
        west_open = !right_sensor;

    } else if(direction == EAST){

        if(prev == WEST){
            west_open = 1;
        }

        south_open = !right_sensor;
        east_open = !front_sensor;
        north_open = !left_sensor;
       

    } else{ //WEST

        if(prev == EAST){
            east_open = 1;
        }

        south_open = !left_sensor;
        west_open = !front_sensor;
        north_open = !right_sensor;
    }

}

// TODO: double check
/* Check the minimum from a set of neighboring cells */
int Mouse:: min_open_neighbor(vector<Cell*> cells) {
    int min = MAX;
    for (vector<Cell *>::iterator it = cells.begin(); it != cells.end(); it++) {
        if ((*it)->dist < min) {
            min = (*it)->dist;
        }
    }
    // printf("min %d\n", min);
    return min;
}


void Mouse:: update_distance(){
    Cell * curr;
    vector<Cell*> neighbor;
    vector<Cell*> open_neighbor;
    int min_dist;
    int x, y;
    int count = 0;


    stk.push_back(detected_maze->maze[mouse_x][mouse_y]);

    //printf("curr dist: %d\n", curr->dist);
    // while(!stk.empty()){
    while(!stk.empty()){
       

        curr = stk.back();
        stk.pop_back();
        
        x = curr->x;
        y = curr->y;

        if(x < MAZE_SIZE-1){ //top cell
            neighbor.push_back(detected_maze->maze[x+1][y]);
            // if(north_open)
            //     open_neighbor.push_back(detected_maze->maze[mouse_x + 1][mouse_y]);
            if(!curr->top_wall){
                open_neighbor.push_back(detected_maze->maze[x + 1][y]);
            }
          
        }


        if(y > 0){ //left cell
            neighbor.push_back(detected_maze->maze[x][y-1]);
            
            if (!(detected_maze->maze[x][y - 1]->right_wall)) {
                open_neighbor.push_back(detected_maze->maze[x][y - 1]);
            }
            // if(west_open)
            //     open_neighbor.push_back(detected_maze->maze[mouse_x][mouse_y-1]);

        }

        if(x > 0){ //bottom cell
            neighbor.push_back(detected_maze->maze[x-1][y]);
            if (!(detected_maze->maze[x-1][y]->top_wall)) {
                open_neighbor.push_back(detected_maze->maze[x-1][y]);
            }
            // if(south_open)
            //     open_neighbor.push_back(detected_maze->maze[mouse_x-1][mouse_y]);
        }

        if(y < MAZE_SIZE -1){ //right_cell
            neighbor.push_back(detected_maze->maze[x][y+1]);
            // if(east_open)
                // open_neighbor.push_back(detected_maze->maze[mouse_x][mouse_y+1]);
            if(!curr->right_wall){
                open_neighbor.push_back(detected_maze->maze[x][y+1]);
            }
        }


  

      

     

        if (open_neighbor.empty()) {
            // printf("This should never happen, error\n");
            neighbor.clear();
            continue;
        }

    

        min_dist = min_open_neighbor(open_neighbor);
  
        open_neighbor.clear();

        //printf("count %d with current cell <%d, %d>, dist: %d\n", count, curr->x, curr->y, curr->dist);

        if (curr->dist - 1 != min_dist) {
            curr->dist = min_dist + 1;
            for (vector<Cell *>::iterator it = neighbor.begin(); it != neighbor.end(); it++) {
                if (!detected_maze->is_center(*it)) {
                    stk.push_back(*it);
                }
            }
            neighbor.clear();
        }


        count++;
    }
    // printf("Finishing updating the distance\n");

}



//void Mouse:: print_open_direction(){
//
//    printf("north open: %d ", north_open);
//    printf("south open: %d ", south_open);
//    printf("east open: %d ", east_open);
//    printf("west open: %d\n", west_open);
//}
//
//void Mouse:: print_sensor_reading(){
//    printf("current direction: %d\n", direction);
//    printf("right sensor: %d ", get_right_sensor_value());
//    printf("left sensor: %d ",get_left_sensor_value());
//    printf("front sensor: %d\n", get_front_sensor_value());
//}

void Mouse:: move_one_cell(){
    //check the open neighbor and update the wall at the same time
//    printf("CURRENT MOUSE POSITION <%d, %d>\n", mouse_x, mouse_y);
//    printf("prev direction is %d\n", prev);
//    printf("current direction is %d\n", direction);

    check_open_neighbor();

    // printf("finish checking open neighbor\n");
    //update the distance properly
    update_distance();

    // printf("update_distance\n");

    //pick the next cell and adjust_direction

//    print_sensor_reading();
//    print_open_direction();

    int min_dist = MAX;
    int temp;
    if(north_open){
        temp = detected_maze->maze[mouse_x+1][mouse_y]->dist;
        if (temp < min_dist) {
            min_dist = temp;
            direction = NORTH;
            prev = SOUTH;
        }
       
    }


    if(east_open){
        temp = detected_maze->maze[mouse_x][mouse_y+1]->dist;
        if (temp < min_dist){
            min_dist = temp;
            direction = EAST;
            prev = WEST;

        }
    }
    
    // printf("after checking north\n");
    if(south_open){
        temp = detected_maze->maze[mouse_x-1][mouse_y]->dist;
        if (temp < min_dist){
            min_dist = temp;
            direction = SOUTH;
            prev = NORTH;

        }
    }
    // printf("after checking south\n");
    
    // printf("after checking east\n");
     
    if(west_open){
        temp = detected_maze->maze[mouse_x][mouse_y-1]->dist;
        if (temp < min_dist) {
            min_dist = temp;
            direction = WEST;
            prev = EAST;
        }
      
    }

   
    // printf("Finishing pick the next cell\n");


    //Move
    if(direction == NORTH){
        mouse_x += 1;
    }
    else if(direction == SOUTH){
        mouse_x -= 1;
    }
    else if (direction == EAST){
        mouse_y += 1;
    }
    else{ //WEST
        mouse_y -= 1;
    }

    north_open = 0;
    south_open = 0;
    east_open = 0;
    west_open = 0;
    
    
//
//    printf("Finishing Reset, METHOD ENDS\n");

}

bool Mouse::center_reached() {
    return !detected_maze->is_center(mouse_x, mouse_y);
}

int Mouse:: solve_maze(){
    const int STRAIGHT = 0, LEFT = 1, RIGHT = 2, UTURN = 3;
    //while the mouse has not find the center of the Maze
    //while(!detected_maze->is_center(mouse_x, mouse_y)){
    //        move_one_cell();
    //        print_maze();
    //    }
    // TODO: double check
        move_one_cell();
        //printf("mouse direction: %d \n", direction);
        
        int state = 5;
        
        if (prev_mouse_dir == direction){
            state = STRAIGHT;
        }
        // Mouse is facing south
        else if (prev_mouse_dir == SOUTH && direction == NORTH) {
            state = UTURN;
            prev_mouse_dir = NORTH;
        }
        else if (prev_mouse_dir == SOUTH && direction == EAST) {
            state = LEFT;
            prev_mouse_dir = EAST;
        }
        else if (prev_mouse_dir == SOUTH && direction == WEST) {
            state = RIGHT;
            prev_mouse_dir = WEST;
        }
        // Mouse is facing north
        else if (prev_mouse_dir == NORTH && direction == EAST) {
            state = RIGHT;
            prev_mouse_dir = EAST;
        }
        else if (prev_mouse_dir == NORTH && direction == WEST) {
            state = LEFT;
            prev_mouse_dir = WEST;
        }
        else if (prev_mouse_dir == NORTH && direction == SOUTH) {
            state = UTURN;
            prev_mouse_dir = SOUTH;
        }
        // Mouse is facing west
        else if (prev_mouse_dir == WEST && direction == NORTH) {
            state = RIGHT;
            prev_mouse_dir = NORTH;
        }
        else if (prev_mouse_dir == WEST && direction == SOUTH) {
            state = LEFT;
            prev_mouse_dir = SOUTH;
        }
        else if (prev_mouse_dir == WEST && direction == EAST) {
            state = UTURN;
            prev_mouse_dir = EAST;
        }
        // Mouse is facing west
        else if (prev_mouse_dir == EAST && direction == NORTH) {
            state = LEFT;
            prev_mouse_dir = NORTH;
        }
        else if (prev_mouse_dir == EAST && direction == SOUTH) {
            state = RIGHT;
            prev_mouse_dir = SOUTH;
        }
        else if (prev_mouse_dir == EAST && direction == WEST) {
            state = UTURN;
            prev_mouse_dir = WEST;
        }
        return state;
}

Mouse:: Mouse(){
    direction = NORTH;
    prev = -1;
    prev_mouse_dir = NORTH;

    north_open = 0;
    south_open = 0;
    east_open = 0;
    west_open = 0;

    mouse_x = 0;
    mouse_y = 0; 
    // reference_maze = new Maze();  //we should not modify any thing inside reference_maze
    detected_maze = new Maze();
}