pfilter prob.nxc

/* Written by Kevin Alexander Lee

  This is an implementation of NXT robot localization using probability and
  particle filters.

  Using the grid based map accompanying the code, the robot which initially
  has no information about its coordinate position will be able to locate itself
  and move to a specific target and destination.

  The NXT robot uses aspects of particle filters and probability sensing to
  determine which position it is likely at.

*/

/* the following section defines macros */

/* macros for directions, chosen to not conflict with other definitions
   and to allow arithmetic between directions*/
#define NORTH 100
#define EAST 101
#define SOUTH 102
#define WEST 103

/* macros for diagonal directions */
#define NORTHEAST 1000
#define SOUTHEAST 1001
#define SOUTHWEST 1002
#define NORTHWEST 1003

/* File related macros */
#define FILENAME "mapdata.txt"
#define READSIZE 10

/* Function like macro to convert characters from files to the corresponding
   int value.*/
#define ASCII_TO_INT(a) a-48

/* macros for the map dimensions. Based on yellow grid intersections*/
#define GWIDTH 3
#define GHEIGHT 5

/* macros for probability sensing
   THRESHOLD - a decimal value in (0,1). Determines when a particle is likely enough to be the robot's
               position
   MATCH - a decimal value in (0,1). How much to affect the probability when a particle's data matches
           the robot's surroundings, should be greater than MISMATCH
   MISMATCH - a decimal value in (0,1). How much to affect the probability when a particle's data does not
              match the robot's surroundings, should be less than MATCH

   decide these values for yourself
   */
#define THRESHOLD 0.7
#define MATCH
#define MISMATCH

#define SPEED 30
#define ROTATIONA 40

#define BLACKAVGR 40
#define BLACKAVGG 45
#define BLACKAVGB 40
#define REDAVGR 235
#define REDAVGG 92
#define REDAVGB 64
#define YELLOWAVGR 228
#define YELLOWAVGG 212
#define YELLOWAVGB 95
#define BLUEAVGR 85
#define BLUEAVGG 88
#define BLUEAVGB 120
#define GREENAVGR 74
#define GREENAVGG 124
#define GREENAVGB 65
#define WHITEAVGR 230
#define WHITEAVGG 210
#define WHITEAVGB 220


/* You may want to define macros to keep track of certain turning angles and
   speeds throughout your code. This provides an easy way to change them */


/* Represents a single particle.

   Each particle has a coordinate given by x and y and also a direction.

   The probability attached to a particle is the likelihood of it being the
   actual position and heading of the robot. This is updated based on the
   robot's surroundings.
   */
struct particle {
    int x;
    int y;
    int theta;
    float prob; /* weight of a particle representing probability */
};

/* Represents an intersection (yellow square) on the grid.

   Holds data on the surroundings of the intersection, namely coloured(or white)
   squares to the NE, SE, SW, NW of the intersection.

   Note that the directions are by default in terms of compass directions.
   When given a heading, the directions will change relative to the heading.
   */
struct intersection {

    int NE;
    int SE;
    int SW;
    int NW;
};
int get_color(ColorSensorReadType &csr) {

    bool red = true,
         black = true,
         blue = true,
         green = true,
         yellow = true,
         white = true;

    // check if input color is black
    black&=csr.NormalizedArray[0] <= BLACKAVGR * 1.8;
    black&=csr.NormalizedArray[0] >= BLACKAVGR / 1.8;
    black&=csr.NormalizedArray[1] <= BLACKAVGG * 1.8;
    black&=csr.NormalizedArray[1] >= BLACKAVGG / 1.8;
    black&=csr.NormalizedArray[2] <= BLACKAVGB * 1.8;
    black&=csr.NormalizedArray[2] >= BLACKAVGB / 1.8;

    // check if input color is yellow
    yellow&=csr.NormalizedArray[0] <= YELLOWAVGR * 1.3;
    yellow&=csr.NormalizedArray[0] >= YELLOWAVGR / 1.3;
    yellow&=csr.NormalizedArray[1] <= YELLOWAVGG * 1.3;
    yellow&=csr.NormalizedArray[1] >= YELLOWAVGG / 1.3;
    yellow&=csr.NormalizedArray[2] <= YELLOWAVGB * 1.3;
    yellow&=csr.NormalizedArray[2] >= YELLOWAVGB / 1.3;
    // check if input color is red
    red&=csr.NormalizedArray[0] <= REDAVGR * 1.3;
    red&=csr.NormalizedArray[0] >= REDAVGR / 1.3;
    red&=csr.NormalizedArray[1] <= REDAVGG * 1.3;
    red&=csr.NormalizedArray[1] >= REDAVGG / 1.3;
    red&=csr.NormalizedArray[2] <= REDAVGB * 1.3;
    red&=csr.NormalizedArray[2] >= REDAVGB / 1.3;
    // check if input color is blue
    blue&=csr.NormalizedArray[0] <= BLUEAVGR * 1.3;
    blue&=csr.NormalizedArray[0] >= BLUEAVGR / 1.3;
    blue&=csr.NormalizedArray[1] <= BLUEAVGG * 1.3;
    blue&=csr.NormalizedArray[1] >= BLUEAVGG / 1.3;
    blue&=csr.NormalizedArray[2] <= BLUEAVGB * 1.3;
    blue&=csr.NormalizedArray[2] >= BLUEAVGB / 1.3;
    // check if input color is green
    green&=csr.NormalizedArray[0] <= GREENAVGR * 1.3;
    green&=csr.NormalizedArray[0] >= GREENAVGR / 1.3;
    green&=csr.NormalizedArray[1] <= GREENAVGG * 1.3;
    green&=csr.NormalizedArray[1] >= GREENAVGG / 1.3;
    green&=csr.NormalizedArray[2] <= GREENAVGB * 1.3;
    green&=csr.NormalizedArray[2] >= GREENAVGB / 1.3;
    // check if input color is white
    white&=csr.NormalizedArray[0] <= WHITEAVGR * 1.1;
    white&=csr.NormalizedArray[0] >= WHITEAVGR / 1.1;
    white&=csr.NormalizedArray[1] <= WHITEAVGG * 1.1;
    white&=csr.NormalizedArray[1] >= WHITEAVGG / 1.1;
    white&=csr.NormalizedArray[2] <= WHITEAVGB * 1.1;
    white&=csr.NormalizedArray[2] >= WHITEAVGB / 1.1;

    if (black) {return 1;}
    else if (blue) {return 2;}
    else if (green) {return 3;}
    else if (yellow) {return 4;}
    else if (red) {return 5;}
    else if (white) {return 6;}
    else {return -1;}

}

/* function prototypes, to be added if needed
*/

