AMiRo-OS

#include "amiro_map.hpp"

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

  // Clear old values in case the map is initialized again

  this->current = 0;

  this->next = 0;

  this->valid = false;

  // convert proto map to internal representation

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

    if(config[i][2] == 0xff && i != 0){

      this->valid = true;

      break;

    } else if (config[i][2] == 0xff && i == 0) {

      this->valid = false;

      return;

    }

    //look for start node (default is Node 0)

    if (config[i][2] == 1 ) {

      this->current= i;

    }

    this->nodeList[i].id = i;

    this->nodeList[i].left = config[i][0];

    this->nodeList[i].right = config[i][1];

    this->nodeList[i].flag = config[i][2];

    this->nodeCount++;

  }

  // TODO make validity check

}

void AmiroMap::update(bool left, bool right, LineFollowStrategy strategy) {

}