Colony

#include <wl_token_ring.h>

#include <stdlib.h>

#include <stdio.h>

#include <wl_defs.h>

#include <wireless.h>

#include <sensor_matrix.h>

#include <queue.h>

#ifdef ROBOT

#include <bom.h>

#include <time.h>

#endif

#define DEFAULT_SENSOR_MATRIX_SIZE 20

/*Ring States*/

#define NONMEMBER 0

#define MEMBER 1

#define JOINING 2

#define ACCEPTED 3

#define LEAVING 4

/*Frame Types*/

#define TOKEN_JOIN_ACCEPT_FRAME 1

/*Function Prototypes*/

/*Wireless Library Prototypes*/

void wl_token_ring_timeout_handler(void);

void wl_token_ring_response_handler(int frame, int received);

void wl_token_ring_receive_handler(char type, int source, unsigned char* packet,

                                                        int length);

void wl_token_ring_cleanup(void);

/*Helper Functions*/

void wl_token_pass_token(void);

int get_token_distance(int robot1, int robot2);

void wl_token_get_token(void);

/*Packet Handling Routines*/

void wl_token_pass_receive(int source, char nextRobot, unsigned char* sensorData, int sensorDataLength);

void wl_token_interrupt_request_receive(int source, int robot);

void wl_token_interrupt_pass_receive(int source, int robot);

void wl_token_bom_on_receive(int source);

void wl_token_join_receive(int source);

void wl_token_join_accept_receive(int source);

/*Global Variables*/

//the sensor matrix

SensorMatrix* sensorMatrix;

//the robot we are waiting to say it has received the token. -1 if unspecified

int wl_token_next_robot = -1;

//true if the robot should be in the token ring, 0 otherwise

int ringState = NONMEMBER;

//the id of the robot who accepted us into the token ring, only used in ACCEPTED state

int acceptor = -1;

//id of the robot we are accepting

int accepted = -1;

//the counter for when we assume a robot is dead

int deathDelay = -1;

//the counter for joining, before we form our own token ring

int joinDelay = -1;

//queue containing ids of interruption requests

Queue* interrupting = NULL;

#ifdef ROBOT

void (*bom_on_function) (void) = bom_on;

void (*bom_off_function) (void) = bom_off;

int (*get_max_bom_function) (void) = get_max_bom;

#else

void do_nothing(void) {}

int get_nothing(void) {return -1;}

void (*bom_on_function) (void) = do_nothing;

void (*bom_off_function) (void) = do_nothing;

int (*get_max_bom_function) (void) = get_nothing;

#endif

PacketGroupHandler wl_token_ring_handler =

                {WL_TOKEN_RING_GROUP, wl_token_ring_timeout_handler,

                wl_token_ring_response_handler, wl_token_ring_receive_handler,

                wl_token_ring_cleanup};

/**

 * Initialize the token ring packet group and register it with the

 * wireless library. The robot will not join a token ring.

 **/

void wl_token_ring_register()

{

        if (wl_get_xbee_id() > 0xFF)

        {

                //Note: if this becomes an issue (unlikely), we could limit sensor information

                //to half a byte and use 12 bits for the id

                WL_DEBUG_PRINT("XBee ID must be single byte for token ring, is ");

                WL_DEBUG_PRINT_INT(wl_get_xbee_id());

                WL_DEBUG_PRINT(".\r\n");

                return;

        }

        sensorMatrix = sensor_matrix_create();

        interrupting = queue_create();

        //add ourselves to the sensor matrix

        sensor_matrix_set_in_ring(sensorMatrix, wl_get_xbee_id(), 0);

        wl_register_packet_group(&wl_token_ring_handler);

}

/**

 * Removes the packet group from the wireless library.

 **/

void wl_token_ring_unregister()

{

        wl_unregister_packet_group(&wl_token_ring_handler);

}

/**

 * Sets the functions that are called when the BOM ought to be

 * turned on or off. This could be used for things such as 

 * charging stations, which have multiple BOMs.

 *

 * @param on_function the function to be called when the BOM

 * should be turned on

 * @param off_function the function to be called when the BOM

 * should be turned off

 * @param max_bom_function the function to be called when a

 * measurement of the maximum BOM reading is needed.

 **/

void wl_token_ring_set_bom_functions(void (*on_function) (void),

        void (*off_function) (void), int (*max_bom_function) (void))

{

        bom_on_function = on_function;

        bom_off_function = off_function;

        get_max_bom_function = max_bom_function;

}

/**

 * Called to cleanup the token ring packet group.

 **/

void wl_token_ring_cleanup()