/* Subroutine which moves the robot forward from one intersection to the next
*/
sub move_robot(ColorSensorReadType &csr) {
    ////////////////////////////////////////////////////////////////////////////
    //  TO DO:
    //
    //  Complete this subroutine.
    //  Your robot will need to nagivates its way from one intersection to the
    //  next. If it reaches a red boundary, it will do a 180 degree turn and
    //  return to the previous intersection facing in the opposite direction.
    //
    //  You may need to account for motor "noise" as your robot will not always
    //  make turns perfectly and go straight forward each time.
    //
    ////////////////////////////////////////////////////////////////////////////
    bool red = false;
    int prev_color = 0;
    int current_color = -1;

    while(true) {

        SysColorSensorRead(csr);

        current_color = get_color(csr);

        NumOut(0,LCD_LINE1,csr.NormalizedArray[0]);
        NumOut(0,LCD_LINE2,csr.NormalizedArray[1]);
        NumOut(0,LCD_LINE3,csr.NormalizedArray[2]);

        if (prev_color == 0 && current_color != -1) {
            prev_color = current_color;
        }
        // set curr to prev color to prevent curr is unknown
        else if (prev_color != 0 && prev_color != -1 && current_color == -1 ) {
            current_color = prev_color;
        }


/*        if (current_color == -1){
        Off(OUT_AC);
        Wait(1000);}*/
        if(current_color == 1) {
            if (red) { // move from red to yellow
                OnRev(OUT_AC, SPEED); // move backwards with an output power of SPEED
            } else {
                OnFwd(OUT_AC, SPEED); // move forwards with an output power of SPEED
            }
        } else if(current_color == 5) { // reads red
            red = true;
            OnRev(OUT_AC, SPEED); // move backwards with an output power of SPEED
        } else if (current_color == 4) { // read yellow
           until (current_color == 1) {
                OnFwd(OUT_AC, SPEED);
                SysColorSensorRead(csr);
                current_color = get_color(csr);
            }
            red = false;
            break;

        } else if (current_color == 2 || current_color == 3 || current_color == 4 || current_color == 6){ //went off black, rotate back to black
           until (current_color == 1) {
               OnFwd(OUT_A, ROTATIONA);
               OnRev(OUT_C, SPEED);
               Wait(1000);
               SysColorSensorRead(csr);
               current_color = get_color(csr);
               if (current_color == 1){
                  break;
               }
               OnRev(OUT_A, ROTATIONA);
               OnFwd(OUT_C, SPEED);
               Wait(2000);
               SysColorSensorRead(csr);
               current_color = get_color(csr);
               if (current_color == 1){
                  break;
               }
               OnFwd(OUT_A, ROTATIONA);
               OnRev(OUT_C, SPEED);
               Wait(800);
               OnRev(OUT_AC, SPEED);
               Wait(200);
               SysColorSensorRead(csr);
               current_color = get_color(csr);
           }
        }
        if (current_color != -1) {
            prev_color = current_color;
        }
    }
}

/* This function scans the surroundings of the robot.
   It takes readings of the colour of the 4 squares surrounding the
   intersection and returns an intersection variable which holds the data.
*/

intersection scan_surroundings(ColorSensorReadType &csr) {
    intersection rsense;

    ////////////////////////////////////////////////////////////////////////////
    // TO DO:
    //
    // Implement a function in which the robot will scan the colour of the
    // surrounding squares at an intersection.
    // It will use the colour sensor to take readings from all four squares
    // in each of the NE, SE, SW, NW directions relative to the forward facing
    // direction of the robot.
    //
    ////////////////////////////////////////////////////////////////////////////
    int current_color;
    int prev_color = 0;
    bool wait = false;
    while(rsense.NE == 0 || rsense.NW == 0 || rsense.SE == 0 || rsense.SW == 0) {
        OnRev(OUT_A, ROTATIONA);
        OnFwd(OUT_C, SPEED);
        SysColorSensorRead(csr);
        current_color = get_color(csr);
        if (current_color != -1 && prev_color == 0){
            prev_color = current_color;
        }
        if (current_color == -1 && prev_color != 0){
            current_color = prev_color;
        }

        if (current_color == 1) {
            wait = false;
        } else if (wait == false){
            if (rsense.NE == 0 && rsense.NW == 0 && rsense.SE == 0 &&
                    rsense.SW == 0) {
                rsense.NE = current_color;
                wait = true;
            } else if (rsense.NW == 0 && rsense.SE == 0 && rsense.SW == 0) {
                rsense.SE = current_color;
                wait = true;
            } else if (rsense.NW == 0 && rsense.SW == 0) {
                rsense.SW = current_color;
                wait = true;
            } else {
                rsense.NW = current_color;
                wait = true;
            }
        }
        if (current_color != -1){
            prev_color = current_color;
        }

        NumOut(0, LCD_LINE5, rsense.NE);
        NumOut(0, LCD_LINE6, rsense.SE);
        NumOut(0, LCD_LINE7, rsense.SW);
        NumOut(0, LCD_LINE8, rsense.NW);
        Wait(10);
    }

    ClearScreen();
    /*until (get_color(csr) != 1){        // 90 degrees left
               OnFwd(OUT_A, ROTATIONA);
               OnRev(OUT_C, SPEED);
            }*/

    SysColorSensorRead(csr);
    current_color = get_color(csr);


    while (current_color != 1) {       // back to original position
        OnRev(OUT_A, ROTATIONA);
        OnFwd(OUT_C, SPEED);
        SysColorSensorRead(csr);
        current_color = get_color(csr);

    }
    Off(OUT_AC);
    return rsense;
}



/* Given the original heading and a new heading, turns the robot to reflect
   the new heading

   This subroutine is used after the robot knows its position
   */

