/**
   * ************************************************************************* commands for
   * interfacing with the driver directly
   *
   * @throws RetryException ************************************************************************
   */
  public void queuePoint(Point5d p) throws RetryException {
    String cmd = "G1 F" + df.format(getCurrentFeedrate());

    sendCommand(cmd);

    cmd =
        "G1 X"
            + df.format(p.x())
            + " Y"
            + df.format(p.y())
            + " Z"
            + df.format(p.z())
            + " F"
            + df.format(getCurrentFeedrate());

    sendCommand(cmd);

    super.queuePoint(p);
  }
  public void setCurrentPosition(Point5d p) throws RetryException {
    sendCommand("G92 X" + df.format(p.x()) + " Y" + df.format(p.y()) + " Z" + df.format(p.z()));

    super.setCurrentPosition(p);
  }
  /**
   * A toolchange is the code that goes in between commands for one head and commands for the other
   * this function creates a toolchange from a tool doing one layer to a tool doing another layer
   */
  private Layer toolchange(
      final ToolheadAlias fromTool,
      final Layer fromLayer,
      final ToolheadAlias toTool,
      final Layer toLayer) {
    /*
     * How does a toolchange work? Glad you asked:
     * First we need to do any operations relating to the previous nozzle.
     *   I think this is only a small reversal. It needs to be small because
     *   the previous layer may have ended with a reversal, and if we then
     *   reverse on top of that we'll lose the filament.
     * We need to prepare the nozzle that we're switching to, which means
     * doing a purge and wipe, if available.
     *   The purge is to undo the reversal from before, the wipe rubs the
     *   nozzle across a special piece on the machine.
     *   If wipes are turned off, do we still do purge? because that could
     *   end us up with all kindsa junk on the outside of the object.
     * For wipes: Since we're moving to another position to do the wipe, we
     *   have to record the next position we want to be at, because if we
     *   start the next layer from a random place we might end up spewing
     *   plastic all the way to that point.
     * At the end of a toolchange, we should disable whichever extruder is
     *   not being used using M18 A B (on the next call to whichever axis
     *   it'll start up again)
     *
     *   toolchange psudocode:
     *
     *   Layer toolchange = new Layer
     *
     *   if wipes
     *     layer.add(wipes)
     *
     *   nextPos = get next position (first G1 of next layer)
     *   layer.add(move up, perhaps just above the next layer height, as quickly as is reasonable)
     *   layer.add(move to nextPos, also fairly quickly)
     *   layer.add(set speed to F from nextPos, or,
     *   								if that's not present, the last F from the previous layer)
     *
     *   layer.add(M18 A B)
     */
    final ArrayList<String> result = new ArrayList<String>();
    // debug code///////////////////////////
    result.add("(*************start toolchange*************)");
    //////////////////////////////////////
    if (useWipes) {
      // The left/right distinction isn't actually important here
      // on a tom you have to wipe both heads, and on a replicator
      // wiping either does both
      result.addAll(wipe(leftWipe));
      if (machineType != MachineType.THE_REPLICATOR) result.addAll(wipe(rightWipe));
    }

    result.add(toTool.getRecallOffsetGcodeCommand());
    result.add("M108 " + toTool.getTcode() + "(Set tool)");

    // Ben's suggestion
    result.add("M18 A B");

    final DecimalFormat nf = (DecimalFormat) Base.getGcodeFormat();
    final Point5d firstPos = getFirstPosition(toLayer);
    firstPos.setZ(getLayerZ(toLayer));

    if (firstPos != null) {
      // The F here is a magic number, you can read about it in the 'wipe()' function
      // move up fairly quickly
      result.add("G1 Z" + nf.format(firstPos.z()) + " F3000");
      // move to the next point
      result.add(
          "G1 X"
              + nf.format(firstPos.x())
              + " Y"
              + nf.format(firstPos.y())
              + " Z"
              + nf.format(firstPos.z())
              + " F3000");
    }
    //		else
    //		{
    ////			System.err.print(toLayer);
    //		}

    // TODO: catch possible null pointer exceptions?
    // set the feedrate with an empty G1
    String feedrate = getFirstFeedrate(toLayer);
    if (feedrate.equals("")) feedrate = getLastFeedrate(fromLayer);
    result.add("G1 " + feedrate);

    // debug code///////////////////////////
    result.add("(*************end toolchange*************)");
    //////////////////////////////////////
    // The 'height' of the toolchange. just the average of the surrounding layers because why not?
    final double height = (toLayer.getHeight() - fromLayer.getHeight()) / 2;

    return new Layer(height, result);
  }