{

        sensor_matrix_destroy(sensorMatrix);

        queue_destroy(interrupting);

}

/**

 * Called approximately every quarter second by the wireless library.

 **/

void wl_token_ring_timeout_handler()

{

        //someone is not responding, assume they are dead

        if (deathDelay == 0)

        {

                //pass the token to the next robot if we think someone has died

                //also, declare that person dead, as long as it isn't us

                if (wl_token_next_robot != wl_get_xbee_id())

                {

                        sensor_matrix_set_in_ring(sensorMatrix, wl_token_next_robot, 0);

                        WL_DEBUG_PRINT("Robot ");

                        WL_DEBUG_PRINT_INT(wl_token_next_robot);

                        WL_DEBUG_PRINT(" has died.\r\n");

                }

                // we may have been dropped from the ring when this is received

                if (ringState == MEMBER)

                        wl_token_pass_token();

        }

        //we must start our own token ring, no one is responding to us

        if (joinDelay == 0)

        {

                if (sensor_matrix_get_joined(sensorMatrix) == 0)

                {

                        WL_DEBUG_PRINT("Creating our own token ring, no robots seem to exist.\r\n");

                        sensor_matrix_set_in_ring(sensorMatrix, wl_get_xbee_id(), 1);

                        ringState = MEMBER;

                        //this will make us pass the token to ourself

                        //repeatedly, and other robots when they join

                        deathDelay = DEATH_DELAY;

                        wl_token_next_robot = wl_get_xbee_id();

                }

                else

                {

                        WL_DEBUG_PRINT("Attempting to join the token ring again.\r\n");

                        //attempt to rejoin with a random delay

                        wl_token_ring_join();

                        joinDelay = rand() / (RAND_MAX / JOIN_DELAY) + 1;

                }

        }

        if (deathDelay >= 0)

                deathDelay--;

        if (joinDelay >= 0)

                joinDelay--;

}

/**

 * Called when the XBee tells us if a packet we sent has been received.

 * 

 * @param frame the frame number assigned when the packet was sent

 * @param received 1 if the packet was received, 0 otherwise

 **/

void wl_token_ring_response_handler(int frame, int received)

{

        if (!received)

        {

                WL_DEBUG_PRINT("FAILED.\r\n");

        }

}

/**

 * Called when we recieve a token ring packet.

 * @param type the type of the packet

 * @param source the id of the robot who sent the packet

 * @param packet the data in the packet

 * @param length the length of the packet in bytes

 **/

void wl_token_ring_receive_handler(char type, int source, unsigned char* packet,

                                                        int length)

{

        switch (type)

        {

                case WL_TOKEN_PASS:

                        if (length < 1)

                        {

                                WL_DEBUG_PRINT("Malformed Token Pass packet received.\r\n");

                                return;

                        }

                        wl_token_pass_receive(source, packet[0], packet + 1, length - 1);

                        break;

                case WL_TOKEN_BOM_ON:

                        //add the robot to the sensor matrix if it is not already there

                        wl_token_bom_on_receive(source);

                        break;

                case WL_TOKEN_INTERRUPT_REQUEST:

                        wl_token_interrupt_request_receive(source, packet[0]);

                        break;

                case WL_TOKEN_INTERRUPT_PASS:

                        wl_token_interrupt_pass_receive(source, packet[0]);

                        break;

                case WL_TOKEN_JOIN:

                        wl_token_join_receive(source);

                        break;

                case WL_TOKEN_JOIN_ACCEPT:

                        wl_token_join_accept_receive(source);

                        break;

                default:

                        WL_DEBUG_PRINT("Unimplemented token ring packet received.\r\n");

                        break;

        }

}

/**

 * Causes the robot to join an existing token ring, or create one

 * if no token ring exists. The token ring uses global and robot to robot

 * packets, and does not rely on any PAN.

 **/

void wl_token_ring_join()

{

        WL_DEBUG_PRINT("Joining the token ring.\r\n");

        ringState = JOINING;

        joinDelay = JOIN_DELAY;

        wl_send_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_JOIN,

                NULL, 0, 0);

}

/**

 * Causes the robot to leave the token ring. The robot stops

 * alerting others of its location, but continues storing the

 * locations of other robots.

 **/

void wl_token_ring_leave()

{

        ringState = LEAVING;

}

/**

 * Requests that the specified robot be given the token and

 * allowed to flash its BOM. After its BOM is flashed, the

 * token will return to the robot who sent it.

 *

 * @param robot the ID of the robot which should flash its BOM

 **/

void wl_token_request(int robot)

