AMiRo-OS

				 amiro_map.cpp \

#include "amiro_map.hpp"

void AmiroMap::initialize(int8_t (&config)[MAX_NODES][NODE_ATTRIBUTES]){

  uint8_t nodeCount = 0;

  bool end = false;

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

    if(config[i][NODE_ATTRIBUTES - 1] == 255){

      end = true;

      break;

    }

    nodeCount++;

  }

  if (!end || nodeCount == 0 ){

    // Failed to determine the amount of nodes

    return;

  }

  this->nodeList = (struct node*) malloc(nodeCount * sizeof(struct node)); 

}

void AmiroMap::update(){

}

void AmiroMap::uninitialize(){

}

#ifndef AMIRO_MAP

#define AMIRO_MAP

#include <ch.hpp>

#include "global.hpp"

#include <amiroosconf.h>

#define MAX_NODES 20

#define NODE_ATTRIBUTES 4

namespace amiro {

  struct node{

    uint8_t id;

    uint8_t flag;

    types::position pL; //Left

    types::position pR; //Right

    types::position pB; //Back

  };

  class AmiroMap{

  public:

    AmiroMap(Global *global):global{global}{}

    void initialize(int8_t (&config)[MAX_NODES][NODE_ATTRIBUTES]);

    void update();

  private:

    Global *global;

    bool valid = false;

    node *nodeList = NULL;

    node *current = NULL;

    node *next = NULL;

    void uninitialize();

  };

};

#endif /* AMIRO_MAP */