uint8_t data[8];
uint8_t mid;
int32_t amt;

boolean dbg = false;

void setup() {
  Serial.begin(115200);
  
  run(0, 0);
  run(1, 0);
  for (uint32_t i=1; i>0; i++) {
    run(0, i);
    run(1, i);

    if ((i & 0xffff) == 0) {
      Serial.print("Testing: ");
      Serial.print((((double)i) / 4294967295.0) * 100.0);
      Serial.println("%");
    }
  }
}


void run(char motor_id, int32_t amount) {
  if (!test(motor_id, amount)) {
    Serial.print("ERROR TESTING (");
    Serial.print(motor_id, DEC);
    Serial.print(", ");
    Serial.print(amount);
    Serial.print("): ");

    if (motor_id != mid) Serial.print("BAD MID! ");
    if (amount != amt) Serial.print("BAD AMT! ");
    Serial.println();
  }
}

boolean test(char motor_id, int32_t amount) {
  receiveEvent(run_step(motor_id, amount));
  return (motor_id == mid) && (amount == amt);
}

void loop() {
  // put your main code here, to run repeatedly:

}

uint8_t run_step(char motor_id, int32_t amount) {
  if (dbg) {
    Serial.print("run_step(");
    Serial.print(motor_id, DEC);
    Serial.print(", ");
    Serial.print(amount);
    Serial.print("): ");
  }

  // Cast to unsigned to get the raw bits properly
  uint32_t raw_amount = amount;
  
  char header = 0;
  header |= raw_amount & 0b1111111;
  header <<= 1;
  header |= motor_id & 0b1;
  if (dbg) Serial.print(header & 0xFF, HEX);
  
  uint8_t len = 0;
  data[len++] = header;
  
  raw_amount >>= 7;
  while (raw_amount) {
    if (dbg) Serial.write(' ');
    if (dbg) Serial.print((uint8_t)raw_amount & 0xFF, HEX);
  
    data[len++] = (uint8_t)raw_amount & 0xFF;

    raw_amount >>= 8;
  }
  if (dbg) Serial.write('\n');
  return len;
}

void receiveEvent(uint8_t length) {
  mid = data[0] & 0b1;
  data[0] >>= 1;
  amt = data[0] & 0b1111111;
  
  if (length > 1) {
    // Since the value in the header is only 7 bits, move it over 1 so the `<< (8 * i)` will line up properly
    amt <<= 1;
    for (uint8_t i=1; i<length; i++) {
      amt |= ((int32_t)data[i] & 0xFF) << (8 * i);
    }
    // We shifted everything to the left by 1, now shift it back right
    amt >>= 1;
  }

  if (dbg) {
    Serial.print("Motor ");
    Serial.print(mid, DEC);
    Serial.print(" moves ");
    Serial.println(amt, DEC);
  }
}