{

        char buf[1];

        buf[0] = robot;

        wl_send_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_INTERRUPT_REQUEST,

                buf, 1, 0);

}

/**

 * Returns the BOM reading robot source has for robot dest.

 *

 * @param source the robot that made the BOM reading

 * @param dest the robot whose relative location is returned

 *

 * @return a BOM reading from robot source to robot dest,

 * in the range 0-15, or -1 if it is unknown

 **/

int wl_token_get_sensor_reading(int source, int dest)

{

        return sensor_matrix_get_reading(sensorMatrix, source, dest);

}

/**

 * Returns the BOM reading we have for robot dest.

 * 

 * @param dest the robot whose relative location is returned

 *

 * @return a BOM reading from us to robot dest, in the range

 * 0-15, or -1 if it is unkown

 **/

int wl_token_get_my_sensor_reading(int dest)

{

        return wl_token_get_sensor_reading(wl_get_xbee_id(), dest);

}

/**

 * This method is called when we receive a token pass packet.

 * @param source is the robot it came from

 * @param nextRobot is the robot the token was passed to

 * @param sensorData a char with an id followed by a char with the sensor

 *                reading for that robot, repeated for sensorDataLength bytes

 * @param sensorDataLength the length in bytes of sensorData

 */

void wl_token_pass_receive(int source, char nextRobot, unsigned char* sensorData, int sensorDataLength)

{

        int i, j;

        deathDelay = -1;

        WL_DEBUG_PRINT("Received the token, next robot is ");

        WL_DEBUG_PRINT_INT((int)nextRobot);

        WL_DEBUG_PRINT(" \r\n");

        sensor_matrix_set_in_ring(sensorMatrix, source, 1);

        //with this packet, we are passed the id of the next robot in the ring

        //and the sensor matrix, a list of id and sensor reading pairs (two bytes for both)

        j = 0;

        for (i = 0; i < sensor_matrix_get_size(sensorMatrix); i++)

        {

                if (i == source)

                        continue;

                //set the sensor information we receive

                if (j < sensorDataLength / 2 && sensorData[2 * j] == i)

                {

                        //the robot we were going to accept has already been accepted

                        if (accepted == i)

                        {

                                accepted = -1;

                                WL_DEBUG_PRINT("Someone accepted the robot we did.\r\n");

                        }

                        sensor_matrix_set_reading(sensorMatrix, source, i,

                                                sensorData[2 * j + 1]);

                        sensor_matrix_set_in_ring(sensorMatrix, i, 1);

                        j++;

                }

                else

                {

                        if (sensor_matrix_get_in_ring(sensorMatrix, i))

                        {

                                WL_DEBUG_PRINT("Robot ");

                                WL_DEBUG_PRINT_INT(i);

                                WL_DEBUG_PRINT(" has been removed from the sensor matrix of robot ");

                                WL_DEBUG_PRINT_INT(wl_get_xbee_id());

                                WL_DEBUG_PRINT(" due to a packet from robot ");

                                WL_DEBUG_PRINT_INT(source);

                                WL_DEBUG_PRINT(".\r\n");

                                sensor_matrix_set_in_ring(sensorMatrix, i, 0);

                        }

                        if (i == wl_get_xbee_id() && ringState == MEMBER)

                        {

                                ringState = NONMEMBER;

                                wl_token_ring_join();

                                WL_DEBUG_PRINT("We have been removed from the ring ");

                                WL_DEBUG_PRINT("and are rejoining.\r\n");

                        }

                        //the person who accepted us is dead... let's ask again

                        if (i == acceptor)

                        {

                                sensor_matrix_set_in_ring(sensorMatrix,

                                                wl_get_xbee_id(), 1);

                                ringState = NONMEMBER;

                                acceptor = -1;

                                wl_token_ring_join();

                        }

                }

        }

        wl_token_next_robot = nextRobot;

        deathDelay = get_token_distance(wl_get_xbee_id(), nextRobot) * DEATH_DELAY;

        //we have the token

        if (wl_token_next_robot == wl_get_xbee_id())

                wl_token_get_token();

}

/**

 * Gets the distance in the token ring between two robots.

 *

 * @param robot1 the first robot

 * @param robot2 the second robot

 *

 * @return the number of passes before the token is expected

 * to reach robot2 from robot1

 **/

int get_token_distance(int robot1, int robot2)

{

        int curr = robot1 + 1;

        int count = 1;

        while (1)

        {

                if (curr == sensor_matrix_get_size(sensorMatrix))

                        curr = 0;

                if (curr == robot2)

                        break;

                if (sensor_matrix_get_in_ring(sensorMatrix, curr))

                        count++;

                curr++;

        }

        return count;

}

