 MotorValues motorB;
 MotorValues motorC;
 
-Robot warby;
+Robot robot;
 
 char buf[BUF_SIZE];
 int bufLen;
 bool bufDone;
 
 void setup() {
-  warby = Robot(ROBOT_ID, dirA, pwmA, dirB, pwmB, dirC, pwmC, true);
-  warby.stop();
+  robot = Robot(ROBOT_ID, dirA, pwmA, dirB, pwmB, dirC, pwmC, LOGGING);
+  robot.stop();
 
   bufLen = 0;
   bufDone = false;

#define dirC 12

#define BUF_SIZE 125

typedef struct {
  int pulse;

  Serial.println("Looping...");
}

inline bool match(const char *haystack, const char *needle, const int len) {
  for (int i = 0; i < len; ++i) {
    if (haystack[i] != needle[i]) {
      return false;
    }
  }
  return true;
}

// buf has format +00000+00000
inline int extractInt(char *buf, const int idx) {
  const int startIdx = idx * BUF_VAL_WIDTH;
  const int endIdx = startIdx + BUF_VAL_WIDTH;

  byte val = buf[endIdx];
  buf[endIdx] = 0;
  const int ret = atoi(buf + startIdx);
  buf[endIdx] = val;

  return ret;
}

// Message format: MRCMD+00000+00000
void handleMessage(char *buf) {
  if (!match(buf, "MR", 2)) {
    // Not a maproom message
    return;
  }

  // Advance 2 char past MR for msg
  char *msg = buf + 2;

  // Vals are 3 after that, as the command names are 3char
  char *vals = msg + 3;

  if (match(msg, "MOV", 3)) {
    // MOVE COMMAND

    int dir = extractInt(vals, 0);
    int mag = extractInt(vals, 1);

    robot.driveManager(dir, mag);
  } else if (match(msg, "DRW", 3)) {
    // DRAW COMMAND

    int dir = extractInt(vals, 0);
    int mag = extractInt(vals, 1);

    robot.drawManager(dir, mag);
  } else if (match(msg, "ROT", 3)) {
    // ROTATE COMMAND

    int desiredAngle = extractInt(vals, 0);
    int measuredAngle = extractInt(vals, 1);

    Serial.println("ROTATE MESSAGE");
    Serial.print("desiredAngle: ");
    Serial.print(desiredAngle);
    Serial.print("measuredAngle: ");
    Serial.print(measuredAngle);

    robot.rotateManager(desiredAngle, measuredAngle);

  } else if (match(msg, "XDR", 3)) {
    // DEBUG

    int w0_mag = extractInt(vals, 0);
    int w1_mag = extractInt(vals, 1);
    int w2_mag = extractInt(vals, 2);

    robot.driveSpecific(w0_mag, w1_mag, w2_mag);
  } else if (match(msg, "CAL", 3)) {
    // CALIBRATE

    int newRotation = extractInt(vals, 0);
    robot.calibrate(newRotation);
  } else if (match(msg, "STP", 3)) {
    // STOP

    robot.stop();
  } else {
    Serial.print("Unknown message: ");
    Serial.println(buf);

    robot.stop();
  }
}

