AMiRo-OS

#include "amiro_map.hpp"

#include <cstdint>

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

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

  uint8_t visitTable[MAX_NODES];

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

      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

  bool all_visited = true;

  for (int j=0; j<nodeCount; j++) {

    visitNode(j);

    for (int k = 0; k < nodeCount; k++) {

      if (nodeList[k].visited == 1) {

        nodeList[k].visited = 0;

      } else {

        this->valid = false;

        return;

      }

    }

  }

  this->valid = true;

}

void AmiroMap::visitNode(uint8_t id){

  if (this->nodeList[id].visited > 0){

    return;

  }else{

    nodeList[id].visited++;

    visitNode(this->nodeList[id].left);

    visitNode(this->nodeList[id].right);

  }

}

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

}