commit bdac5becc4bc61d529b72f2063d7198c008d060b
Author: galberding <galberding@techfak.uni-bielefeld.de>
Date:   Fri Jun 26 15:10:16 2020 +0200

    Add debug return for AmiroMap update, test fixpoint detection

diff --git a/devices/DiWheelDrive/amiro_map.cpp b/devices/DiWheelDrive/amiro_map.cpp
index bf28585..70acdaf 100644
--- a/devices/DiWheelDrive/amiro_map.cpp
+++ b/devices/DiWheelDrive/amiro_map.cpp
@@ -1,9 +1,5 @@
 #include "amiro_map.hpp"
 
-// #include <cstdint>
-// #include <math.h>
-
-
 uint8_t AmiroMap::initialize(){
 
   // Clear old values in case the map is initialized again
@@ -11,6 +7,7 @@ uint8_t AmiroMap::initialize(){
   this->state.next = 0;
   this->state.valid = false;
   this->nodeCount = 0;
+  this->state.strategy = 0x1;
 
   // convert proto map to internal representation
   for (int i=0; i<MAX_NODES; i++){
@@ -32,6 +29,7 @@ uint8_t AmiroMap::initialize(){
     this->nodeList[i].flag = global->testmap[i][2];
     this->nodeCount++;
   }
+  this->state.next = this->nodeList[this->state.current].right;
 
   // TODO make validity check
 
@@ -62,17 +60,17 @@ 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.
+uint8_t AmiroMap::update(uint16_t WL, uint16_t WR, LineFollowStrategy strategy) {
+  // Called each time at the end of the user thread state machine
+  // The bottom sensors will be checked for black ground which is interpreted as
+  // filxpoint
 
   // 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.
+  uint8_t flag = 0;
   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();
+  // 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;
@@ -81,6 +79,7 @@ void AmiroMap::update(LineFollowStrategy strategy) {
 
   if (left && right) {
     // TODO A dangerous case -> amiro could be lifted
+    flag |= 255;
   }
   else if (left && !leftDetected) {
     // The sensor on the left side of the Amiro is driving on black
@@ -97,6 +96,8 @@ void AmiroMap::update(LineFollowStrategy strategy) {
     state.current = state.next;
     state.next = nodeList[state.current].right;
     state.strategy = 0x01;
+    state.eLength = 0; // Reset length to get recalculated after fixpoint
+    flag |= 0x1;
   }
   else if (right && !rightDetected) {
     // Same as left only for the right sensor.
@@ -110,29 +111,48 @@ void AmiroMap::update(LineFollowStrategy strategy) {
     nodeList[state.next].visited |= 0x02;
     state.current = state.next;
     state.next = nodeList[state.current].left;
-    state.strategy = 0x02;
+    state.strategy = 0x2;
+    state.eLength = 0; // Reset length to get recalculated after fixpoint
+    flag |= 0x2;
   }
   else if (!left && !right) {
     // in case the fixpoint is not detected anymore
     leftDetected = false;
     rightDetected = false;
+    flag |= 0x4;
   }
 
 
   // 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
-
-  //   }
-  // }
+  if (state.strategy
+      == (nodeList[state.current].visited & nodeList[state.next].visited)) {
+    flag |= 0x8;
+    // only update distance if both nodes were visited
+    // Calculate estimated length of the edge
+    if (state.strategy == 0x01) {
+      // Amiro is driving on the right edge
+      if (state.eLength == 0) {
+        state.eLength = calculateDist(&nodeList[state.next].pR,
+                                      &nodeList[state.current].pR);
+      }
+      state.dist = calculateDist(&nodeList[state.next].pR, &currentPos) * 100 /
+                   state.eLength;
+    } else {
+      // Driving on the left edge
+      if (state.eLength == 0) {
+        state.eLength =
+            calculateDist(&nodeList[state.next].pL, &nodeList[state.current].pL);
+      }
+      state.dist = calculateDist(&nodeList[state.next].pL, &currentPos) * 100 / state.eLength;
+
+    }
+  }
+  return flag;
+}
+
+uint32_t AmiroMap::calculateDist(types::position *p1, types::position *p2) {
+  return (uint32_t)  sqrt(pow(p2->x - p1->x, 2) +
+              pow(p2->y - p1->y, 2));
 }
