Colony

#include "stdlib.h"

#include "mapping.h"

#include "lineDrive.h"

#include <dragonfly_lib.h>

#include <wl_basic.h>

//The last intersection that we encountered

int lastInt;

/* This array holds all of the intersections that are represented in the graph

 * after its creation, the graph is transmitted wirelessly */

node intersections[NUM_FEATURES]; 

/* This is run only once at the beginning of mapping when the robot is

 * placed somewhere randomly on the map in the middle of the road*/

char driveToNextInt(){

        char barcode; 

        do {

            barcode = (char) doDrive(200);

        } 

        while(barcode < 0); /* Condition codes are all neg. */

        return barcode;

}

/* 

 * Traverses the map using DFS 

 * Returns 0 if all of the intersections in the database were seen

 * 1 otherwise 

 */

char createEdge(edge* newEdge, int type, int direction){

        char barcode;

        char time;

        rtc_init(SIXTEENTH_SECOND, NULL);

        rtc_reset();

        turn(type, direction);

        do {

            barcode = (char) doDrive(200);

        }

        while(barcode < 0); /* Condition codes are all neg. */

        time = rtc_get();

        newEdge->to = barcode;

        newEdge->dist = time;

        return barcode;

}

/* This function performs mapping */

int createMap(){

    int seenout_count = 0;                        /* The number of intersections we have completely seen */

    char seen[NUM_FEATURES];                /* Have we seen this node? */

    char seenout[NUM_FEATURES];                /* Have we seen the outoging edges of this node? */

    int outEdges, currInt, chosenDir;

    initGraph(seen, seenout);

    /* Drives to the nearest intersection */

    char barcode = driveToNextInt();

    while(seenout_count < NUM_FEATURES)

    {

         currInt = getIntersectNum(barcode);

         seen[currInt] = 1;

         /* Get the number of outgoing edges */

         outEdges = getNumOut(getIntersectType(currInt));

        /* Randomly choose an outgoing edge that we have not seen to go down 

         * if no such edge exists */

        // TODO: include the appropriate header file for rand()

        chosenDir = rand() % outEdges;

        /* We have not seen all the outgoing edges */

        if(!seenout[currInt]){

            intersections[currInt].outSeen++;

            /* We have finished seeing all outgoing edges of the intersection */

            if(intersections[currInt].numOut == intersections[currInt].outSeen){

                seenout[currInt] = 1;

                seenout_count ++;

            }

        }

        /* Traverses edge, stores information in struct */

        createEdge(&(intersections[currInt].outgoingEdges[chosenDir]),

                    getIntersectType(currInt), chosenDir);

    }

/* We are done traversing send the graph to the robots */

sendIntersectionGraph();

return 0;

}

/* Initializes the elements in the graph to 0 */

void initGraph(char* seen, char* seenout){

    int i;

    /* Initialize all of these arrays */

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

            seen[i] = 0;

            seenout[i] = 0;

            intersections[i].type = 0;

            intersections[i].intNum = 0;

            intersections[i].numOut = 0;

            intersections[i].outSeen = 0;

            intersections[i].outgoingEdges[0].to = 0;

            intersections[i].outgoingEdges[0].dist = 0;

            intersections[i].outgoingEdges[1].to = 0;

            intersections[i].outgoingEdges[1].dist = 0;

            intersections[i].outgoingEdges[2].to = 0;

            intersections[i].outgoingEdges[2].dist = 0;

            intersections[i].outgoingEdges[3].to = 0;

            intersections[i].outgoingEdges[3].dist = 0;        

    }

}

/* Given an intersection type, returns the number of outgoing edges */

int getNumOut(int type) {

    switch(type){

        case 0: return 3;

        case 1: return -1;

        case 2: return -1;

        case 3: return 3;

        case 4: return 2;

    }

    return -1;

}

/* 

 * Drives to the next intersection and returns its ID

 * If we are at a dead end, returns -1

 */

int nextInt(){

        return 0;

}

/*

 * Given an intersection node returns an integer that

 * can be sent wirelessly

 *

 */

int encodeNode(node n){

        return 0;

}