Colony

/*

Colony BFS

Felix Duvallet

*/

#include <stdio.h>

#include <string.h>

#include <firefly+_lib.h>

#include <ring_buffer.h>

#include "colonybfs.h"

#define NO_VAL 0

RING_BUFFER_NEW(Queue, MAX_ROBOTS, char, q);

/*

colony_bfs_find_leader -- given an adjacency matrix, with a start and end

    robots, this will tell you which robot to follow next.  If no path exists,

    it will return -1 (which may or may not go to 255 on the unsigned side

    of things -- be wary).

    It does find the entire path, perhaps someday I will make two function,

    where find_leader calls bfs(..) and only uses the first element.

    I ported this code from a MATLAB test file (also in the libs_general folder),

    and this has been tested on September 15, 2006 by Felix

    Todo: change NO_VAL to 255 and reflect those changes in the localization code

Author: Felix Duvallet

        CMU Robotics Club, Colony Project

*/

char colony_bfs_find_leader ( unsigned char matrix[MAX_ROBOTS][MAX_ROBOTS], 

                                unsigned char start, unsigned char end ) {

/*

//pseudocode for BFS

procedure bfs(v)

    q := make_queue()

    enqueue(q,v)

    mark v as visited

    while q is not empty

        v = dequeue(q)

        process v

        for all unvisited vertices v' adjacent to v

            mark v' as visited

            enqueue(q,v')

*/

    // keep track of which nodes you have visited.  Indexed by robot #

    // 1 if visited, 0 if not

    unsigned char visited_nodes[MAX_ROBOTS];

    // keep track of the distance from the start robot to other robots

    // also indexed by robot#

    unsigned char node_distances[MAX_ROBOTS];

    // this is the path you take

    unsigned char path[MAX_ROBOTS];

    //variables you will need

    unsigned char current_node;                 //current node

    unsigned char current_neighbour_node;       //neighbor to the current node

    unsigned char current_neighbour_val;        //value of that neighbour's sensors

    unsigned char current_distance;              //keep track of current distance to the start

    unsigned char i;                            //counter variable

    char large_number = 255;

    //set visited nodes to all 0

    memset(visited_nodes, 0, MAX_ROBOTS);

    //set all the distances to a very large number

    //this isn't technically necessary, but it's probably a good thing -- just in case

    memset(node_distances, large_number, MAX_ROBOTS);

    //set the path to all LARGE_NUMBER as well

    memset(path, large_number, MAX_ROBOTS);

    RING_BUFFER_CLEAR(q);

    //add the start node

    RING_BUFFER_ADD(q, start);

    visited_nodes[start] = 1;

    node_distances[start] = 0;

    while(!RING_BUFFER_EMPTY(q)){

        RING_BUFFER_REMOVE(q, current_node);

        //get the current distance from you to the start

        current_distance = node_distances[current_node];

        //this node is on your 'path'

        path[current_distance] = current_node;

        //note: it's OK if this path leads nowhere -- the path will be

        //overwritten by a node that does lead to the goal

        //(if there is no path, we set path to null anyways)

        //from now on, all further nodes will be one further away -- increment

        current_distance++;

        //process node -- basically check if it's the goal

        if(current_node == end) {

            //you reach the goal

            //return the first robot in the path, which is the one

            //to follow

            lcd_putchar('.');

            lcd_putchar('.');

            for(i = 0; i < MAX_ROBOTS; i++)

                lcd_putchar(path[i] +48);           //print out the path for fun -- remove this later

            lcd_putchar('.');

            lcd_putchar('.');

            return path[1];         //path[0] is you.  path[1] is who you want to follow

        }

        // go through all nodes adjacent to current

        // in effect, this means going through the current node's row in the matrix

        for(i = 0; i < MAX_ROBOTS; i++) {

            //the robot # is actually just the index

            current_neighbour_node = i;

            //the value is what is stored in the matrix (this is important)

            current_neighbour_val = matrix[current_node][i];

            //check for connected-ness and that it was not visited

            //            unconnected                           visited

            if( (current_neighbour_val == NO_VAL) || (visited_nodes[current_neighbour_node]) ) {

                //if it is either unconnected or previously visited, exit

                continue;   //go to the next node

            }

            //update the distance from the start node to this node

            node_distances[current_neighbour_node] = current_distance;

            //also mark it as visited

            visited_nodes[current_neighbour_node] = 1;

            //also enqueue each neighbour

            RING_BUFFER_ADD(q, current_neighbour_node);

        }

    }

    //if we get to here, there is no path

    memset(path, 0, MAX_ROBOTS);

    return -1;

}