#ifndef AMIRO_MAP
#define AMIRO_MAP

#include "global.hpp"
#include "linefollow.hpp"


namespace amiro {

  struct node {
    uint8_t id;
    uint8_t flag;
    uint8_t left;
    uint8_t right;
    union edge {
      struct edge_id{
        uint8_t left;
        uint8_t right;
      } edge_id;
      uint8_t arr[2];
    } edge;
    uint8_t visited;
    types::position pL; // Left
    types::position pR; // Right
    types::position pB; // Back
    union p{
      struct{types::position pL, pR, pB;} points;
      types::position arr[3];
    } p;
  };


class AmiroMap {
public:
  map_state * get_state() { return &state; }
  uint8_t getNodeCount(){ return nodeCount; }
  node * getNodeList(){return nodeList; }


  AmiroMap(Global *global, LineFollowStrategy lfStrategy) : global(global), lfStrategy(lfStrategy) {}

  /**
   * Initialize a new map from configuration.
   * @param config map configuration array
   *
   */
  uint8_t initialize();

  void reset();

  /**
   * 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
   *
   *
   */
  uint8_t update(uint16_t WL, uint16_t WR, LineFollowStrategy strategy);


  /**
    If this is called instead of update new fixpoints can automativally
    get detected and will be added to the current nodeList.
    Internally the update will be called.
    If there are no nodes in the node list they will be created.
   */
  uint8_t trackUpdate(uint16_t WL, uint16_t WR, LineFollowStrategy strategy, ut_states ut_state);





private:
  Global *global;
  LineFollowStrategy lfStrategy = LineFollowStrategy::EDGE_RIGHT;
  // Keeps track of the internal map state
  map_state state;
  // Holds the amount of detected node of the map
  uint8_t nodeCount = 0;
  // Stores all nodes and the corresponding coordinates of last detected fixpoints
  node nodeList[MAX_NODES];
  // If driving over fixpoint prevent continuous updating
  // True if left sensor is driving over black
  bool fxpDetected = false;

  /**
   * Recursively search through all nodes in the node list and
   * mark all reached nodes as visited.
   *
   * @param id node from where to start visiting the other nodes
   *
   */
  void visitNode(uint8_t id);

  /**
   *  Calculate the distance between two given points
   *
   *  @param p1 point 1
   *  @param p2 point 2
   *
   */
  uint32_t calculateDist(types::position *p1, types::position *p2);

  // add node/fxp to the nodeList and return its id
  uint8_t addNode(uint8_t l, uint8_t r, uint8_t flags);

  // Calculate dist and elength if possibel
  void calTravelState(types::position *p1);

  // Check if all nodes have followers
  // Calculate validity check
  void checkMap();

  void switchToNext(types::position *p1);

  // Copy contents of point from to point to (x, y, f_x)
  void copyPoint(types::position *from, types::position *to);

  // Create the first fxp with flag 1
  void createInitNode();

  uint8_t getNearest(types::position *p1);

  // Either create new fxpoint or assign point to existing one
  uint8_t assignFxp(types::position *p1);

};

}; // namespace amiro

#endif /* AMIRO_MAP */