Colony

#include "intersectData.h"

#include "../linefollowing/lineDrive.h"

int NodeData[NUM_FEATURES];

/*

 * This function serves to make the code for the intersection database

 * as reuseable as possible. This part creates the implementation of the

 * database that can be copied over for all projects.  

 *

 */

// Does nothing if your indmax is out of bounds.

void insertNode(int key, int featureid, int intersect_num, int node_type, int node_info){

    if(key < NUM_FEATURES)

      NodeData[key] = (featureid<<12) + (intersect_num<<5) + (node_type<<2) + node_info;

    return;

}

void insertIntersection(int key, int intersect_num, int node_type, int intersection_position){

    if(key < NUM_FEATURES)

       NodeData[key] = (INTERSECTION_NODE<<12) + (intersect_num<<5) + (node_type<<2)

           + intersection_position;

    return;

}

void insertRoad(int key, int intersect_num, int node_type, int road_type){

    if(key < NUM_FEATURES)

       NodeData[key] = (ROAD_NODE<<12) + (intersect_num<<5) + (node_type<<2) + road_type;

    return;

}

/*

 * This function serves to make the code for the intersection database

 * as reuseable as possible. This part initializes the data and is the

 * only one that needs to be rewritten for ech map. 

 *

 */

void initializeData(){

    //insert all intersections that are in the map.

    //THIS DATA FOR THE DEMO MAP

    insertIntersection(0, UNUSED_INTER, 0, 0);

    insertIntersection(1, 1, SINGLE, SACROSS);

    insertIntersection(2, UNUSED_INTER, 0, 0);

    insertIntersection(3, 3, DOUBLE_C, CBOT);

    insertIntersection(4, UNUSED_INTER, 0, 0);

    insertIntersection(5, 5, DOUBLE_T, TMIDDLE);

    insertIntersection(6, 3, DOUBLE_C, CLEFT);

    insertIntersection(7, 5, DOUBLE_T, TRIGHT);

    insertIntersection(8, UNUSED_INTER, 0, 0);

    insertIntersection(9, 5, DOUBLE_T, TLEFT);

    insertIntersection(10, UNUSED_INTER, 0, 0);

    insertIntersection(11, 1, SINGLE, SUP);

    insertIntersection(12, UNUSED_INTER, 0, 0);

    insertIntersection(13, 2, SINGLE, SUP);

    insertIntersection(14, 3, DOUBLE_C, CRIGHT);

    insertIntersection(15, 3, DOUBLE_C, CTOP);

    insertIntersection(16, 2, SINGLE, SACROSS);

    return;

}

// Functions to get data from the Database:

int getFeatureId(int key)

{

    if(key < NUM_FEATURES)

      return (NodeData[key]>>12)&15;

    else return -1;

}

int getIntersectType(int key){

    if(key < NUM_FEATURES && getFeatureId(key) == INTERSECTION_NODE)

            return (NodeData[key]>>2)&7;

    else return -1;

}

int getIntersectPos(int key){

    if(key < NUM_FEATURES && getFeatureId(key) == INTERSECTION_NODE)

            return (NodeData[key])&3;

    else return -1;

}

int getRoadType(int key){

    if(key < NUM_FEATURES && getFeatureId(key) == ROAD_NODE)

            return (NodeData[key]>>2)&7;

    else return -1;

}

int getRoadInfo(int key){

    if(key < NUM_FEATURES && getFeatureId(key) == ROAD_NODE)

            return (NodeData[key])&3;

    else return -1;

}

int getIntersectNum(int key){

    if(key < NUM_FEATURES && getFeatureId(key) == INTERSECTION_NODE) 

            return (NodeData[key]>>5)&127;

    else return -1;

}