Colony

#include <colonet_wireless.h>

#include <wireless.h>

#include <stdio.h>

#include <string.h>

#include <stdlib.h>

#include <signal.h>

#include <unistd.h>

#include <pthread.h>

#define SWARM_PACKET_GROUP_ID 0x8

static PacketGroupHandler colonet_pgh;

static void colonet_handle_receive(char type, int wl_source, unsigned char* packet, int length);

void cleanup(int a)

{

    a = a;

    wl_terminate();

    printf("todo - close wl port\n");

    exit(0);

}

int main(int argc, char* argv[])

{

    struct sigaction act;

    memset(&act, 0, sizeof(struct sigaction));

    act.sa_handler = cleanup;

    if (argc < 2) return -1;

    //register a signal handler to be called when we get a SIGINT signal (killing the server by cntrl+c would generate this)

    sigaction(SIGINT, &act, NULL);

    char *wireless_port = argv[1];

    //initialize the wireless library giving it the port and the log file name

    if (colonet_wl_init(wireless_port) != 0) {

        printf("colonet_wl_init failed.\n");

        return -1;

    }

    colonet_pgh.groupCode = SWARM_PACKET_GROUP_ID;

    colonet_pgh.timeout_handler = NULL;

    colonet_pgh.handle_response = NULL;

    colonet_pgh.handle_receive = colonet_handle_receive;

    colonet_pgh.unregister = NULL;

    wl_register_packet_group(&colonet_pgh);

    //call the function that will start the thread that will listen for wireless messages

    if (colonet_wl_run_listener_thread()) {

        printf("colonet_wl_run_listener_thread failed.\n");

        return -1;

    }

    unsigned char i, j;

    int numRobots = 1;

    //int* robots = (int *)malloc(0);

    int robots[] = {13};

    for(i=0; ; i++)

    {

        if (i==0)

        {

            //numRobots = colonet_get_num_robots();

            //robots = (int *)realloc(robots, sizeof(int) * numRobots);

            //colonet_get_xbee_ids(robots);

        }

        /*for (j=0; j < numRobots; j++)

        {

            usleep(1000);

            printf("\rSending %d to %d    ", i, robots[j]);

            char packet[] = { i, i, i, i };

            wl_send_robot_to_robot_packet(SWARM_PACKET_GROUP_ID, 0, packet, sizeof(char) * 4, robots[j], 0);

        }*/

        pthread_yield();

    }

    //colonet_wl_join();

    printf("Exiting normally\n");

    return 0;

}

unsigned char count[] = { -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1};

static void colonet_handle_receive(char type, int wl_source, unsigned char* packet, int length)

{

    type = type;

    if (length != 17)

    {

        printf("Bad packet length from %d\n", wl_source);

        return;

    }

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

    {

        /*if (packet[i] != i)

        {

            printf("Corrupted packet from %d\n", wl_source);

            break;

        }*/

        printf("%d ", packet[i]);

    }

    printf("\n");

    if (packet[16]!=++count[wl_source])

        printf("Bad packet number from %d (%d, should be %d)\n", wl_source, packet[16], count[wl_source]);

    if (packet[16] == 0)

    {

        printf("Packet 0 received from %d\n", wl_source);

    }

    count[wl_source] = packet[16];

    fflush(stdout);

}