class RobotArm
{
  static final int CMD_OFF  = 0x20;
  static final int CMD_MOVE = 0x40;
  static final int CMD_GRIP = 0x60;
  static final int ACK  = 0xFF;

  private static final boolean INTERPOLATE = false;
  private static final boolean DEBUG = true;
  private static final int SCALE = 1000;
  private static final int INCR = 4;

  private static final String hex(int x)
  {
    return "0x"+Integer.toHexString(x);
  }

  private static void printArray(String name, int[] array)
  {
    System.out.print(name+" =");
    for (int i = 0; i < array.length; i++)
      System.out.print(" "+array[i]);
    System.out.println("");
  }

  private CricketComm fComm;
  private int[] fCurrent;

  RobotArm(String port)
  {
    if (port != null)
      fComm = new CricketComm(port);
  }

  void close()
  {
    if (fComm != null)
      fComm.close();
  }

  private void send(int data)
  {
    if (fComm != null) {
      fComm.send(data);
      int echo = fComm.receive();
      if (echo != data)
        throw new Problem("Send "+hex(data)+", echo "+hex(echo));
    }
  }

  private void send(int command, int[] data)
  {
    int i;

    if (data != null) {
      for(i = 0; i < data.length; i++) {
        if (data[i] > 0x3f + 0xff)
          throw new Problem("Data value "+hex(data[i])+" too large");
        if (data[i] > 0xff) {
          command += 1<<i;
          data[i] -= 0x100;
        }
      }

      if (DEBUG) {
        System.out.print("RobotArm sending "+hex(command));
        for(i = 0; i < data.length; i++)
          System.out.print(", "+hex(data[i]));
        System.out.println("");
      }
    }

    // Send the command and arguments
    send(command);
    if (data != null) {
      for(i = 0; i < data.length; i++)
        send(data[i]);
    }

    if (fComm != null) {
      // Wait for the ack
      int ack = fComm.receive();
      if (ack != ACK)
        throw new Problem("Unrecognized character "+ack);
    }
  }

  void off()
  {
    if (DEBUG) System.out.println("RobotArm off");
    fCurrent = null;
    send(CMD_OFF, null);
  }

  void move(Position p)
  {
    if (DEBUG) System.out.println("RobotArm move "+p);

    int[] target = new int[] {p.fBase, p.fShoulder, p.fElbow, p.fWrist};

    if (INTERPOLATE && fCurrent != null) {
      int i;
      int count = 0;
      int[] step = new int[4];
      int[] curr = new int[4];

      // Calculate the total amount of change for each joint and the number
      // of intermediate positions.
      for (i = 0; i < 4; i++) {
        step[i] = target[i] - fCurrent[i];
        count = Math.max(Math.abs(step[i]), count);
      }

      // Scale the number of intermediate positions.
      count /= INCR;

      if (count > 0) {
        // Scale the step and current positions, to avoid integer truncation.
        for (i = 0; i < 4; i++) {
          step[i] = (step[i] * SCALE) / count;
          curr[i] = fCurrent[i] * SCALE;
        }

        if (false && DEBUG) {
          printArray("fCurrent", fCurrent);
          printArray("target", target);
          System.out.println("count = "+count);
          printArray("step", step);
          printArray("curr", curr);
        }

        if (DEBUG) System.out.println("Generating "+count+" steps");
        for (; count > 0; count--) {
          // Scale the positions down and send the move command.
          int[] tmp = new int[4];
          for (i = 0; i < 4; i++) {
            curr[i] += step[i];
            tmp[i] =  curr[i] / SCALE;
          }
          if (false && DEBUG) {
            printArray("submove: curr", curr);
            printArray("submove: tmp", tmp);
          }
          send(CMD_MOVE, tmp);
        }
      }
    }

    fCurrent = target;
    send(CMD_MOVE, target);
  }

  void grip(int value)
  {
    if (DEBUG) System.out.println("RobotArm grip "+value);
    send(CMD_GRIP, new int[] {value});
  }
}