AMiRo-OS

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

  };

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

  uint8_t getNodeCount(){ return nodeCount; }

  node * getNodeList(){return nodeList; }

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

  /**

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

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

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