#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
   *
   */
 uint8_t 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 */