AMiRo-OS

#include "linefollow.hpp"

#include <cstdint>

  this->lfStrategy = strategy;

  uint8_t actualStrategy = this->lfStrategy == EDGE_LEFT ? 1 : 2;

  uint32_t thresh = 1;          // TODO: find good thresh value in cm

  uint8_t id = 255;

  uint32_t smallestDist = thresh;

  uint8_t currentStrategy;

  // Calculate the point which is nearest to the current one

  // check if distance and strategy match

  // If right point is found but no left point set choose this as the fitting point

  // Check how point was visited before calculating distance (non visited points are always (0,0))

  for (int i = 0; i < nodeCount; i++) {

    for (int j = 0; j < 2; j++){ // Iterate over l and r point

      if ((nodeList[i].visited & (j+1)) == 0){

        // Skip point if it was not visited for the given strategy

        continue;

      }

      uint32_t tmpDist = calculateDist(&nodeList[i].p.arr[j], p1);

      if (tmpDist < smallestDist){

        smallestDist = tmpDist;

        id = i;

        // Store strategy to match the correct point at the end

        currentStrategy = j;

      }

    }

  }

  if (id == 255){

    return 255;

  }

  // update point at fixpoint if it is not visited

  if ((nodeList[id].visited & actualStrategy) == 0){

    copyPoint(p1, &nodeList[id].p.arr[actualStrategy]);

    nodeList[id].visited |= actualStrategy;

  } // else point was already visited and is assigned

  return id;