#include "Dezibot.h"

Dezibot dezibot;

// How many times to run a command on the IMU consecutively;
const uint16_t iterations = 5000;

void setup() {
  dezibot.motion.detection.begin();
  //dezibot.motion.detection.end();
  // put your setup code here, to run once:
  Serial.begin(115200);
  // Wait for Serial to init
  while (!Serial) {
    ;;
  }
  // Test if IMU is working correctly
  char imu_whoami = dezibot.motion.detection.getWhoAmI();
  if (imu_whoami == 0x67) {
    Serial.println("IMU seems to be working. Starting measurements...");
  } else {
    Serial.println("IMU does not seemm to be working correctly.");
    exit(0);
  }
  dezibot.motion.detection.end();
  Serial.println("Returned IMU to low-power mode. Killing Serial peripheral. Goodbye!");
  delay(1000);
  Serial.end();
  delay(1000);
}

void loop() {
  // This puts both accelerometer and gyroscope into low noise mode, which is their highest
  // power consumption state
  dezibot.motion.detection.begin();
  for (uint16_t iter = 0; iter < iterations; iter++) {
    dezibot.motion.detection.getRotation();
    dezibot.motion.detection.getAcceleration();
    delay(1);
  }
  // Turn everything back off to measure baseline power consumption
  delay(iterations);
}