Colony

#include "charging.h"

#include "charging_defs.h"

#include "bays.h"

#include <stdlib.h>

#include <serial.h>

#include <wl_token_ring.h>

#include "wl_charging_station.h"

#define BATTERY_LOWV 152

//set this to the number of bays

#define NUM_BAYS 2

//Function prototypes

void charging_bom_on(void);

void charging_bom_off(void);

int charging_bom_read(void);

//Structures

typedef struct

{

        int state;

        int bay;

        int verifyCount;

        int battery;

} ChargingRobot;

ChargingRobot** robots;

int robotsSize = 0;

int numRobots = 0;

int iteratorPos = 0;

int bays[NUM_BAYS];

int is_seeking = 0;

/**

 * Call this function to initialize charging.

 * wl_token_ring_register must be called before

 * this function.

 **/

void charging_init(void)

{

        robots = malloc(robotsSize * sizeof(ChargingRobot*));

        if (!robots)

        {

                STATION_DEBUG_PRINT("Out of memory.\n");

                return;

        }

        int i;

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

                robots[i] = NULL;

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

                bays[i] = -1;

        bays_init();

        // call our own special function to set the BOM

        wl_token_ring_set_bom_functions(charging_bom_on,

                charging_bom_off, charging_bom_read);

        wl_station_register();

}

/**

 * Initializes the iterator through the robots

 * which are using the charging station.

 **/

void charging_robot_iterator_init(void)

{

        iteratorPos = 0;

}

/**

 * Returns zero if the iterator has gone through

 * all robots that are using the charging station

 * since the last call to charging_robot_iterator_init,

 * nonzero otherwise.

 *

 * @return if the iterator has gone through all the robots

 **/

int charging_robot_iterator_has_next(void)

{

        while (iteratorPos < robotsSize)

        {

                if (robots[iteratorPos] != NULL)

                        return 1;

                iteratorPos++;

        }

        return 0;

}

/**

 * Returns the id of the next robot in the iterator

 * of robots that are using the charging station.

 *

 * @return the id of a robot using the station

 **/

int charging_robot_iterator_next(void)

{

        while (iteratorPos < robotsSize)

        {

                if (robots[iteratorPos] != NULL)

                {

                        iteratorPos++;

                        return iteratorPos - 1;

                }

        }

        return -1;

}

/**

 * Return the verification count for a robot

 * using the station. If the count is zero,

 * then the robot is probably dead.

 *

 * @param robot the id of the robot to check

 * 

 * @return the verify count for robot robot

 **/

int charging_get_verify_count(int robot)

{

        if (robot >= robotsSize || robot < 0)

        {

                STATION_DEBUG_PRINT("Index out of range.\n");

                return -1;

        }

        if (robots[robot] == NULL)

        {

                STATION_DEBUG_PRINT("Robot not known to charging station.\n");

                return -1;

        }

        return robots[robot]->verifyCount;

}

/**

 * Sets the verify count for the specified robot.

 *

 * @param robot the robot to specify the count for

 * @param count the new verify count

 **/

void charging_set_verify_count(int robot, int count)

{

        if (robot >= robotsSize || robot < 0)

        {

                STATION_DEBUG_PRINT("Index out of range.\n");

                return;

        }

        if (robots[robot] == NULL)

        {

                STATION_DEBUG_PRINT("Robot not known to charging station.\n");

                return;

        }

        robots[robot]->verifyCount = count;

}

/**

 * Called when charging has been cancelled for a 

 * specific robot.

 *

 * @param robot the robot to cancel charging for

 **/

void charging_cancel(int robot)

{

        if (robot < 0 || robot >= robotsSize || robots[robot] == NULL)

        {

                STATION_DEBUG_PRINT("Attempted to cancel charging for an ");

                STATION_DEBUG_PRINT("unknown robot.\n");

                return;

        }

        STATION_DEBUG_PRINT("Cancelling charging for robot ");

        STATION_DEBUG_PUTI(robot);

        STATION_DEBUG_PRINT(".\n");

        //allow other robots to seek

        if (robots[robot]->state == STATE_SEEKING)

        {

                bay_disable();

                is_seeking = 0;

        }

        //free the bay

        bays[robots[robot]->bay] = -1;

        free(robots[robot]);

        robots[robot] = NULL;

        numRobots--;

}

/**

 * Assigns a bay to a specific robot.

 *

 * @param robot the robot to assign the bay to

 *

 * @return the identifier for the assigned bay

 **/

int assign_bay(int robot)

{

        int bay = 0;

        //TODO: use better algorithm based on distance

        for (bay = 0; bay < NUM_BAYS; bay++)

                if (bays[bay] == -1)

                        break;

        if (bay == NUM_BAYS)

        {

                STATION_DEBUG_PRINT("Attempted to assign a bay when all ");

                STATION_DEBUG_PRINT("are full.\n");

        }

        bays[bay] = robot;

        return bay;

}

/**

 * Called when a robot begins seeking.

 *

 * @param robot the robot which has begun seeking

 **/

void charging_begin_seeking(int robot)

