void FullSpiralStc::scan(CellPtr current) {
  std::cout << "\033[1;34mcurrent-\033[0m\033[1;32mBEGIN:\033[0m " << current->get_center()->x << ","
            << current->get_center()->y << "\n";
  VectorPtr direction = (current->get_parent()->get_center() - current->get_center()) / 2 / tool_size;
  VectorPtr initial_direction = direction++;
  // Check current cell has diagonally opposite obstacles or not
  PartiallyOccupiableCellPtr c = boost::static_pointer_cast<PartiallyOccupiableCell>(current);
  Quadrant q = c->get_current_quadrant();
  Orientation orientation;
  if (q == I)
    orientation = AT_LEFT_SIDE;
  else if (q == II)
    orientation = IN_BACK;
  else if (q == III)
    orientation = AT_RIGHT_SIDE;
  else if (q == IV)
    orientation = IN_FRONT;
  c->set_quadrants_state(+q, see_obstacle(~orientation, tool_size) ? OBSTACLE : NEW);
  c->set_quadrants_state(-q, see_obstacle(~++orientation, tool_size) ? OBSTACLE : NEW);
  // While current cell has a new obstacle-free neighboring cell
  bool is_starting_cell = current == starting_cell;
  do {
    // Scan for new neighbor of current cell in counterclockwise order
    PartiallyOccupiableCellPtr neighbor = PartiallyOccupiableCellPtr(
        new PartiallyOccupiableCell(current->get_center() + direction * 2 * tool_size, 2 * tool_size));
    std::cout << "  \033[1;33mneighbor:\033[0m " << neighbor->get_center()->x << "," << neighbor->get_center()->y;
    // go_from(current, DONT_PASS, neighbor); // Full Scan-STC preparing
    if (should_go_to(neighbor, direction)) {
      // Go to free subcell of neighbor
      bool successful = go_from(current, PASS, neighbor);
      if (!successful) { // Obstacle
      } else {           // New free neighbor
        // Construct a spanning-tree edge
        neighbor->set_parent(current);
        old_cells.insert(neighbor);
        scan(neighbor);
      }
    } else {
      // Go to next subcell
      go_from(current, DONT_PASS, neighbor);
      continue;
    }
  } while (direction++ % initial_direction != (is_starting_cell ? IN_FRONT : AT_RIGHT_SIDE));
  // Back to subcell of parent
  if (!is_starting_cell) {
    go_from(current, PASS, current->get_parent());
  }
  std::cout << "\033[1;34mcurrent-\033[0m\033[1;31mEND:\033[0m " << current->get_center()->x << ","
            << current->get_center()->y << "\n";
}
void MstcOnline::scan(CellPtr current) {
  std::string status;
  communicator->set_current_cell(current);
  // FIXME
  status = communicator->create_status_message(boost::static_pointer_cast<IdentifiableCell>(current));
  communicator->write_status_message(status);
  communicator->read_message_then_update_old_cells();
  std::cout << "\033[1;34mcurrent-\033[0m\033[1;32mBEGIN:\033[0m " << current->get_center()->x << ","
            << current->get_center()->y << "\n";
  VectorPtr direction = (current->get_parent()->get_center() - current->get_center()) / 2 / tool_size;
  VectorPtr initial_direction = direction++;
  // While current cell has a new obstacle-free neighboring cell
  bool is_starting_cell = current == starting_cell;
  do {
    // Scan for new neighbor of current cell in counterclockwise order
    IdentifiableCellPtr neighbor = IdentifiableCellPtr(new IdentifiableCell(
        current->get_center() + direction * 2 * tool_size, 2 * tool_size, communicator->get_robot_name()));
    std::cout << "  \033[1;33mneighbor:\033[0m " << neighbor->get_center()->x << "," << neighbor->get_center()->y;
    communicator->read_message_then_update_old_cells();
    if (state_of(neighbor) == OLD) { // Check neighbor with current old cells
      // Go to next sub-cell
      if (communicator->ask_other_robot_still_alive(communicator->find_robot_name(neighbor))) {
        // Still alive
        communicator->set_current_cell(current);
        go_with(++direction, tool_size);
        continue;
      } else {
        // Dead
        if (state_of(neighbor) == OLD) { // Check again neighbor with new old cells
          communicator->set_current_cell(current);
          go_with(++direction, tool_size);
          continue;
        } else {
          std::cout << "\n";
          neighbor->set_parent(current);
          communicator->read_message_then_update_old_cells();
          communicator->insert_old_cell(neighbor);
          std::string message = communicator->create_old_cells_message();
          communicator->write_old_cells_message(message);
          communicator->set_current_cell(current);
          go_with(direction++, tool_size);
          scan(neighbor);
          continue;
        }
      }
    }
    if (see_obstacle(direction, tool_size / 2)) { // Obstacle
      // Go to next sub-cell
      communicator->set_current_cell(current);
      go_with(++direction, tool_size);
    } else { // New free neighbor
      std::cout << "\n";
      // Construct a spanning-tree edge
      neighbor->set_parent(current);
      communicator->read_message_then_update_old_cells();
      communicator->insert_old_cell(neighbor);
      std::string message = communicator->create_old_cells_message();
      communicator->write_old_cells_message(message);
      communicator->set_current_cell(current);
      go_with(direction++, tool_size);
      scan(neighbor);
    }
  } while (direction % initial_direction != (is_starting_cell ? AT_LEFT_SIDE : IN_FRONT));
  // Back to sub-cell of parent
  if (!is_starting_cell) {
    communicator->set_current_cell(current);
    go_with(direction, tool_size);
  }
  std::cout << "\033[1;34mcurrent-\033[0m\033[1;31mEND:\033[0m " << current->get_center()->x << ","
            << current->get_center()->y << "\n";
}
bool FullSpiralStc::go_from(CellPtr current, bool need_to_pass, CellPtr next) {
  VectorPtr direction = (next->get_center() - current->get_center()) / (2 * tool_size);
  PartiallyOccupiableCellPtr c = boost::static_pointer_cast<PartiallyOccupiableCell>(current);
  PartiallyOccupiableCellPtr n = boost::static_pointer_cast<PartiallyOccupiableCell>(next);
  Quadrant quadrant = c->get_current_quadrant();
  Quadrant q;
  Quadrant q1;
  Quadrant q2;
  Quadrant q3;
  Quadrant q4;
  if (~direction == AT_RIGHT_SIDE)
    q = IV;
  else if (~direction == IN_FRONT)
    q = I;
  else if (~direction == AT_LEFT_SIDE)
    q = II;
  else if (~direction == IN_BACK)
    q = III;
  q1 = q;
  q2 = ++q;
  q3 = ++q;
  q4 = ++q;
  if (quadrant == q1 || quadrant == q2) {
    if (!see_obstacle(direction, tool_size)) {
      if (need_to_pass == DONT_PASS)
        return true;
      bool successful = visit(next, quadrant == q1 ? q4 : q3);
      std::cout << "\n";
      return successful;
    } else {
      n->set_quadrants_state(q1 ? q4 : q3, OBSTACLE);
      if (!see_obstacle(quadrant == q1 ? ++direction : --direction, tool_size)) {
        visit(current, quadrant == q1 ? q2 : q1);
        if (!see_obstacle(quadrant == q1 ? --direction : ++direction, tool_size)) {
          if (need_to_pass == DONT_PASS)
            return true;
          bool successful = visit(next, quadrant == q1 ? q3 : q4);
          std::cout << "\n";
          return successful;
        } else {
          n->set_quadrants_state(q1 ? q3 : q4, OBSTACLE);
          return false;
        }
      } else {
        c->set_quadrants_state(q1 ? q2 : q1, OBSTACLE);
        return false;
      }
    }
  } else if (quadrant == q4 || quadrant == q3) {
    if (!see_obstacle(direction, tool_size)) {
      visit(c, quadrant == q4 ? q1 : q2);
      return go_from(c, need_to_pass, next);
    } else {
      c->set_quadrants_state(quadrant == q4 ? q1 : q2, OBSTACLE);
      if (!see_obstacle(quadrant == q4 ? ++direction : --direction, tool_size)) {
        visit(c, quadrant == q4 ? q3 : q4);
        if (!see_obstacle(quadrant == q4 ? --direction : ++direction, tool_size)) {
          visit(c, quadrant == q4 ? q2 : q1);
          return go_from(c, need_to_pass, next);
        } else {
          c->set_quadrants_state(quadrant == q4 ? q2 : q1, OBSTACLE);
          return false;
        }
      } else {
        c->set_quadrants_state(quadrant == q4 ? q3 : q4, OBSTACLE);
        return false;
      }
    }
  } else
    return false;
}