commit 7520e117dc27600780a308161b9322d8ce372500
Author: galberding <galberding@techfak.uni-bielefeld.de>
Date:   Sun Jun 21 13:22:59 2020 +0200

    Remove getter/setter, introduce state object for map information, first
    implementation of update()

diff --git a/devices/DiWheelDrive/amiro_map.cpp b/devices/DiWheelDrive/amiro_map.cpp
index f9522b1..289164a 100644
--- a/devices/DiWheelDrive/amiro_map.cpp
+++ b/devices/DiWheelDrive/amiro_map.cpp
@@ -1,33 +1,34 @@
 #include "amiro_map.hpp"
-#include <cstdint>
 
+// #include <cstdint>
+// #include <math.h>
 
-uint8_t AmiroMap::initialize(uint8_t (&config)[MAX_NODES][NODE_ATTRIBUTES]){
+
+uint8_t AmiroMap::initialize(){
 
   // Clear old values in case the map is initialized again
-  uint8_t visitTable[MAX_NODES];
-  this->current = 0;
-  this->next = 0;
-  this->valid = false;
+  this->state.current = 0;
+  this->state.next = 0;
+  this->state.valid = false;
 
   // convert proto map to internal representation
   for (int i=0; i<MAX_NODES; i++){
-    if(config[i][2] == 0xff && i != 0){
+    if(global->testmap[i][2] == 0xff && i != 0){
       break;
-    } else if (config[i][2] == 0xff && i == 0) {
-      this->valid = false;
+    } else if (global->testmap[i][2] == 0xff && i == 0) {
+      this->state.valid = false;
       return 255;
     }
 
     //look for start node (default is Node 0)
-    if (config[i][2] == 1 ) {
-      this->current= i;
+    if (global->testmap[i][2] == 1 ) {
+      this->state.current = i;
     }
 
     this->nodeList[i].id = i;
-    this->nodeList[i].left = config[i][0];
-    this->nodeList[i].right = config[i][1];
-    this->nodeList[i].flag = config[i][2];
+    this->nodeList[i].left = global->testmap[i][0];
+    this->nodeList[i].right = global->testmap[i][1];
+    this->nodeList[i].flag = global->testmap[i][2];
     this->nodeCount++;
   }
 
@@ -40,13 +41,13 @@ uint8_t AmiroMap::initialize(uint8_t (&config)[MAX_NODES][NODE_ATTRIBUTES]){
       if (this->nodeList[k].visited == 1) {
         this->nodeList[k].visited = 0;
       } else {
-        this->valid = false;
+        this->state.valid = false;
         return k;
       }
     }
   }
 
-  this->valid = true;
+  this->state.valid = true;
   return 42;
 }
 
@@ -60,7 +61,77 @@ void AmiroMap::visitNode(uint8_t id){
   }
 }
 
+void AmiroMap::update(LineFollowStrategy strategy) {
+  // Each time at the end of the user thread state maschine, when the strategy
+  // is changed or a fixpoint is detected, this method gets called.
+  // The strategy change only has an effect on the fixpoint or in particular if
+  // a fixpoint is detected.
+
+  // set the strategy directly, actually there is no need to store that variable in the class
+  // but we will go with it for now to initialize everything properly.
+  this->lfStrategy = strategy;
+  uint16_t WL = global->vcnl4020[constants::DiWheelDrive::PROX_WHEEL_LEFT].getProximityScaledWoOffset();
+  uint16_t WR = global->vcnl4020[constants::DiWheelDrive::PROX_WHEEL_RIGHT].getProximityScaledWoOffset();
+
+  // Check the wheel sensors
+  bool left = global->linePID.BThresh >= WL;
+  bool right = global->linePID.BThresh >= WR;
+  types::position currentPos = global->odometry.getPosition();
+
+  if (left && right) {
+    // TODO A dangerous case -> amiro could be lifted
+  }
+  else if (left && !leftDetected) {
+    // The sensor on the left side of the Amiro is driving on black
+    // To prevent continous fixpoint detection a point needs to be marked as currently detected
+    // and released.
+    leftDetected = true;
+    nodeList[state.next].pR.f_x = currentPos.f_x;
+    nodeList[state.next].pR.f_y = currentPos.f_y;
+    nodeList[state.next].pR.f_z = currentPos.f_z;
+    nodeList[state.next].pR.x = currentPos.x;
+    nodeList[state.next].pR.y = currentPos.y;
+    nodeList[state.next].pR.z = currentPos.z;
+    nodeList[state.next].visited |= 0x01;
+    state.current = state.next;
+    state.next = nodeList[state.current].right;
+    state.strategy = 0x01;
+  }
+  else if (right && !rightDetected) {
+    // Same as left only for the right sensor.
+    rightDetected = true;
+    nodeList[state.next].pL.f_x = currentPos.f_x;
+    nodeList[state.next].pL.f_y = currentPos.f_y;
+    nodeList[state.next].pL.f_z = currentPos.f_z;
+    nodeList[state.next].pL.x = currentPos.x;
+    nodeList[state.next].pL.y = currentPos.y;
+    nodeList[state.next].pL.z = currentPos.z;
+    nodeList[state.next].visited |= 0x02;
+    state.current = state.next;
+    state.next = nodeList[state.current].left;
+    state.strategy = 0x02;
+  }
+  else if (!left && !right) {
+    // in case the fixpoint is not detected anymore
+    leftDetected = false;
+    rightDetected = false;
+  }
+
 
-void AmiroMap::update(bool left, bool right, LineFollowStrategy strategy) {
+  // update internal map_state
+  // Update travel distance
+  // check if the nodes of the specific strategy where visited
+  // if (state.strategy
+  //     == (nodeList[state.current].visited & nodeList[state.next].visited)) {
+  //   // only update distance if both nodes were visited
+  //   if (state.strategy == 0x01) {
+  //     // Amiro is driving on the right edge
+  //     state.dist = (uint8_t) sqrt(
+  //                                 pow(nodeList[state.next].pR.x - nodeList[state.current].pR.x, 2)
+  //                                 + pow(nodeList[state.next].pR.y - nodeList[state.current].pR.y, 2));
+  //   } else {
+  //     // Driving on the left edge
 
+  //   }
+  // }
 }
diff --git a/devices/DiWheelDrive/amiro_map.hpp b/devices/DiWheelDrive/amiro_map.hpp
index 567c2fd..69b1d35 100644
--- a/devices/DiWheelDrive/amiro_map.hpp
+++ b/devices/DiWheelDrive/amiro_map.hpp
@@ -5,43 +5,36 @@
 #include "linefollow.hpp"
 #include <amiroosconf.h>
 #include <ch.hpp>
-#include <cstdint>
-
-#define MAX_NODES 10
-#define NODE_ATTRIBUTES 3
 
 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 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 %
+    uint8_t dist;
+    // True if the current loaded map is valid
+    bool valid;
+  };
 
 class AmiroMap {
 public:
-  uint8_t get_next()  { return next; }
-
-  void set_next( uint8_t next) { this->next = next; }
-
-  uint8_t get_current()  { return current; }
-
-  void set_current( uint8_t current) { this->current = current; }
-
-  node* get_nodeList()  { return nodeList; }
-
-  uint8_t get_nodeCount()  { return nodeCount; }
-
-  void set_nodeCount( uint8_t nodeCount) { this->nodeCount = nodeCount; }
-
-  bool get_valid()  { return valid; }
-
-  void set_valid( bool valid) { this->valid = valid; }
+  map_state * get_state() { return &state; }
 
   AmiroMap(Global *global) : global{global} {}
 
@@ -50,30 +43,45 @@ public:
    * @param config map configuration array
    *
    */
- uint8_t initialize(uint8_t (&config)[MAX_NODES][NODE_ATTRIBUTES]);
+  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
-
-
+   * 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
+   *
+   *
    */
-  void update(bool left, bool right, LineFollowStrategy strategy);
+  void update(LineFollowStrategy strategy);
 
 private:
   Global *global;
-  bool valid = false;
+  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];
-  uint8_t current = 0;
-  uint8_t next = 0;
+  // 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);
   };
 }; // namespace amiro
