#ifndef AMIRO_MAP
#define AMIRO_MAP

#include "global.hpp"
#include "linefollow.hpp"
#include <amiroosconf.h>
#include <ch.hpp>


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
  };

  struct map_state {
    // 0 - left, 1- right
    uint8_t strategy;
    // Node ID of last detected fixpoint
    uint8_t current;
    // Next node ID
    uint8_t next;
    //Traveled Distance between current and next in %
    uint32_t dist;
    // True if the current loaded map is valid
    bool valid;
    // Length of the currently traveled edge
    uint32_t eLength;
  };

class AmiroMap {
public:
  map_state * get_state() { return &state; }

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

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


  /**
   * 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);

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 leftDetected = false;
  // True if right sensor is driving over black
  bool rightDetected = 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);
  };
}; // namespace amiro

#endif /* AMIRO_MAP */