/**

 * Passes the token to the next robot in the token ring.

 **/

void wl_token_pass_token()

{

        char nextRobot;

        int i = wl_get_xbee_id() + 1;

        if (accepted == -1)

        {

                while (1)

                {

                        if (i == sensor_matrix_get_size(sensorMatrix))

                                i = 0;

                        if (sensor_matrix_get_in_ring(sensorMatrix, i))

                        {

                                nextRobot = (char)i;

                                break;

                        }

                        i++;

                }

        }

        else

        {

                WL_DEBUG_PRINT("Accepting new robot, sending it the token.\r\n");

                //add a new robot to the token ring

                sensor_matrix_set_in_ring(sensorMatrix, accepted, 1);

                nextRobot = accepted;

                accepted = -1;

        }

        //we don't include ourself

        int packetSize = 1 + 2 * (sensor_matrix_get_joined(sensorMatrix) - 1);

        char* buf = (char*)malloc(packetSize * sizeof(char));

        if (!buf)

        {

                WL_DEBUG_PRINT_INT(packetSize);

                WL_DEBUG_PRINT("Out of memory - pass token.\r\n");

                return;

        }

        buf[0] = nextRobot;

        int j = 0;

        for (i = 0; i < sensor_matrix_get_size(sensorMatrix); i++)

                if (sensor_matrix_get_in_ring(sensorMatrix, i) && i != wl_get_xbee_id())

                {

                        buf[2*j + 1] = i;

                        buf[2*j + 2] = sensor_matrix_get_reading(sensorMatrix, wl_get_xbee_id(), i);

                        j++;

                }

        WL_DEBUG_PRINT("Passing the token to robot ");

        WL_DEBUG_PRINT_INT(buf[0]);

        WL_DEBUG_PRINT(".\r\n");

        wl_send_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_PASS,

                buf, packetSize, 3);

        wl_token_next_robot = nextRobot;

        deathDelay = DEATH_DELAY;

        free(buf);

}

/**

 * Called when a packet is received stating that another robot has turned

 * its BOM on. Our BOM is then read, and the data is added to the sensor

 * matrix.

 *

 * @param source the robot whose BOM is on

 **/

void wl_token_bom_on_receive(int source)

{

        WL_DEBUG_PRINT("Robot ");

        WL_DEBUG_PRINT_INT(source);

        WL_DEBUG_PRINT(" has flashed its bom.\r\n");

        sensor_matrix_set_reading(sensorMatrix, wl_get_xbee_id(), 

                source, get_max_bom_function());

}

/**

 * This method is called when we receive the token. Upon receiving

 * the token, we must send a BOM_ON packet, flash the BOM, and send

 * the token to the next robot.

 * 

 * If there is a pending request for the token, this is processed first.

 **/

void wl_token_get_token()

{

        WL_DEBUG_PRINT("We have the token.\r\n");

        if (ringState == ACCEPTED)

        {

                sensor_matrix_set_in_ring(sensorMatrix,

                        wl_get_xbee_id(), 1);

                WL_DEBUG_PRINT("Now a member of the token ring.\r\n");

                ringState = MEMBER;

        }

        if (ringState == LEAVING || ringState == NONMEMBER)

        {

                sensor_matrix_set_in_ring(sensorMatrix, wl_get_xbee_id(), 0);

                if (ringState == NONMEMBER)

                {

                        WL_DEBUG_PRINT("We should have left the token ring, but didn't.\r\n");

                }

                return;

        }

        //check for interruption requests

        if (queue_size(interrupting) > 0)

        {

                char buf[1];

                buf[0] = (char)(int)queue_remove(interrupting);

                //in case this robot has requested multiple times

                queue_remove_all(interrupting, (void*)(int)buf[0]);

                wl_send_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_INTERRUPT_PASS,

                        buf, 1, 0);

                deathDelay = DEATH_DELAY;

                wl_token_next_robot = buf[0];

                return;

        }

        WL_DEBUG_PRINT("Our BOM has been flashed.\r\n");

        wl_send_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_BOM_ON,

                NULL, 0, 0);

        bom_on_function();

        #ifdef ROBOT

        delay_ms(BOM_DELAY);

        #endif

        bom_off_function();

        if (!sensor_matrix_get_in_ring(sensorMatrix, wl_get_xbee_id()))

        {

                WL_DEBUG_PRINT("Removed from sensor matrix while flashing BOM.\r\n");

        }

        wl_token_pass_token();

}