sub change_direction(int c, ColorSensorReadType &csr){
    ////////////////////////////////////////////////////////////////////////////
    // TO DO:
    //
    // Implement each of the turns specified below.
    //
    ////////////////////////////////////////////////////////////////////////////
    SysColorSensorRead(csr);
    int dest_color = get_color(csr);
    int prev_color = dest_color;
    int curr_color = dest_color;
//    int change = new - orig;
    int second_color;


// selects the appropriate turn based on the headings given

    switch (c){
           case 3: //turn right 45 degrees
                while (curr_color == dest_color){        // color changes
                   OnFwd(OUT_A, ROTATIONA);
                   OnRev(OUT_C, SPEED);
                   SysColorSensorRead(csr);
                   curr_color = get_color(csr);
                   if (curr_color == -1 && prev_color != -1){
                        curr_color = prev_color;
                   }
                   if (curr_color != -1){
                        prev_color = curr_color;
                   }
                }
                break;
           case -1: //turn right 90 degrees

                // color changes twice
                while (curr_color == dest_color){        // color changes
                   OnFwd(OUT_A, ROTATIONA);
                   OnRev(OUT_C, SPEED);
                   SysColorSensorRead(csr);
                   curr_color = get_color(csr);
                   if (curr_color == -1 && prev_color != -1){
                        curr_color = prev_color;
                   }
                   if (curr_color != -1){
                        prev_color = curr_color;
                   }
                }
                SysColorSensorRead(csr);
                second_color = get_color(csr);
                curr_color = second_color;
                NumOut(0, LCD_LINE1, second_color);
                NumOut(0, LCD_LINE2, curr_color);
                while (curr_color != 1){        // color changes
                   TextOut(0, LCD_LINE5, "IN LOOP");
                   OnFwd(OUT_A, ROTATIONA);
                   OnRev(OUT_C, SPEED);
                   SysColorSensorRead(csr);
                   curr_color = get_color(csr);
                   if (curr_color == -1 && prev_color != -1){
                        curr_color = prev_color;
                   }
                   if (curr_color != -1){
                        prev_color = curr_color;
                   }
                }
                break;
           case -3: //turn left 45 degrees
                while (curr_color == dest_color){        // color changes
                   OnRev(OUT_A, ROTATIONA);
                   OnFwd(OUT_C, SPEED);
                   SysColorSensorRead(csr);
                   curr_color = get_color(csr);
                   if (curr_color == -1 && prev_color != -1){
                        curr_color = prev_color;
                   }
                   if (curr_color != -1){
                        prev_color = curr_color;
                   }
                }
                break;
           case 1: //turn left 90 degrees

                // color changes twice
                while (curr_color == dest_color){        // color changes
                   OnRev(OUT_A, ROTATIONA);
                   OnFwd(OUT_C, SPEED);
                   SysColorSensorRead(csr);
                   curr_color = get_color(csr);
                   if (curr_color == -1 && prev_color != -1){
                        curr_color = prev_color;
                   }
                   if (curr_color != -1){
                        prev_color = curr_color;
                   }
                }
                SysColorSensorRead(csr);
                second_color = get_color(csr);
                curr_color = second_color;
                NumOut(0, LCD_LINE1, second_color);
                NumOut(0, LCD_LINE2, curr_color);
                while (curr_color != 1){        // color changes
                   TextOut(0, LCD_LINE5, "IN LOOP");
                   OnRev(OUT_A, ROTATIONA);
                   OnFwd(OUT_C, SPEED);
                   SysColorSensorRead(csr);
                   curr_color = get_color(csr);
                   if (curr_color == -1 && prev_color != -1){
                        curr_color = prev_color;
                   }
                   if (curr_color != -1){
                        prev_color = curr_color;
                   }
                }
                break;
           case 2:
		   case -2:
                // 180 degree turn
                /*
                until (get_color(csr) != dest_color){       // color changes
                   OnRev(OUT_A, ROTATIONA);
                   OnFwd(OUT_C, SPEED);
                }
				int second_color = get_color(csr);
				until (get_color(csr) != second_color){       // color changes twice
                   OnRev(OUT_A, ROTATIONA);
                   OnFwd(OUT_C, SPEED);
                }
				int third_color = get_color(csr);
				until (get_color(csr) != second_color){       // color changes three times
                   OnRev(OUT_A, ROTATIONA);
                   OnFwd(OUT_C, SPEED);
                }int forth_color = get_color(csr);
				until (get_color(csr) != second_color){       // color changes four times
                   OnRev(OUT_A, ROTATIONA);
                   OnFwd(OUT_C, SPEED);
                }*/
                break;
    }
}



/* Given a start coordinate, finish coordinate, and a heading, this function
   moves the robot from the start to finish.

   The coordinates given are in terms of index in the map array. The x and y
   coordinates need to be extracted from that index.

   After the robot has finished moving, the current heading of the robot is
   returned.

   This function is used after the robot knows it's own position.
*/

int move_to(int s, int f, int h, ColorSensorReadType &csr){
    int sx, sy, fx, fy, tx, ty;
    int heading;
    // extract coordinates from given information
    sx = s/GHEIGHT;
    sy = s%GHEIGHT;
    fx = f/GHEIGHT;
    fy = f%GHEIGHT;

/* calculate how far to go in manhatten distances */

    tx = fx - sx;
    ty = fy - sy;
    heading = h;

    ///////////////////////////////////////////////////////////////////////////
    // TO DO:
    //
    // Move the robot from the starting point described by (sx, sy) to (fx, fy)
    // Be sure to keep track of the direction the robot is facing when it
    // makes turns.
    //
    ////////////////////////////////////////////////////////////////////////////

    /*
        move to changes direction once, calls move robot, call move to again
    */

	while(tx > 0 || ty > 0){
		if (heading == NORTH){
			if (tx > 0){
				change_direction(-1, csr); //right 90 degrees
				move_robot(csr);
				heading = EAST;
			}else if (tx < 0){
				change_direction(1, csr); //left 90 degrees
				move_robot(csr);
				heading = WEST;
			}else if (ty > 0){
				move_robot(csr);
			}else if (ty < 0){
				change_direction(1, csr); //left 90 degrees
				change_direction(1, csr); //left 90 degrees
				move_robot(csr);
				heading = SOUTH;
			}
		}else if (heading == EAST){
			if (tx > 0){
				move_robot(csr);
			}else if (tx < 0){
				change_direction(1, csr); //left 90 degrees
				change_direction(1, csr); //left 90 degrees
				move_robot(csr);
				heading = WEST;
			}else if (ty > 0){
				change_direction(1, csr); //left 90 degrees
				move_robot(csr);
				heading = NORTH;
			}else if (ty < 0){
				change_direction(-1, csr); //right 90 degrees
				move_robot(csr);
				heading = SOUTH;
			}
		}else if (heading == SOUTH){
			if (tx > 0){
				change_direction(1, csr); //left 90 degrees
				move_robot(csr);
				heading = EAST;
			}else if (tx < 0){
				change_direction(-1, csr); //right 90 degrees
				move_robot(csr);
				heading = WEST;
			}else if (ty > 0){
				change_direction(1, csr); //left 90 degrees
				change_direction(1, csr); //left 90 degrees
				move_robot(csr);
				heading = NORTH;
			}else if (ty < 0){
				move_robot(csr);
			}
		}else if (heading == WEST){
			if (tx > 0){
				change_direction(-1, csr); //right 90 degrees
				change_direction(-1, csr); //right 90 degrees
				move_robot(csr);
				heading = EAST;
			}else if (tx < 0){
				move_robot(csr);
			}else if (ty > 0){
				change_direction(-1, csr); //right 90 degrees
				move_robot(csr);
				heading = NORTH;
			}else if (ty < 0){
				change_direction(1, csr); //left 90 degrees
				move_robot(csr);
				heading = SOUTH;
			}
		}
	}
    return heading;
}

