AMiRo-OS

#ifndef AMIRO_MAP

#define AMIRO_MAP

#include "global.hpp"

#include "linefollow.hpp"

#include <amiroosconf.h>

#include <ch.hpp>

#include <cstdint>

#define MAX_NODES 10

#define NODE_ATTRIBUTES 3

namespace amiro {

struct node {

  uint8_t id;

  uint8_t flag;

  uint8_t left;

  uint8_t right;

  uint8_t visited;

  types::position pL; // Left

  types::position pR; // Right

  types::position pB; // Back

};

class AmiroMap {

public:

  uint8_t get_next()  { return next; }

  void set_next( uint8_t next) { this->next = next; }

  uint8_t get_current()  { return current; }

  void set_current( uint8_t current) { this->current = current; }

  node* get_nodeList()  { return nodeList; }

  uint8_t get_nodeCount()  { return nodeCount; }

  void set_nodeCount( uint8_t nodeCount) { this->nodeCount = nodeCount; }

  bool get_valid()  { return valid; }

  void set_valid( bool valid) { this->valid = valid; }

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

  /**

   * Initialize a new map from configuration.

   * @param config map configuration array

   *

   */

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

  /**

     Update the internal map state according to the detected fixpoint

     This function should be called for a generic update, each can cycle and in

     case a fixpoint on one side is detected.

     Internal state maschine will go into an error state in case both sensors are black.

     @param left fixpoint on left side detected

     @param right fixpoint on right side detected

     @param strategy current line follow strategy

   */

  void update(bool left, bool right, LineFollowStrategy strategy);

private:

  Global *global;

  bool valid = false;

  uint8_t nodeCount = 0;

  node nodeList[MAX_NODES];

  uint8_t current = 0;

  uint8_t next = 0;

  void visitNode(uint8_t id);

  };

}; // namespace amiro

#endif /* AMIRO_MAP */