AMiRo-OS

    copyPoint(&currentPos, &nodeList[state.next].pR);

    copyPoint(&currentPos, &nodeList[state.next].pR);

uint8_t AmiroMap::trackUpdate(uint16_t WL, uint16_t WR, LineFollowStrategy strategy,

                    ut_states ut_state) {

  // Check if map is valid

  if (this->state.valid){

    return update(WL, WR, strategy);

  }

  // Create init node if none is there

  // We will not assign a point to the initial fixpoint because it is not clear if

  // start position is at the correct point

  if (nodeCount == 0) {

    createInitNode();

  }

  bool left = global->linePID.BThresh >= WL;

  bool right = global->linePID.BThresh >= WR;

  types::position currentPos = global->odometry.getPosition();

  // Assign fixpoint if side sensor is black

  // Do not update if update was already applied the round before (leftDetected || rightDetected) == true

  if ((left || right) && !(leftDetected || rightDetected)) {

    // Determine what strategy to use

    // assignFxp() will use strategy to assign the next point

    state.strategy = right ? 1 : 2;

    // Check if next point is reachable

    if (state.next == 255){

      // Prepare state values for switch

      assignFxp(&currentPos);

    }

  }else if (!(left || right)) {

    // TODO: do we need both?

    leftDetected = rightDetected = true;

}

}

void AmiroMap::calTravelState() {

}

void AmiroMap::checkMap() {

}

void AmiroMap::switchToNext(types::position *p1) {

  // Update point if node was not visited before

  if ((nodeList[state.next].visited & state.strategy) == 0){

      copyPoint(p1, &nodeList[state.next].p.arr[state.strategy - 1]);

      nodeList[state.next].visited |= state.strategy;

  }

  leftDetected = true;

  state.current = state.next;

  state.next = nodeList[state.current].edge.arr[state.strategy - 1];

  state.eLength = 0; // Reset length to get recalculated after fixpoint

  return;

}

void AmiroMap::copyPoint(types::position* from, types::position* to) {

  to->x = from->x;

  to->y = from->y;

  to->f_x = from->f_x;

}

void AmiroMap::createInitNode() {

  this->nodeCount = 0;

  state.current = addNode(255, 255, 1);

  state.next = 255;

}

uint8_t AmiroMap::getNearest(types::position *p1) {

  uint8_t b;

  return b;

}

uint8_t AmiroMap::assignFxp(types::position *p1) {

  // Magic happens to determine if fixpoint is close enough

  uint8_t id = getNearest(p1);

  if(id < 255){

    // strategy is either 1 - right or 2 - left

    // Copy current point to either left or right point

    copyPoint(p1, &nodeList[id].p.arr[state.strategy - 1]);

    }else {

    // A new fixpoint needs to be created

    id = addNode(255, 255, 0);

    copyPoint(p1, &nodeList[id].p.arr[state.strategy - 1]);

  }

  // Prepare values for switching

  state.next = id;

  nodeList[state.current].edge.arr[state.strategy - 1] = id;

  // Mark point as visited

  nodeList[id].visited |= state.strategy;

  return id;