/* This function takes a particle and returns an updated particle after moving

   To "move" a particle is to move it forward in the direction of the particle.
   We move these particles in the same way we would move the robot.

   Upon reaching a boundary, the particle is turned 180 degrees around.

*/
particle move(particle p) {

    ///////////////////////////////////////////////////////////////////////
    // TO DO:
    //
    // Move particle p forward one intersection in the direction it is
    // facing.
    // Similar to the robot's behaviour, if a particle reaches the end of
    // the map, it turns in the opposite direction and returns to the
    // previous intersection it was at.
    //
    ///////////////////////////////////////////////////////////////////////
    if(p.theta == NORTH) {
        if(p.y != 4) {
            p.y = p.y + 1;
        } else {
            // prob doubles
        }
    } else if(p.theta == EAST) {
        if(p.x != 2) {
            p.x = p.x + 1;
        } else {
            // prob doubles
        }
    } else if(p.theta == SOUTH) {
        if(p.y != 0) {
            p.y = p.y - 1;
        } else {
            // prob doubles
        }
    } else if(p.theta == WEST) {
        if(p.x != 0) {
            p.x = p.x + 1;
        } else {
            // prob doubles
        }
    }

    return p;
}

/* This function returns data on intersection surroundings for particles.

   Based on the map data array, we want the colour of the square at the given x
   and y coordinates in direction dir which is one of NE, SE, SW, NW.

   Take note that the heading given will change the perceived directions.
   For example, facing north, NE in terms of compass directions is northeast.
   Facing south however, NE in terms of compass directions is southwest.

   Because the robot does not know it's own heading, the particles must reflect
   this by using sensing relative to heading.
*/
int senseParticle(intersection map[], int x, int y, int dir, int heading){

    ////////////////////////////////////////////////////////////////////////////
    // TO DO:
    //
    // Implement the function which checks the surroundings of the particle at
    // (x, y) facing the direction given by heading.
    //
    // The data you need to determine the particle's surroundings is given by
    // the map data array.
    //
    // The integer returned is the
    // colour of the square in the direction of dir which is one of NE, SE, SW,
    // or NW.
    // Keep in mind that the direction is relative to the heading as mentioned
    // in the function description.
    //
    ////////////////////////////////////////////////////////////////////////////
	int color = 0;
	if(x == 0 && y == 0){
		if (heading == NORTH){
			if(dir == NORTHEAST){
				color = map[0].NE;
			}else if(dir == NORTHWEST){
				color = map[0].NW;
			}else if(dir == SOUTHEAST){
				color = map[0].SE;
			}else if(dir == SOUTHWEST){
				color = map[0].SW;
			}
		}else if (heading == EAST){
			if(dir == NORTHEAST){
				color = map[0].SE;
			}else if(dir == NORTHWEST){
				color = map[0].NE;
			}else if(dir == SOUTHEAST){
				color = map[0].SW;
			}else if(dir == SOUTHWEST){
				color = map[0].NW;
			}
		}else if (heading == SOUTH){
			if(dir == NORTHEAST){
				color = map[0].SW;
			}else if(dir == NORTHWEST){
				color = map[0].SE;
			}else if(dir == SOUTHEAST){
				color = map[0].NW;
			}else if(dir == SOUTHWEST){
				color = map[0].NE;
			}
		}else if (heading == WEST){
			if(dir == NORTHEAST){
				color = map[0].NW;
			}else if(dir == NORTHWEST){
				color = map[0].SW;
			}else if(dir == SOUTHEAST){
				color = map[0].NE;
			}else if(dir == SOUTHWEST){
				color = map[0].SE;
			}
		}
	}else if(x == 1 && y == 0){
		if (heading == NORTH){
			if(dir == NORTHEAST){
				color = map[5].NE;
			}else if(dir == NORTHWEST){
				color = map[5].NW;
			}else if(dir == SOUTHEAST){
				color = map[5].SE;
			}else if(dir == SOUTHWEST){
				color = map[5].SW;
			}
		}else if (heading == EAST){
			if(dir == NORTHEAST){
				color = map[5].SE;
			}else if(dir == NORTHWEST){
				color = map[5].NE;
			}else if(dir == SOUTHEAST){
				color = map[5].SW;
			}else if(dir == SOUTHWEST){
				color = map[5].NW;
			}
		}else if (heading == SOUTH){
			if(dir == NORTHEAST){
				color = map[5].SW;
			}else if(dir == NORTHWEST){
				color = map[5].SE;
			}else if(dir == SOUTHEAST){
				color = map[5].NW;
			}else if(dir == SOUTHWEST){
				color = map[5].NE;
			}
		}else if (heading == WEST){
			if(dir == NORTHEAST){
				color = map[5].NW;
			}else if(dir == NORTHWEST){
				color = map[5].SW;
			}else if(dir == SOUTHEAST){
				color = map[5].NE;
			}else if(dir == SOUTHWEST){
				color = map[5].SE;
			}
		}
	}else if(x == 2 && y == 0){
		if (heading == NORTH){
			if(dir == NORTHEAST){
				color = map[10].NE;
			}else if(dir == NORTHWEST){
				color = map[10].NW;
			}else if(dir == SOUTHEAST){
				color = map[10].SE;
			}else if(dir == SOUTHWEST){
				color = map[10].SW;
			}
		}else if (heading == EAST){
			if(dir == NORTHEAST){
				color = map[10].SE;
			}else if(dir == NORTHWEST){
				color = map[10].NE;
			}else if(dir == SOUTHEAST){
				color = map[10].SW;
			}else if(dir == SOUTHWEST){
				color = map[10].NW;
			}
		}else if (heading == SOUTH){
			if(dir == NORTHEAST){
				color = map[10].SW;
			}else if(dir == NORTHWEST){
				color = map[10].SE;
			}else if(dir == SOUTHEAST){
				color = map[10].NW;
			}else if(dir == SOUTHWEST){
				color = map[10].NE;
			}
		}else if (heading == WEST){
			if(dir == NORTHEAST){
				color = map[10].NW;
			}else if(dir == NORTHWEST){
				color = map[10].SW;
			}else if(dir == SOUTHEAST){
				color = map[10].NE;
			}else if(dir == SOUTHWEST){
				color = map[10].SE;
			}
		}
	}else if(x == 0 && y == 1){
		if (heading == NORTH){
			if(dir == NORTHEAST){
				color = map[1].NE;
			}else if(dir == NORTHWEST){
				color = map[1].NW;
			}else if(dir == SOUTHEAST){
				color = map[1].SE;
			}else if(dir == SOUTHWEST){
				color = map[1].SW;
			}
		}else if (heading == EAST){
			if(dir == NORTHEAST){
				color = map[1].SE;
			}else if(dir == NORTHWEST){
				color = map[1].NE;
			}else if(dir == SOUTHEAST){
				color = map[1].SW;
			}else if(dir == SOUTHWEST){
				color = map[1].NW;
			}
		}else if (heading == SOUTH){
			if(dir == NORTHEAST){
				color = map[1].SW;
			}else if(dir == NORTHWEST){
				color = map[1].SE;
			}else if(dir == SOUTHEAST){
				color = map[1].NW;
			}else if(dir == SOUTHWEST){
				color = map[1].NE;
			}
		}else if (heading == WEST){
			if(dir == NORTHEAST){
				color = map[1].NW;
			}else if(dir == NORTHWEST){
				color = map[1].SW;
			}else if(dir == SOUTHEAST){
				color = map[1].NE;
			}else if(dir == SOUTHWEST){
				color = map[1].SE;
			}
		}
	}else if(x == 1 && y == 1){
		if (heading == NORTH){
			if(dir == NORTHEAST){
				color = map[6].NE;
			}else if(dir == NORTHWEST){
				color = map[6].NW;
			}else if(dir == SOUTHEAST){
				color = map[6].SE;
			}else if(dir == SOUTHWEST){
				color = map[6].SW;
			}
		}else if (heading == EAST){
			if(dir == NORTHEAST){
				color = map[6].SE;
			}else if(dir == NORTHWEST){
				color = map[6].NE;
			}else if(dir == SOUTHEAST){
				color = map[6].SW;
			}else if(dir == SOUTHWEST){
				color = map[6].NW;
			}
		}else if (heading == SOUTH){
			if(dir == NORTHEAST){
				color = map[6].SW;
			}else if(dir == NORTHWEST){
				color = map[6].SE;
			}else if(dir == SOUTHEAST){
				color = map[6].NW;
			}else if(dir == SOUTHWEST){
				color = map[6].NE;
			}
		}else if (heading == WEST){
			if(dir == NORTHEAST){
				color = map[6].NW;
			}else if(dir == NORTHWEST){
				color = map[6].SW;
			}else if(dir == SOUTHEAST){
				color = map[6].NE;
			}else if(dir == SOUTHWEST){
				color = map[6].SE;
			}
		}
	}else if(x == 2 && y == 1){
		if (heading == NORTH){
			if(dir == NORTHEAST){
				color = map[11].NE;
			}else if(dir == NORTHWEST){
				color = map[11].NW;
			}else if(dir == SOUTHEAST){
				color = map[11].SE;
			}else if(dir == SOUTHWEST){
				color = map[11].SW;
			}
		}else if (heading == EAST){
			if(dir == NORTHEAST){
				color = map[11].SE;
			}else if(dir == NORTHWEST){
				color = map[11].NE;
			}else if(dir == SOUTHEAST){
				color = map[11].SW;
			}else if(dir == SOUTHWEST){
				color = map[11].NW;
			}
		}else if (heading == SOUTH){
			if(dir == NORTHEAST){
				color = map[11].SW;
			}else if(dir == NORTHWEST){
				color = map[11].SE;
			}else if(dir == SOUTHEAST){
				color = map[11].NW;
			}else if(dir == SOUTHWEST){
				color = map[11].NE;
			}
		}else if (heading == WEST){
			if(dir == NORTHEAST){
				color = map[11].NW;
			}else if(dir == NORTHWEST){
				color = map[11].SW;
			}else if(dir == SOUTHEAST){
				color = map[11].NE;
			}else if(dir == SOUTHWEST){
				color = map[11].SE;
			}
		}
	}else if(x == 0 && y == 2){
		if (heading == NORTH){
			if(dir == NORTHEAST){
				color = map[2].NE;
			}else if(dir == NORTHWEST){
				color = map[2].NW;
			}else if(dir == SOUTHEAST){
				color = map[2].SE;
			}else if(dir == SOUTHWEST){
				color = map[2].SW;
			}
		}else if (heading == EAST){
			if(dir == NORTHEAST){
				color = map[2].SE;
			}else if(dir == NORTHWEST){
				color = map[2].NE;
			}else if(dir == SOUTHEAST){
				color = map[2].SW;
			}else if(dir == SOUTHWEST){
				color = map[2].NW;
			}
		}else if (heading == SOUTH){
			if(dir == NORTHEAST){
				color = map[2].SW;
			}else if(dir == NORTHWEST){
				color = map[2].SE;
			}else if(dir == SOUTHEAST){
				color = map[2].NW;
			}else if(dir == SOUTHWEST){
				color = map[2].NE;
			}
		}else if (heading == WEST){
			if(dir == NORTHEAST){
				color = map[2].NW;
			}else if(dir == NORTHWEST){
				color = map[2].SW;
			}else if(dir == SOUTHEAST){
				color = map[2].NE;
			}else if(dir == SOUTHWEST){
				color = map[2].SE;
			}
		}
	}else if(x == 1 && y == 2){
		if (heading == NORTH){
			if(dir == NORTHEAST){
				color = map[7].NE;
			}else if(dir == NORTHWEST){
				color = map[7].NW;
			}else if(dir == SOUTHEAST){
				color = map[7].SE;
			}else if(dir == SOUTHWEST){
				color = map[7].SW;
			}
		}else if (heading == EAST){
			if(dir == NORTHEAST){
				color = map[7].SE;
			}else if(dir == NORTHWEST){
				color = map[7].NE;
			}else if(dir == SOUTHEAST){
				color = map[7].SW;
			}else if(dir == SOUTHWEST){
				color = map[7].NW;
			}
		}else if (heading == SOUTH){
			if(dir == NORTHEAST){
				color = map[7].SW;
			}else if(dir == NORTHWEST){
				color = map[7].SE;
			}else if(dir == SOUTHEAST){
				color = map[7].NW;
			}else if(dir == SOUTHWEST){
				color = map[7].NE;
			}
		}else if (heading == WEST){
			if(dir == NORTHEAST){
				color = map[7].NW;
			}else if(dir == NORTHWEST){
				color = map[7].SW;
			}else if(dir == SOUTHEAST){
				color = map[7].NE;
			}else if(dir == SOUTHWEST){
				color = map[7].SE;
			}
		}
	}else if(x == 2 && y == 2){
		if (heading == NORTH){
			if(dir == NORTHEAST){
				color = map[12].NE;
			}else if(dir == NORTHWEST){
				color = map[12].NW;
			}else if(dir == SOUTHEAST){
				color = map[12].SE;
			}else if(dir == SOUTHWEST){
				color = map[12].SW;
			}
		}else if (heading == EAST){
			if(dir == NORTHEAST){
				color = map[12].SE;
			}else if(dir == NORTHWEST){
				color = map[12].NE;
			}else if(dir == SOUTHEAST){
				color = map[12].SW;
			}else if(dir == SOUTHWEST){
				color = map[12].NW;
			}
		}else if (heading == SOUTH){
			if(dir == NORTHEAST){
				color = map[12].SW;
			}else if(dir == NORTHWEST){
				color = map[12].SE;
			}else if(dir == SOUTHEAST){
				color = map[12].NW;
			}else if(dir == SOUTHWEST){
				color = map[12].NE;
			}
		}else if (heading == WEST){
			if(dir == NORTHEAST){
				color = map[12].NW;
			}else if(dir == NORTHWEST){
				color = map[12].SW;
			}else if(dir == SOUTHEAST){
				color = map[12].NE;
			}else if(dir == SOUTHWEST){
				color = map[12].SE;
			}
		}
	}else if(x == 0 && y == 3){
		if (heading == NORTH){
			if(dir == NORTHEAST){
				color = map[3].NE;
			}else if(dir == NORTHWEST){
				color = map[3].NW;
			}else if(dir == SOUTHEAST){
				color = map[3].SE;
			}else if(dir == SOUTHWEST){
				color = map[3].SW;
			}
		}else if (heading == EAST){
			if(dir == NORTHEAST){
				color = map[3].SE;
			}else if(dir == NORTHWEST){
				color = map[3].NE;
			}else if(dir == SOUTHEAST){
				color = map[3].SW;
			}else if(dir == SOUTHWEST){
				color = map[3].NW;
			}
		}else if (heading == SOUTH){
			if(dir == NORTHEAST){
				color = map[3].SW;
			}else if(dir == NORTHWEST){
				color = map[3].SE;
			}else if(dir == SOUTHEAST){
				color = map[3].NW;
			}else if(dir == SOUTHWEST){
				color = map[3].NE;
			}
		}else if (heading == WEST){
			if(dir == NORTHEAST){
				color = map[3].NW;
			}else if(dir == NORTHWEST){
				color = map[3].SW;
			}else if(dir == SOUTHEAST){
				color = map[3].NE;
			}else if(dir == SOUTHWEST){
				color = map[3].SE;
			}
		}
	}else if(x == 1 && y == 3){
		if (heading == NORTH){
			if(dir == NORTHEAST){
				color = map[8].NE;
			}else if(dir == NORTHWEST){
				color = map[8].NW;
			}else if(dir == SOUTHEAST){
				color = map[8].SE;
			}else if(dir == SOUTHWEST){
				color = map[8].SW;
			}
		}else if (heading == EAST){
			if(dir == NORTHEAST){
				color = map[8].SE;
			}else if(dir == NORTHWEST){
				color = map[8].NE;
			}else if(dir == SOUTHEAST){
				color = map[8].SW;
			}else if(dir == SOUTHWEST){
				color = map[8].NW;
			}
		}else if (heading == SOUTH){
			if(dir == NORTHEAST){
				color = map[8].SW;
			}else if(dir == NORTHWEST){
				color = map[8].SE;
			}else if(dir == SOUTHEAST){
				color = map[8].NW;
			}else if(dir == SOUTHWEST){
				color = map[8].NE;
			}
		}else if (heading == WEST){
			if(dir == NORTHEAST){
				color = map[8].NW;
			}else if(dir == NORTHWEST){
				color = map[8].SW;
			}else if(dir == SOUTHEAST){
				color = map[8].NE;
			}else if(dir == SOUTHWEST){
				color = map[8].SE;
			}
		}
	}else if(x == 2 && y == 3){
		if (heading == NORTH){
			if(dir == NORTHEAST){
				color = map[13].NE;
			}else if(dir == NORTHWEST){
				color = map[13].NW;
			}else if(dir == SOUTHEAST){
				color = map[13].SE;
			}else if(dir == SOUTHWEST){
				color = map[13].SW;
			}
		}else if (heading == EAST){
			if(dir == NORTHEAST){
				color = map[13].SE;
			}else if(dir == NORTHWEST){
				color = map[13].NE;
			}else if(dir == SOUTHEAST){
				color = map[13].SW;
			}else if(dir == SOUTHWEST){
				color = map[13].NW;
			}
		}else if (heading == SOUTH){
			if(dir == NORTHEAST){
				color = map[13].SW;
			}else if(dir == NORTHWEST){
				color = map[13].SE;
			}else if(dir == SOUTHEAST){
				color = map[13].NW;
			}else if(dir == SOUTHWEST){
				color = map[13].NE;
			}
		}else if (heading == WEST){
			if(dir == NORTHEAST){
				color = map[13].NW;
			}else if(dir == NORTHWEST){
				color = map[13].SW;
			}else if(dir == SOUTHEAST){
				color = map[13].NE;
			}else if(dir == SOUTHWEST){
				color = map[13].SE;
			}
		}
	}else if(x == 0 && y == 4){
		if (heading == NORTH){
			if(dir == NORTHEAST){
				color = map[4].NE;
			}else if(dir == NORTHWEST){
				color = map[4].NW;
			}else if(dir == SOUTHEAST){
				color = map[4].SE;
			}else if(dir == SOUTHWEST){
				color = map[4].SW;
			}
		}else if (heading == EAST){
			if(dir == NORTHEAST){
				color = map[4].SE;
			}else if(dir == NORTHWEST){
				color = map[4].NE;
			}else if(dir == SOUTHEAST){
				color = map[4].SW;
			}else if(dir == SOUTHWEST){
				color = map[4].NW;
			}
		}else if (heading == SOUTH){
			if(dir == NORTHEAST){
				color = map[4].SW;
			}else if(dir == NORTHWEST){
				color = map[4].SE;
			}else if(dir == SOUTHEAST){
				color = map[4].NW;
			}else if(dir == SOUTHWEST){
				color = map[4].NE;
			}
		}else if (heading == WEST){
			if(dir == NORTHEAST){
				color = map[4].NW;
			}else if(dir == NORTHWEST){
				color = map[4].SW;
			}else if(dir == SOUTHEAST){
				color = map[4].NE;
			}else if(dir == SOUTHWEST){
				color = map[4].SE;
			}
		}
	}else if(x == 1 && y == 4){
		if (heading == NORTH){
			if(dir == NORTHEAST){
				color = map[9].NE;
			}else if(dir == NORTHWEST){
				color = map[9].NW;
			}else if(dir == SOUTHEAST){
				color = map[9].SE;
			}else if(dir == SOUTHWEST){
				color = map[9].SW;
			}
		}else if (heading == EAST){
			if(dir == NORTHEAST){
				color = map[9].SE;
			}else if(dir == NORTHWEST){
				color = map[9].NE;
			}else if(dir == SOUTHEAST){
				color = map[9].SW;
			}else if(dir == SOUTHWEST){
				color = map[9].NW;
			}
		}else if (heading == SOUTH){
			if(dir == NORTHEAST){
				color = map[9].SW;
			}else if(dir == NORTHWEST){
				color = map[9].SE;
			}else if(dir == SOUTHEAST){
				color = map[9].NW;
			}else if(dir == SOUTHWEST){
				color = map[9].NE;
			}
		}else if (heading == WEST){
			if(dir == NORTHEAST){
				color = map[9].NW;
			}else if(dir == NORTHWEST){
				color = map[9].SW;
			}else if(dir == SOUTHEAST){
				color = map[9].NE;
			}else if(dir == SOUTHWEST){
				color = map[9].SE;
			}
		}
	}else if(x == 2 && y == 4){
		if (heading == NORTH){
			if(dir == NORTHEAST){
				color = map[14].NE;
			}else if(dir == NORTHWEST){
				color = map[14].NW;
			}else if(dir == SOUTHEAST){
				color = map[14].SE;
			}else if(dir == SOUTHWEST){
				color = map[14].SW;
			}
		}else if (heading == EAST){
			if(dir == NORTHEAST){
				color = map[14].SE;
			}else if(dir == NORTHWEST){
				color = map[14].NE;
			}else if(dir == SOUTHEAST){
				color = map[14].SW;
			}else if(dir == SOUTHWEST){
				color = map[14].NW;
			}
		}else if (heading == SOUTH){
			if(dir == NORTHEAST){
				color = map[14].SW;
			}else if(dir == NORTHWEST){
				color = map[14].SE;
			}else if(dir == SOUTHEAST){
				color = map[14].NW;
			}else if(dir == SOUTHWEST){
				color = map[14].NE;
			}
		}else if (heading == WEST){
			if(dir == NORTHEAST){
				color = map[14].NW;
			}else if(dir == NORTHWEST){
				color = map[14].SW;
			}else if(dir == SOUTHEAST){
				color = map[14].NE;
			}else if(dir == SOUTHWEST){
				color = map[14].SE;
			}
		}
	}
    return color; // you may need to remove this
}

