AMiRo-OS

#ifndef AMIRO_MAP

#define AMIRO_MAP

#include "global.hpp"

#include "linefollow.hpp"

#include <amiroosconf.h>

#include <ch.hpp>

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

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