/**

 * Called when a request to join the token ring is received.

 * If we are the robot preceding the requester in the ring,

 * we respond with a JOIN_ACCEPT packet and pass the token to

 * this robot when we receive the token.

 *

 * @param source the robot who requested to join

 **/

void wl_token_join_receive(int source)

{

        WL_DEBUG_PRINT("Received joining request from robot ");

        WL_DEBUG_PRINT_INT(source);

        WL_DEBUG_PRINT(".\r\n");

        //we cannot accept the request if we are not a member

        if (ringState != MEMBER)

                return;

        //if they didn't get our response, see if we should respond again

        if (accepted == source)

                accepted = -1;

        //we can only accept one request at a time

        if (accepted != -1)

                return;

        //check if we are the preceding robot in the token ring

        int i = source - 1;

        while (1)

        {

                if (i < 0)

                        i = sensor_matrix_get_size(sensorMatrix) - 1;

                //we must send a join acceptance

                if (i == wl_get_xbee_id())

                        break;

                //another robot will handle it

                if (sensor_matrix_get_in_ring(sensorMatrix, i))

                        return;

                i--;

        }

        accepted = source;

        wl_send_robot_to_robot_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_JOIN_ACCEPT,

                NULL, 0, source, TOKEN_JOIN_ACCEPT_FRAME);

        WL_DEBUG_PRINT("Accepting robot ");

        WL_DEBUG_PRINT_INT(source);

        WL_DEBUG_PRINT(" into the token ring.\r\n");

        joinDelay = -1;

        // the token ring has not started yet

        if (sensor_matrix_get_joined(sensorMatrix) == 1)

                wl_token_pass_token();

}

/**

 * Called when we receive a JOIN_ACCEPT packet in attempting to join

 * the token ring.

 * Our attempt to join the ring is stopped, and we wait for the token.

 *

 * @param source the robot who accepted us

 **/

void wl_token_join_accept_receive(int source)

{

        WL_DEBUG_PRINT("Accepted into the token ring by robot ");

        WL_DEBUG_PRINT_INT(source);

        WL_DEBUG_PRINT(".\r\n");

        joinDelay = -1;

        ringState = ACCEPTED;

        acceptor = source;

        //add ourselves to the token ring

        sensor_matrix_set_in_ring(sensorMatrix, wl_get_xbee_id(), 1);

}

/**

 * Called when we receive a packet passing the token and interrupting

 * the token ring.

 * If the token has been passed to us, we flash our BOM

 * and pass it back.

 *

 * @param source the robot who sent the interrupt packet

 * @param robot the robot the token has been passed to

 **/

void wl_token_interrupt_pass_receive(int source, int robot)

{

        if (wl_get_xbee_id() != robot)

        {

                queue_remove_all(interrupting, (void*)robot);

                wl_token_next_robot = robot;

                deathDelay = DEATH_DELAY + rand() / (RAND_MAX / (2 * DEATH_DELAY));

                return;

        }

        wl_send_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_BOM_ON,

                NULL, 0, 0);

        bom_on_function();

        #ifdef ROBOT

        delay_ms(BOM_DELAY);

        #endif

        bom_off_function();

        //we don't include ourself, only if we are in the ring

        int packetSize = 1 + 2 * (sensor_matrix_get_joined(sensorMatrix) - 1);

        if (!sensor_matrix_get_in_ring(sensorMatrix, wl_get_xbee_id()))

                packetSize += 2;

        char* buf = (char*)malloc(packetSize * sizeof(char));

        if (!buf)

        {

                WL_DEBUG_PRINT("Out of memory - pass_receive.\r\n");

                return;

        }

        //return the token to where we got it from

        buf[0] = source;

        int i = 0, j = 0;

        for (i = 0; i < sensor_matrix_get_size(sensorMatrix); i++)

                if (sensor_matrix_get_in_ring(sensorMatrix, i) && i != wl_get_xbee_id())

                {

                        buf[2*j + 1] = i;

                        buf[2*j + 2] = sensor_matrix_get_reading(sensorMatrix, wl_get_xbee_id(), i);

                        j++;

                }

        wl_send_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_PASS,

                buf, packetSize, 0);

        wl_token_next_robot = source;

        deathDelay = DEATH_DELAY;

        free(buf);

}

/**

 * Called when we receive a request to interrupt the token ring.

 * We add the robot to our list of interrupt requests,

 * and will send the token to this robot when we next receive the

 * token, unless someone else does so first.

 *

 * @param source the robot requesting interruption

 * @param robt the robot requested to interrupt the token ring

 **/

void wl_token_interrupt_request_receive(int source, int robot)

{

        queue_add(interrupting, (void*)robot);

}