/* The main task which initializes data
   and executes the update loop for the particle filters
*/
task main() {

    /* variable declarations */
    intersection map [GWIDTH *GHEIGHT]; /* holds map data */

    int guess[3]; /* holds x y and theta of the best guess at robot's position */
    float robp = 0.0; /* holds the prob that the robot is at guess */
    /* note initializing to just '0' can cause the brick to freeze! */
    intersection rsense; /* holds data about the robot's surroundings */

    particle list[GWIDTH*GHEIGHT*4]; /* list of particles */
    int psense[4]; /* holds data on a particle's surroundings */

    int i, j; /* variables for loop counters */
    int hits, misses; /* keeps track of how many of a particle's
                          surroundings match the robot's */
    float maxprob, totalprob;

    byte fd, charat;         /* variables for file descriptors and characters */
    string line;
    int target, dest; /* records the target and destination coordinates for use
                          after finding the robot's position */

    /* sensor initialization */
    SetSensorColorFull(S1);
    ColorSensorReadType csr;
    csr.Port=S1;
    /* initialize the particle list, each coordinate has 4 particles, one for each direction */
    for (i = 0; i < GWIDTH*GHEIGHT*4; i = i + 4) {
        list[i].x=(i/4)%GWIDTH;
        list[i].y=i/(4*GWIDTH);
        list[i].theta = NORTH;
        list[i].prob = 1.0/(GWIDTH*GHEIGHT*4);
        list[i+1].x=(i/4)%GWIDTH;
        list[i+1].y=i/(4*GWIDTH);
        list[i+1].theta = SOUTH;
        list[i+1].prob = 1.0/(GWIDTH*GHEIGHT*4);
        list[i+2].x=(i/4)%GWIDTH;
        list[i+2].y=i/(4*GWIDTH);
        list[i+2].theta = EAST;
        list[i+2].prob = 1.0/(GWIDTH*GHEIGHT*4);
        list[i+3].x=(i/4)%GWIDTH;
        list[i+3].y=i/(4*GWIDTH);
        list[i+3].theta = WEST;
        list[i+3].prob = 1.0/(GWIDTH*GHEIGHT*4);
    }

    /* Initialize the map and target from file

       The format of the file:
           - one line per intersection containing 4 integers seperated by spaces
             these integers represent NE SE SW NW respectively
           - the order of the intersections is rows top to bottom then columns
             left to right
           - after all the intersection data there are two lines with 2 integers
             each. this describes the target and destination that the robot must
             move to after finding its position

       Colour values are based on NXC colour sensor constants

       Array format:
             - indices are obtained using the formula i = GHEIGHT*x + y
             - each index contains an interesction element that holds information
               about it's surroundings
       */
    fd = fopen(FILENAME, "r");
    /* read as many lines as there are squares on the grid */
    for (i=0; i < GWIDTH*GHEIGHT; i++) {
        fgets(line, READSIZE, fd);
        /* parse each line into the coresponding directions and store them in
           the map data array */
        for (j = 0; j < 7; j = j + 2) {
            charat = StrIndex(line, j);
            switch (j) {
            case 0:
                map[i].NE = ASCII_TO_INT(charat);
                break;
            case 2:
                map[i].SE = ASCII_TO_INT(charat);
                break;
            case 4:
                map[i].SW = ASCII_TO_INT(charat);
                break;
            case 6:
                map[i].NW = ASCII_TO_INT(charat);
                break;
            }
        }
    }
    /* read information about the target which should be following the map data */
    fgets(line, READSIZE, fd);
    charat = StrIndex(line,0);
    target = ASCII_TO_INT(charat);
    target = target*GHEIGHT; // add x coordinate calculation
    charat = StrIndex(line,2);
    target = target + ASCII_TO_INT(charat); // add y coordinate

    /* read information about the destination which should be following the target data */
    fgets(line, READSIZE, fd);
    charat = StrIndex(line,0);
    dest = ASCII_TO_INT(charat);
    dest = dest*GHEIGHT; // add x coordinate calculation
    charat = StrIndex(line,2);
    dest = dest + ASCII_TO_INT(charat); // add y coordinate

    fclose(fd); /* file reading finished */

    /* main execution loop for particle filter

       How particle filters work:

       Normally we would have a list of particles initilalized with random
       coordinates and headings.

       At each iteration of the loop, we move the robot and the particles.
       Surroundings for both are examined and compared.

       Particles which have surroundings that seem close to the robots are given
       larger weights compared to those that don't.

       We then choose from the particles in a way that the particles with higher
       weights are likley to remain, while particles with smaller weights are
       eliminated. This is called resampling.

       After several iterations, we would have a group of particles which is
       very likley to represent the robot's actual position.

       How this implementation works:

       We have simplified the large possibility of coordinates to a grid.

       === Particle List ===
       For each intersection on the grid map, we have four particles.
       One for each of north, east, south, and west, which the robot could be
       facing since it will move vertically and horizontally.
       These are all the possible starting positions and headings the robot could
       have.

       === Updating weights ===
       Each iteration we reassign the particle weights.
       We move the robot, and also the particles.
       The surroundings, being the four coloured squares around the intersection
       are checked. The robot's and particles surroundings are compared and the
       weights are changed based on whether they match or not.

       === Resampling ==
       Take note that we do NOT do resampling in this implementation.
       Instead we have a probability threshold that a particle must pass to be
       accepted as the actual position of the robot.

    */

    ////////////////////////////////////////////////////////////////////////////
    // TO DO:
    //
    // Write the main particle filter execution loop. It will loop until a
    // particle weight reaches the threshold
    //
    // At each iteration the following steps take place:
    //    - robot checks its surroundings
    //    - each particle checks its surroundings and compares them with the
    //      robot's
    //    - adjust particle weights based on the comparison
    //    - normalize particle weights
    //    - find the best guess of the robot's position
    //    - move the robot and the particles forward to the next intersection
    //
    /////////////////////////////////////////////////////////////////////////////

	while (robp < THRESHOLD){ /* run the update loop until an acceptable
								probability is obtained */
		/* implement me */
		intersection robot_scan = scan_surroundings(csr);
		for (i = 0; i < GWIDTH*GHEIGHT*4; i++) {
			int p_NE_N_Color = senseParticle(map, list[i].x, list[i].y, NORTHEAST, NORTH);
			if (robot_scan.NE == p_NE_N_Color){
				int p_NW_N_Color = senseParticle(map, list[i].x, list[i].y, NORTHWEST, NORTH);
				list[i].prob = list[i].prob + 0.10;
				if (robot_scan.NW == p_NW_N_Color){
					int p_SE_N_Color = senseParticle(map, list[i].x, list[i].y, SOUTHEAST, NORTH);
					list[i].prob = list[i].prob + 0.10;
					if (robot_scan.SE == p_SE_N_Color){
						int p_SW_N_Color = senseParticle(map, list[i].x, list[i].y, SOUTHWEST, NORTH);
						list[i].prob = list[i].prob + 0.10;
						if(robot_scan.SW == p_SW_N_Color){
							list[i].prob = list[i].prob + 0.10;
						}
					}
				}
			}
			int p_NE_E_Color = senseParticle(map, list[i].x, list[i].y, NORTHEAST, EAST);
			if (robot_scan.NE == p_NE_E_Color){
				int p_NW_E_Color = senseParticle(map, list[i].x, list[i].y, NORTHWEST, EAST);
				list[i].prob = list[i].prob + 0.10;
				if (robot_scan.NW == p_NW_E_Color){
					int p_SE_E_Color = senseParticle(map, list[i].x, list[i].y, SOUTHEAST, EAST);
					list[i].prob = list[i].prob + 0.10;
					if (robot_scan.SE == p_SE_E_Color){
						int p_SW_E_Color = senseParticle(map, list[i].x, list[i].y, SOUTHWEST, EAST);
						list[i].prob = list[i].prob + 0.10;
						if(robot_scan.SW == p_SW_E_Color){
							list[i].prob = list[i].prob + 0.10;
						}
					}
				}
			}
			int p_NE_S_Color = senseParticle(map, list[i].x, list[i].y, NORTHEAST, SOUTH);
			if (robot_scan.NE == p_NE_S_Color){
				int p_NW_S_Color = senseParticle(map, list[i].x, list[i].y, NORTHWEST, SOUTH);
				list[i].prob = list[i].prob + 0.10;
				if (robot_scan.NW == p_NW_S_Color){
					int p_SE_S_Color = senseParticle(map, list[i].x, list[i].y, SOUTHEAST, SOUTH);
					list[i].prob = list[i].prob + 0.10;
					if (robot_scan.SE == p_SE_S_Color){
						int p_SW_S_Color = senseParticle(map, list[i].x, list[i].y, SOUTHWEST, SOUTH);
						list[i].prob = list[i].prob + 0.10;
						if(robot_scan.SW == p_SW_S_Color){
							list[i].prob = list[i].prob + 0.10;
						}
					}
				}
			}
			int p_NE_W_Color = senseParticle(map, list[i].x, list[i].y, NORTHEAST, WEST);
			if (robot_scan.NE == p_NE_W_Color){
				int p_NW_W_Color = senseParticle(map, list[i].x, list[i].y, NORTHWEST, WEST);
				list[i].prob = list[i].prob + 0.10;
				if (robot_scan.NW == p_NW_W_Color){
					int p_SE_W_Color = senseParticle(map, list[i].x, list[i].y, SOUTHEAST, WEST);
					list[i].prob = list[i].prob + 0.10;
					if (robot_scan.SE == p_SE_W_Color){
						int p_SW_W_Color = senseParticle(map, list[i].x, list[i].y, SOUTHWEST, WEST);
						list[i].prob = list[i].prob + 0.10;
						if(robot_scan.SW == p_SW_W_Color){
							list[i].prob = list[i].prob + 0.10;
						}
					}
				}
			}
		}
		int sum = 0;
		for (i = 0; i < GWIDTH*GHEIGHT*4; i++) {
			sum += list[i].prob;
		}
		for (i = 0; i < GWIDTH*GHEIGHT*4; i++) {
			list[i].prob = (list[i].prob / sum);
		}
		particle highest_prob = list[0];
		for (i = 0; i < GWIDTH*GHEIGHT*4; i++) {
			if(list[i].prob > highest_prob.prob){
				highest_prob = list[i];
			}
		}
		guess[0] = highest_prob.x;
		guess[1] = highest_prob.y;
		guess[2] = highest_prob.theta;
		robp = highest_prob.theta;
		move_robot(csr);
		for (i = 0; i < GWIDTH*GHEIGHT*4; i++) {
			move(list[i]);
		}
	}
	PlayToneEx(Random(1000) + 200, 400, 3, FALSE);

	/* When we are fairly confident about the robot's position, we can move on
		to the task given.

		Once the robot has found where it is, it must head to the target given in
		the initialization file.
		After picking up the confidential item at the target point it must move to
		the delivery point given by the destination from the initialization file.

	*/
	// guess[0] = 2; guess[1] = 0; guess[2] = NORTH; // for testing
	guess[2] = move_to(guess[0]*GHEIGHT + guess[1], target, guess[2], csr);
	PlayToneEx(Random(1000) + 200, 400, 3, FALSE);
	TextOut(0,LCD_LINE1,"--P-R-A-I-S-E--T-H-E---");
	TextOut(0,LCD_LINE2,"--\----__-----__----/--");
	TextOut(0,LCD_LINE3,"---\---|._____.|---/---");
	TextOut(0,LCD_LINE4,"----\--|   |   |--/----");
	TextOut(0,LCD_LINE5,"-----\-|   |   |-/-----");
	TextOut(0,LCD_LINE6,"------\|_..|.._|/------");
	TextOut(0,LCD_LINE7,"---------S-U-N---------");
	Wait(5000);
	ClearScreen();
	move_to(target, dest,guess[2], csr);
	PlayToneEx(Random(1000) + 200, 400, 3, FALSE);
	TextOut(0,LCD_LINE1,"--P-R-A-I-S-E--T-H-E---");
	TextOut(0,LCD_LINE2,"--\----__-----__----/--");
	TextOut(0,LCD_LINE3,"---\---|._____.|---/---");
	TextOut(0,LCD_LINE4,"----\--|   |   |--/----");
	TextOut(0,LCD_LINE5,"-----\-|   |   |-/-----");
	TextOut(0,LCD_LINE6,"------\|_..|.._|/------");
	TextOut(0,LCD_LINE7,"---------S-U-N---------");
	Wait(5000);
}