Colony

#define MAX(a,b) ((a) > (b) ? (a) : (b))

#define MIN(a,b) ((a) < (b) ? (a) : (b))

void bay_tester(void)

{

    bayStates[1] = 50;

    bayStates[2] = 51;

    bayStates[3] = 52;

}

void print_bays_debug(void)

{

	usb_puts("\n\r");

	usb_puts("Bays: ");

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

		usb_puti(bayStates[i]);

		usb_putc(' ');

	}

	usb_puts("\n\r");

	usb_puts("Seeking robot: ");

	usb_puti(robot_seeking);

	usb_puts("\n\r");

}

void do_consistency_checks(void)

{

    if(robot_seeking != 255 && robot_assignment(robot_seeking) == 255) // the only free bay from bay_tester()

    {

      usb_puts("BAD STUFF HAPPENED...\n\r");

      usb_puts("robot assigned to BAY 1 does not equal robot_seeking\n\r");

      usb_puts("\n\rrobot_seeking: ");

      usb_puti(robot_seeking);

      usb_puts("Bay 1 contains:  ");

      usb_puti(bayStates[1]);

      usb_puts("\n\r");

      //while(1);

    }

}

// END DEBUGGING FUNCTIONS

	int i;

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

		bayStates[i] = 255;

	robot_seeking = 255;

	bay_tester();

/*int assign_bay(int robot)

{

	int max_bom = sensors[robot]; // where the station sees the robot (max bom)

	int closest = 255;

	int distance = 16;

	int i;

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

	{

		if (bayStates[i] != 255)

			continue;

		int d = max_bom - i * 4; // distance (BOM-wise) from bay

		if (d < -8)

			d += 16;

		else

			if (d > 8)

				d -= 16;

		if (d < 0)

			d *= -1;

		if (d < distance) // finds closest bay to robot's current position

		{

			distance = d; // distance (BOM-wise)

			closest = i; // actual bay number

		}

	}

	if (closest == 255) // all full

		return 255;

	bayStates[closest] = robot;

	return closest;

}*/

/*int assign_bay(int robot) // returns number of an empty bay (255 if all full)

{

    for(int i = 0; i < NUM_BAYS; i++) // right now, go thru linearly and allocate first available bay

    {

        if(bayStates[i] == 255)

        {

            bayStates[i] = robot;

            return i;

        }

    }

}*/

int left_index( int pos )

{

	int i;

	for(i = pos; i >= 0; i--)

	{

		if(bayStates[i] != 255)

			return i;

	}

	return -1;

}

int right_index( int pos )

{

	int i;

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

	{

		if(bayStates[i] != 255)

			return i;

	}

	return NUM_BAYS;

}

/*int assign_bay( int robot ) // 'smart' assign_bay()

{

	int ldist = 0;

	int rdist = 0;

	int score = 0;

	int max = 0;

	int bay = 255;

	int count = 0;

	int i;

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

	{

		if(bayStates[i] == 255)

			count++;

	}

	if(count == NUM_BAYS)

	{

		bayStates[0] = robot;

		return 0;

	}

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

	{

		if(bayStates[i] == 255)

		{

			ldist = i - left_index(i);

			rdist = right_index(i) - i;

			if(ldist - i == 1)

				score = rdist;

			else if(i + rdist == NUM_BAYS)

				score = ldist;

			else

				score = MIN(ldist,rdist);

			if(score > max)

			{

				max = score;

				bay = i;

			}

		}

		ldist = rdist = 0;

	}

	bayStates[bay] = robot;

	return bay;

}*/

int robot_assignment(int robot) //returns bay to which 'robot' has been assigned, 255 if not yet assigned

{

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

    {

        if(bayStates[i] == robot)

            return i;

    }

    return 255;

}

int is_empty(void) //returns 1 if there is an empty bay, 0 if not

{

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

    {

        if(bayStates[i] == 255)

            return 1;

    }

    return 0;

}