AMiRo-OS

#ifndef AMIRO_MAP

#define AMIRO_MAP

#include "global.hpp"

#include "linefollow.hpp"

#include <amiroosconf.h>

#include <ch.hpp>

#include <cstdint>

#include <math.h>

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

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

  };

}; // namespace amiro

#endif /* AMIRO_MAP */