{

        STATION_DEBUG_PRINT("Robot ");

        STATION_DEBUG_PUTI(robot);

        STATION_DEBUG_PRINT(" has begun seeking.\n");

        if (robot > robotsSize)

        {

                int nextSize = robot + 1;

                ChargingRobot** temp = malloc(nextSize * sizeof(ChargingRobot*));

                if (temp == NULL)

                {

                        STATION_DEBUG_PRINT("Out of memory.\n");

                        return;

                }

                int i;

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

                        temp[i] = robots[i];

                for (; i < nextSize; i++)

                        temp[i] = NULL;

                free(robots);

                robots = temp;

                robotsSize = nextSize;

        }

        if (charging_is_robot_seeking())

        {

                STATION_DEBUG_PRINT("A robot is already seeking.\n");

                return;

        }

        //check if robot has reverted from docking to seeking

        if (robots[robot] != NULL)

        {

                switch (robots[robot]->state)

                {

                        case STATE_SEEKING:

                                //do nothing

                                STATION_DEBUG_PRINT("Robot is already ");

                                STATION_DEBUG_PRINT("seeking.\n");

                                break;

                        case STATE_DOCKED:

                                if (is_seeking)

                                {

                                        //we can only allow one robot to seek

                                        robots[robot]->state = STATE_NOT_CHARGING;

                                        bays[robots[robot]->bay] = -1;

                                        robots[robot]->bay = -1;

                                        break;

                                }

                                robots[robot]->state = STATE_SEEKING;

                                is_seeking = 1;

                                bay_enable(robots[robot]->bay);

                                break;

                        default:

                                STATION_DEBUG_PRINT("Robot is in an ");

                                STATION_DEBUG_PRINT("unexpected state.\n");

                                break;

                }

                return;

        }

        if (!charging_is_space_available())

        {

                STATION_DEBUG_PRINT("No bays are available for charging.\n");

                return;

        }

        ChargingRobot* r = malloc(sizeof(ChargingRobot));

        if (!r)

        {

                STATION_DEBUG_PRINT("Out of memory.\n");

                return;

        }

        r->state = STATE_SEEKING;

        r->bay = assign_bay(robot);

        r->verifyCount = VERIFY_DELAY;

        r->battery = 0;

        robots[robot] = r;

        numRobots++;

        is_seeking = 1;

        bay_enable(r->bay);

}

/**

 * Called when a robot has docked.

 *

 * @param robot the robot which has docked

 **/

void charging_dock(int robot)

{

        STATION_DEBUG_PRINT("Robot ");

        STATION_DEBUG_PUTI(robot);

        STATION_DEBUG_PRINT(" has docked with the station.\n");

        if (robots[robot] == NULL)

        {

                STATION_DEBUG_PRINT("Should not proceed from not ");

                STATION_DEBUG_PRINT("charging to docked.\n");

                return;

        }

        if (robots[robot]->state == STATE_DOCKED)

        {

                STATION_DEBUG_PRINT("Robot is already docked.\n");

                return;

        }

        //robot was seeking before

        is_seeking = 0;

        bay_disable();

        robots[robot]->state = STATE_DOCKED;

}

/**

 * Checks if there are any available bays

 * for a robot to occupy.

 *

 * @return nonzero if there are bays available,

 * and zero otherwise.

 **/

int charging_is_space_available(void)

{

        return numRobots < NUM_BAYS;

}

/**

 * Checks if there is a robot which

 * is currently seeking the station.

 *

 * @return nonzero if a robot is seeking the

 * station, zero otherwise.

 **/

int charging_is_robot_seeking(void)

{

        return is_seeking;

}

/**

 * Returns the state of the specified robot.

 *

 * @param robot the robot to get the state of

 *

 * @return either STATE_NOT_CHARGING, STATE_SEEKING, or STATE_DOCKED

 **/

int charging_get_robot_state(int robot)

{

        if (robot >= robotsSize || robots[robot] == NULL)

                return STATE_NOT_CHARGING;

        return robots[robot]->state;

}

/**

 * Update battery reading of a robot

 *

 * @param robot the robot to update battery reading

 * @param battery battery level of that robot

 **/

void update_battery(int robot, int battery)

{

        if (robot < robotsSize && robots[robot] != NULL)

                robots[robot]->battery = battery;

}

/**

 * Kick off charging robots for robot with lower battery level

 *

 * @param robot the robot that needs to be charged

 * @param battery battery level of that robot

 **/

void check_battery(int robot, int battery)

{

        if (charging_is_space_available()) return;

        // there's no way to read the voltage of a charging robot

        // so just eject the seeking robot if there's one, or the last charging robot

        charging_robot_iterator_init();

        int r,b;

        while (charging_robot_iterator_has_next())

        {

                r = charging_robot_iterator_next();

                b = robots[r]->battery;

                if (robots[r]->state==STATE_SEEKING) break;

        }

        if (battery<=BATTERY_LOWV&&b>BATTERY_LOWV)

        {

                //cancel robot r

                wl_charging_send_cancel(r);

                charging_cancel(r);

                //tell robot that charging station is now available

                wl_charging_send_station_available(robot);

        }

}

/**

 * Called when the BOM needs to be turned on for the 

 * token ring. If a robot is seeking, only the BOM of

 * the bay that is being sought will turn on. If no

 * robot is seeking, the BOM for all of the bays will

 * turn on.

 **/

void charging_bom_on(void)

{

        lbom_on();

}

/**

 * Called when the BOM needs to be turned off for the

 * token ring. Turns off the BOM for all bays.

 **/

void charging_bom_off(void)

{

        lbom_off();

}

/**

 * The charging station BOM can only emit, so

 * we just return -1.

 **/

int charging_bom_read(void)

{

        return -1;

}