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; 

        /* Keep driving until we see a 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);

        /* Keep driving until we see a barcode */

        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(){

        char seen[NUM_FEATURES];                /* Have we been at this intersection before*/

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

        char seenall[NUM_FEATURES];     /* This is how the array should look like once we have

                                         * seen everything */

        int outEdges, currInt, chosenDir, i;

        /* Initialize the graph to all zeros */

        initGraph(seen, seenout);

        /* When we see all the outgoing edges for the intersections, 

         * seenout will look as follows */

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

                seenall[i] = 1;

        /* First drives to the nearest intersection */

        char barcode = driveToNextInt();

        /* Randomly traverses the graph until all edges are seen */

        while(seenout !=  seenall)

        {

                currInt = getIntersectNum(barcode);

                seen[currInt] = 1;

                /* Get the number of outgoing edges */

                outEdges = getNumOut(getIntersectType(currInt));

                /* Assign the number of outgoing edges for the current int */ 

                intersections[currInt].numOut = outEdges;

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

                 * if no such edge exists */

                chosenDir = rand() % outEdges;

                /* We have not seen all the outgoing edges for the intersection we are at */

                if(!seenout[currInt]){

                        intersections[currInt].outSeen++;

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

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

                                seenout[currInt] = 1;

                        }

                }

                /* Traverses edge, stores information in struct */

                //TODO: Does the chosendDir, correspond with what is in Priya's Code?

                //A number between 0 and the number of outgoing edges ?

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

                                getIntersectType(currInt), chosenDir);

        }

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

        while(1) sendIntersectionGraph();

        return 0;

}

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

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

        int i, j;

        /* Set all graph storge elements to 0 */

        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;

                for(j = 0; j<4; j++){

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

                        intersections[i].outgoingEdges[0].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;

}