/*
 * Copyright (C) 2016 The Android Open Source Project
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <algos/gyro_cal.h>

/////// DEFINITIONS AND MACROS ///////////////////////////////////////

// Maximum gyro bias correction (should be set based on
// expected max bias of the given sensor).
#define MAX_GYRO_BIAS 0.096f // [rad/sec]

// Helper constants for converting units.
#define RAD_TO_MDEG (float)(1e3*180.0/M_PI)
#define GRAVITY_TO_G (float)(1.0/9.80665)

/////// FORWARD DECLARATIONS /////////////////////////////////////////

static void deviceStillnessCheck(struct gyroCal_t* gyro_cal,
                                 uint64_t sample_time);

static void computeGyroCal(struct gyroCal_t* gyro_cal,
                           uint64_t calibration_time);

static void checkWatchdog(struct gyroCal_t* gyro_cal,
                          uint64_t sample_time);

#ifdef GYRO_CAL_DBG_ENABLED
static void gyroCalUpdateDebug(struct gyroCal_t* gyro_cal,
                               struct debugGyroCal_t* debug_gyro_cal);

static void gyroCalTuneDebugPrint(struct gyroCal_t* gyro_cal,
                                  uint64_t sample_time);

static void gyroCalDebugPrintData(int count,
                                  struct debugGyroCal_t *debug_data);

// Information print macro.
#define GYRO_INFO_PRINT(fmt, ...) do { \
        osLog(LOG_INFO, "%s " fmt, "[GYRO CAL]", ##__VA_ARGS__); \
    } while (0);

// Macro used to print out floating point numbers.
#define GYRO_ENCODE_FLOAT(x, num_digits) ((x < 0) ? "-" : ""), \
    (int)floorf(fabsf(x)),                                \
    (int)((fabsf(x) - floorf(fabsf(x))) * powf(10,num_digits))
#endif

/////// FUNCTION DEFINITIONS /////////////////////////////////////////

// Initialize the gyro calibration data structure.
void gyroCalInit(struct gyroCal_t* gyro_cal, uint64_t min_still_duration,
                 uint64_t max_still_duration,
                 float bias_x, float bias_y, float bias_z,
                 uint64_t calibration_time,
                 uint64_t window_time_duration,
                 float gyro_var_threshold,
                 float gyro_confidence_delta,
                 float accel_var_threshold,
                 float accel_confidence_delta,
                 float mag_var_threshold,
                 float mag_confidence_delta,
                 float stillness_threshold,
                 int remove_bias_enable) {

  // Clear gyro_cal structure memory.
  memset(gyro_cal, 0, sizeof(struct gyroCal_t));

  // Initialize the stillness detectors.
  // Gyro parameter input units are [rad/sec]
  // Accel parameter input units are [m/sec^2]
  // Magnetometer parameter input units are [uT]
  gyroStillDetInit(&gyro_cal->gyro_stillness_detect,
                 gyro_var_threshold,
                 gyro_confidence_delta);
  gyroStillDetInit(&gyro_cal->accel_stillness_detect,
                 accel_var_threshold,
                 accel_confidence_delta);
  gyroStillDetInit(&gyro_cal->mag_stillness_detect,
                 mag_var_threshold,
                 mag_confidence_delta);

  // Reset stillness flag and start timestamp.
  gyro_cal->prev_still = false;
  gyro_cal->start_still_time = 0;

  // Set the min and max window stillness duration.
  gyro_cal->min_still_duration = min_still_duration;
  gyro_cal->max_still_duration = max_still_duration;

  // Sets the duration of the stillness processing windows.
  gyro_cal->window_time_duration = window_time_duration;

  // Set the watchdog timeout duration.
  gyro_cal->gyro_watchdog_timeout_duration =
      2 * window_time_duration;

  // Load the last valid cal from system memory.
  gyro_cal->bias_x = bias_x; // [rad/sec]
  gyro_cal->bias_y = bias_y; // [rad/sec]
  gyro_cal->bias_z = bias_z; // [rad/sec]
  gyro_cal->calibration_time = calibration_time;

  // Set the stillness threshold required for gyro bias calibration.
  gyro_cal->stillness_threshold = stillness_threshold;

  // Current window end time used to assist in keeping sensor data
  // collection in sync. Setting this to zero signals that sensor data
  // will be dropped until a valid end time is set from the first gyro
  // timestamp received.
  gyro_cal->stillness_win_endtime = 0;

  // Gyro calibrations will be applied (see, gyroCalRemoveBias()).
  gyro_cal->gyro_calibration_enable = (remove_bias_enable > 0);

#ifdef GYRO_CAL_DBG_ENABLED
  GYRO_INFO_PRINT("Gyro Cal: Initialized.");
#endif
}

// Void all pointers in the gyro calibration data structure
// (prevents compiler warnings).
void gyroCalDestroy(struct gyroCal_t* gyro_cal) {
  (void)gyro_cal;
}

// Get the most recent bias calibration value.
void gyroCalGetBias(struct gyroCal_t* gyro_cal,
                    float* bias_x, float* bias_y, float* bias_z) {
  if (gyro_cal->gyro_calibration_enable) {
    *bias_x = gyro_cal->bias_x;
    *bias_y = gyro_cal->bias_y;
    *bias_z = gyro_cal->bias_z;
  }
}

// Set an initial bias calibration value.
void gyroCalSetBias(struct gyroCal_t* gyro_cal,
                    float bias_x, float bias_y, float bias_z,
                    uint64_t calibration_time) {
  gyro_cal->bias_x = bias_x;
  gyro_cal->bias_y = bias_y;
  gyro_cal->bias_z = bias_z;
  gyro_cal->calibration_time = calibration_time;
}

// Remove bias from a gyro measurement [rad/sec].
void gyroCalRemoveBias(struct gyroCal_t* gyro_cal,
                       float xi, float yi, float zi,
                       float* xo, float* yo, float* zo) {
  if (gyro_cal->gyro_calibration_enable) {
    *xo = xi - gyro_cal->bias_x;
    *yo = yi - gyro_cal->bias_y;
    *zo = zi - gyro_cal->bias_z;
  }
}

// Returns true when a new gyro calibration is available.
bool gyroCalNewBiasAvailable(struct gyroCal_t* gyro_cal) {
  bool new_gyro_cal_available =
      (gyro_cal->gyro_calibration_enable &&
       gyro_cal->new_gyro_cal_available);

  // Clear the flag.
  gyro_cal->new_gyro_cal_available = false;

  return new_gyro_cal_available;
}

// Update the gyro calibration with gyro data [rad/sec].
void gyroCalUpdateGyro(struct gyroCal_t* gyro_cal,
                       uint64_t sample_time,
                       float x, float y, float z,
                       float temperature) {

  // Make sure that a valid window end time is set,
  // and start the watchdog timer.
  if (gyro_cal->stillness_win_endtime <= 0) {
    gyro_cal->stillness_win_endtime =
        sample_time + gyro_cal->window_time_duration;

    // Start the watchdog timer.
    gyro_cal->gyro_watchdog_start = sample_time;
  }

  // Record the latest temperture sample.
  gyro_cal->latest_temperature_celcius = temperature;

  // Pass gyro data to stillness detector
  gyroStillDetUpdate(&gyro_cal->gyro_stillness_detect,
                     gyro_cal->stillness_win_endtime,
                     sample_time, x, y, z);

  // Perform a device stillness check, set next window end time, and
  // possibly do a gyro bias calibration and stillness detector reset.
  deviceStillnessCheck(gyro_cal, sample_time);
}

// Update the gyro calibration with mag data [micro Tesla].
void gyroCalUpdateMag(struct gyroCal_t* gyro_cal,
                      uint64_t sample_time,
                      float x, float y, float z) {

  // Pass magnetometer data to stillness detector.
  gyroStillDetUpdate(&gyro_cal->mag_stillness_detect,
                     gyro_cal->stillness_win_endtime,
                     sample_time, x, y, z);

  // Received a magnetometer sample; incorporate it into detection.
  gyro_cal->using_mag_sensor = true;

  // Perform a device stillness check, set next window end time, and
  // possibly do a gyro bias calibration and stillness detector reset.
  deviceStillnessCheck(gyro_cal, sample_time);
}

// Update the gyro calibration with accel data [m/sec^2].
void gyroCalUpdateAccel(struct gyroCal_t* gyro_cal,
                        uint64_t sample_time,
                        float x, float y, float z) {
  // Pass accelerometer data to stillnesss detector.
  gyroStillDetUpdate(&gyro_cal->accel_stillness_detect,
                     gyro_cal->stillness_win_endtime,
                     sample_time, x, y, z);

  // Perform a device stillness check, set next window end time, and
  // possibly do a gyro bias calibration and stillness detector reset.
  deviceStillnessCheck(gyro_cal, sample_time);
}

// Checks the state of all stillness detectors to determine
// whether the device is "still".
void deviceStillnessCheck(struct gyroCal_t* gyro_cal,
                          uint64_t sample_time) {

  bool stillness_duration_exceeded = false;
  bool stillness_duration_too_short = false;
  bool device_is_still = false;
  float conf_not_rot = 0;
  float conf_not_accel = 0;
  float conf_still = 0;

  // Check the watchdog timer.
  checkWatchdog(gyro_cal, sample_time);

  // Is there enough data to do a stillness calculation?
  if ((!gyro_cal->mag_stillness_detect.stillness_window_ready &&
       gyro_cal->using_mag_sensor) ||
      !gyro_cal->accel_stillness_detect.stillness_window_ready ||
      !gyro_cal->gyro_stillness_detect.stillness_window_ready) {
    return; // Not yet, wait for more data.
  }

  // Set the next window end time for the stillness detectors.
  gyro_cal->stillness_win_endtime =
      sample_time + gyro_cal->window_time_duration;

  // Update the confidence scores for all sensors.
  gyroStillDetCompute(&gyro_cal->accel_stillness_detect);
  gyroStillDetCompute(&gyro_cal->gyro_stillness_detect);
  if (gyro_cal->using_mag_sensor) {
    gyroStillDetCompute(&gyro_cal->mag_stillness_detect);
  } else {
    // Not using magnetometer, force stillness confidence to 100%.
    gyro_cal->mag_stillness_detect.stillness_confidence = 1.0f;
  }

  // Determine motion confidence scores (rotation, accelerating, and stillness).
  conf_not_rot = gyro_cal->gyro_stillness_detect.stillness_confidence *
      gyro_cal->mag_stillness_detect.stillness_confidence;
  conf_not_accel = gyro_cal->accel_stillness_detect.stillness_confidence;
  conf_still = conf_not_rot * conf_not_accel;

  // determine if the device is currently still.
  device_is_still = (conf_still > gyro_cal->stillness_threshold);

  if (device_is_still) {

    // Device is still logic:
    // If not previously still, then record the start time.
    // If stillness period is too long, then do a calibration.
    // Otherwise, continue collecting stillness data.

    // If device was not previously still, set new start timestamp.
    if (!gyro_cal->prev_still) {

      // Record the starting timestamp of the current stillness window.
      // This enables the calculation of total duration of the stillness period.
      gyro_cal->start_still_time =
          gyro_cal->gyro_stillness_detect.window_start_time;
    }

    // Check to see if current stillness period exceeds the desired limit
    // to avoid corrupting the .
    stillness_duration_exceeded =
        ((gyro_cal->gyro_stillness_detect.last_sample_time -
          gyro_cal->start_still_time) >
          gyro_cal->max_still_duration);

    if (stillness_duration_exceeded) {

      // The current stillness has gone too long. Do a calibration with the
      // current data and reset.

      // Update the gyro bias estimate with the current window data and
      // reset the stats.
      gyroStillDetReset(&gyro_cal->accel_stillness_detect, true);
      gyroStillDetReset(&gyro_cal->gyro_stillness_detect, true);
      gyroStillDetReset(&gyro_cal->mag_stillness_detect, true);

      // Perform calibration.
      computeGyroCal(gyro_cal,
                     gyro_cal->gyro_stillness_detect.last_sample_time);

      // Update stillness flag. Force the start of a new stillness period.
      gyro_cal->prev_still = false;
    } else {

      // Continue collecting stillness data.

      // Reset stillness detectors, and extend stillness period.
      gyroStillDetReset(&gyro_cal->accel_stillness_detect, false);
      gyroStillDetReset(&gyro_cal->gyro_stillness_detect, false);
      gyroStillDetReset(&gyro_cal->mag_stillness_detect, false);

      // Update stillness flag.
      gyro_cal->prev_still = true;
    }
  } else {

    // Device is NOT still; motion detected.

    // If device was previously still and the total stillness duration is not
    // "too short", then do a calibration with the data accumulated thus far.
    stillness_duration_too_short =
        ((gyro_cal->gyro_stillness_detect.window_start_time -
          gyro_cal->start_still_time) < gyro_cal->min_still_duration);

    if (gyro_cal->prev_still && !stillness_duration_too_short) {
      computeGyroCal(gyro_cal,
                     gyro_cal->gyro_stillness_detect.window_start_time);
    }

    // Reset stillness detectors and the stats.
    gyroStillDetReset(&gyro_cal->accel_stillness_detect, true);
    gyroStillDetReset(&gyro_cal->gyro_stillness_detect, true);
    gyroStillDetReset(&gyro_cal->mag_stillness_detect, true);

    // Update stillness flag.
    gyro_cal->prev_still = false;
  }

  // Reset the watchdog timer after we have processed data.
  gyro_cal->gyro_watchdog_start = sample_time;

#ifdef GYRO_CAL_DBG_ENABLED
  // Debug information available.
  gyro_cal->debug_processed_data_available = true;
  gyro_cal->debug_processed_data_time = sample_time;
#endif
}

// Calculates a new gyro bias offset calibration value.
void computeGyroCal(struct gyroCal_t* gyro_cal,
                    uint64_t calibration_time) {

  // Current calibration duration.
  uint64_t cur_cal_dur =
      calibration_time - gyro_cal->start_still_time;

  // Check to see if new calibration values is within acceptable range.
  if (!(gyro_cal->gyro_stillness_detect.prev_mean_x < MAX_GYRO_BIAS &&
        gyro_cal->gyro_stillness_detect.prev_mean_x >-MAX_GYRO_BIAS &&
        gyro_cal->gyro_stillness_detect.prev_mean_y < MAX_GYRO_BIAS &&
        gyro_cal->gyro_stillness_detect.prev_mean_y >-MAX_GYRO_BIAS &&
        gyro_cal->gyro_stillness_detect.prev_mean_z < MAX_GYRO_BIAS &&
        gyro_cal->gyro_stillness_detect.prev_mean_z >-MAX_GYRO_BIAS)) {
    // Outside of range. Ignore, reset, and continue.
    return;
  }

  // Record new gyro bias offset calibration.
  gyro_cal->bias_x = gyro_cal->gyro_stillness_detect.prev_mean_x;
  gyro_cal->bias_y = gyro_cal->gyro_stillness_detect.prev_mean_y;
  gyro_cal->bias_z = gyro_cal->gyro_stillness_detect.prev_mean_z;

  // Record final stillness confidence.
  gyro_cal->stillness_confidence =
      gyro_cal->gyro_stillness_detect.prev_stillness_confidence *
      gyro_cal->accel_stillness_detect.prev_stillness_confidence *
      gyro_cal->mag_stillness_detect.prev_stillness_confidence;

  // Store calibration stillness duration.
  gyro_cal->calibration_time_duration = cur_cal_dur;

  // Store calibration time stamp.
  gyro_cal->calibration_time = calibration_time;

  // Set flag to indicate a new gyro calibration value is available.
  gyro_cal->new_gyro_cal_available = true;

#ifdef GYRO_CAL_DBG_ENABLED
  // Increment the total count of calibration updates.
  gyro_cal->debug_calibration_count++;

  // Store the last 'DEBUG_GYRO_SHORTTERM_NUM_CAL' calibration debug data
  // in a circular buffer, 'debug_cal_data[]'. 'debug_head' is the index
  // of the last valid calibration.
  gyro_cal->debug_head++;
  if (gyro_cal->debug_head >= DEBUG_GYRO_SHORTTERM_NUM_CAL) {
    gyro_cal->debug_head = 0;
  }
  if (gyro_cal->debug_num_cals < DEBUG_GYRO_SHORTTERM_NUM_CAL) {
    gyro_cal->debug_num_cals++;
  }

  // Update the calibration debug information.
  gyroCalUpdateDebug(gyro_cal,
                     &gyro_cal->debug_cal_data[gyro_cal->debug_head]);

  // Improve the collection of historic calibrations. Limit frequency to
  // every N hours.
  if ((gyro_cal->debug_num_cals_hist <= 0) ||
      (calibration_time -
       gyro_cal->
       debug_cal_data_hist[gyro_cal->debug_head_hist].calibration_time)
      >= DEBUG_GYRO_CAL_LIMIT) {
    gyro_cal->debug_head_hist++;
    if (gyro_cal->debug_head_hist >= DEBUG_GYRO_LONGTERM_NUM_CAL) {
      gyro_cal->debug_head_hist = 0;
    }
    if (gyro_cal->debug_num_cals_hist < DEBUG_GYRO_LONGTERM_NUM_CAL) {
      gyro_cal->debug_num_cals_hist++;
    }

    // Update the calibration debug information.
    gyroCalUpdateDebug(gyro_cal,
                       &gyro_cal->
                       debug_cal_data_hist[gyro_cal->debug_head_hist]);
  }
#endif
}

// Check for a watchdog timeout condition.
void checkWatchdog(struct gyroCal_t* gyro_cal, uint64_t sample_time) {

  bool watchdog_timeout;

  // Check for initialization of the watchdog time (=0).
  if (gyro_cal->gyro_watchdog_start <= 0) {
    return;
  }

  // Check for timeout condition of watchdog.
  watchdog_timeout = ((sample_time - gyro_cal->gyro_watchdog_start) >
                      gyro_cal->gyro_watchdog_timeout_duration);

  // If a timeout occurred then reset to known good state.
  if (watchdog_timeout) {

    // Reset stillness detectors and restart data capture.
    gyroStillDetReset(&gyro_cal->accel_stillness_detect, true);
    gyroStillDetReset(&gyro_cal->gyro_stillness_detect, true);
    gyroStillDetReset(&gyro_cal->mag_stillness_detect, true);
    gyro_cal->mag_stillness_detect.stillness_confidence = 0;
    gyro_cal->stillness_win_endtime = 0;

    // Force stillness confidence to zero.
    gyro_cal->accel_stillness_detect.prev_stillness_confidence = 0;
    gyro_cal->gyro_stillness_detect.prev_stillness_confidence = 0;
    gyro_cal->mag_stillness_detect.prev_stillness_confidence = 0;
    gyro_cal->stillness_confidence = 0;
    gyro_cal->prev_still = false;

    // If there are no magnetometer samples being received then
    // operate the calibration algorithm without this sensor.
    if (!gyro_cal->mag_stillness_detect.stillness_window_ready &&
       gyro_cal->using_mag_sensor) {
      gyro_cal->using_mag_sensor = false;
    }

    // Assert watchdog timeout flags.
    gyro_cal->gyro_watchdog_timeout |= watchdog_timeout;
    gyro_cal->gyro_watchdog_start = 0;
#ifdef GYRO_CAL_DBG_ENABLED
    gyro_cal->debug_watchdog_count++;
#endif
  }
}

#ifdef GYRO_CAL_DBG_ENABLED
// Update the calibration debug information.
void gyroCalUpdateDebug(struct gyroCal_t* gyro_cal,
                        struct debugGyroCal_t* debug_gyro_cal) {
  // Probability of stillness (acc, rot, still), duration, timestamp.
  debug_gyro_cal->accel_stillness_conf =
      gyro_cal->accel_stillness_detect.prev_stillness_confidence;
  debug_gyro_cal->gyro_stillness_conf =
      gyro_cal->gyro_stillness_detect.prev_stillness_confidence;
  debug_gyro_cal->mag_stillness_conf =
      gyro_cal->mag_stillness_detect.prev_stillness_confidence;

  // Magnetometer usage.
  debug_gyro_cal->used_mag_sensor = gyro_cal->using_mag_sensor;

  // Temperature at calibration time.
  debug_gyro_cal->temperature_celcius =
      gyro_cal->latest_temperature_celcius;

  // Calibration time and stillness duration.
  debug_gyro_cal->calibration_time_duration =
      gyro_cal->calibration_time_duration;
  debug_gyro_cal->calibration_time =
      gyro_cal->calibration_time;

  // Record the current calibration values
  debug_gyro_cal->calibration[0] = gyro_cal->bias_x;
  debug_gyro_cal->calibration[1] = gyro_cal->bias_y;
  debug_gyro_cal->calibration[2] = gyro_cal->bias_z;

  // Record the previous window means
  debug_gyro_cal->accel_mean[0] =
      gyro_cal->accel_stillness_detect.prev_mean_x;
  debug_gyro_cal->accel_mean[1] =
      gyro_cal->accel_stillness_detect.prev_mean_y;
  debug_gyro_cal->accel_mean[2] =
      gyro_cal->accel_stillness_detect.prev_mean_z;

  debug_gyro_cal->gyro_mean[0] =
      gyro_cal->gyro_stillness_detect.prev_mean_x;
  debug_gyro_cal->gyro_mean[1] =
      gyro_cal->gyro_stillness_detect.prev_mean_y;
  debug_gyro_cal->gyro_mean[2] =
      gyro_cal->gyro_stillness_detect.prev_mean_z;

  debug_gyro_cal->mag_mean[0] =
      gyro_cal->mag_stillness_detect.prev_mean_x;
  debug_gyro_cal->mag_mean[1] =
      gyro_cal->mag_stillness_detect.prev_mean_y;
  debug_gyro_cal->mag_mean[2] =
      gyro_cal->mag_stillness_detect.prev_mean_z;

  // Record the variance data.
  debug_gyro_cal->accel_var[0] =
      gyro_cal->accel_stillness_detect.win_var_x;
  debug_gyro_cal->accel_var[1] =
      gyro_cal->accel_stillness_detect.win_var_y;
  debug_gyro_cal->accel_var[2] =
      gyro_cal->accel_stillness_detect.win_var_z;

  debug_gyro_cal->gyro_var[0] =
      gyro_cal->gyro_stillness_detect.win_var_x;
  debug_gyro_cal->gyro_var[1] =
      gyro_cal->gyro_stillness_detect.win_var_y;
  debug_gyro_cal->gyro_var[2] =
      gyro_cal->gyro_stillness_detect.win_var_z;

  debug_gyro_cal->mag_var[0] =
      gyro_cal->mag_stillness_detect.win_var_x;
  debug_gyro_cal->mag_var[1] =
      gyro_cal->mag_stillness_detect.win_var_y;
  debug_gyro_cal->mag_var[2] =
      gyro_cal->mag_stillness_detect.win_var_z;
}

// Helper function to print debug statements.
void gyroCalDebugPrintData(int count,
                       struct debugGyroCal_t *debug_data) {
  GYRO_INFO_PRINT(
      "#%d Gyro Bias Calibration = {%s%d.%06d, %s%d.%06d, %s%d.%06d} [mdps]\n",
      count,
      GYRO_ENCODE_FLOAT(debug_data->calibration[0] * RAD_TO_MDEG, 6),
      GYRO_ENCODE_FLOAT(debug_data->calibration[1] * RAD_TO_MDEG, 6),
      GYRO_ENCODE_FLOAT(debug_data->calibration[2] * RAD_TO_MDEG, 6)
                  );

  if (debug_data->used_mag_sensor) {
    GYRO_INFO_PRINT("   Using Magnetometer.\n");
  } else {
    GYRO_INFO_PRINT("   Not Using Magnetometer.\n");
  }

  GYRO_INFO_PRINT("   Temperature = %s%d.%06d [C]\n",
                  GYRO_ENCODE_FLOAT(debug_data->temperature_celcius, 6));

  GYRO_INFO_PRINT("   Calibration Timestamp = %llu [nsec]\n",
                  debug_data->calibration_time);

  GYRO_INFO_PRINT("   Stillness Duration = %llu [nsec]\n",
                  debug_data->calibration_time_duration);

  GYRO_INFO_PRINT("   Accel Mean = {%s%d.%06d, %s%d.%06d, %s%d.%06d} [g]\n",
                  GYRO_ENCODE_FLOAT(debug_data->accel_mean[0] *
                                    GRAVITY_TO_G, 6),
                  GYRO_ENCODE_FLOAT(debug_data->accel_mean[1] *
                                    GRAVITY_TO_G, 6),
                  GYRO_ENCODE_FLOAT(debug_data->accel_mean[2] *
                                    GRAVITY_TO_G, 6));

  GYRO_INFO_PRINT("   Mag Mean = {%s%d.%06d, %s%d.%06d, %s%d.%06d} [uT]\n",
                  GYRO_ENCODE_FLOAT(debug_data->mag_mean[0], 6),
                  GYRO_ENCODE_FLOAT(debug_data->mag_mean[1], 6),
                  GYRO_ENCODE_FLOAT(debug_data->mag_mean[2], 6)
                  );
}

// Print Debug data useful for tuning the stillness detectors.
void gyroCalTuneDebugPrint(struct gyroCal_t* gyro_cal, uint64_t sample_time) {

  static uint64_t counter = 0;

  // Output sensor variance levels to assist with tuning thresholds
  // every N seconds.
  if ((sample_time - counter) > 10000000000) { //nsec
    GYRO_INFO_PRINT("");
    GYRO_INFO_PRINT(
        "   Gyro Variance = {%s%d.%08d, %s%d.%08d, %s%d.%08d} [rad/sec]^2\n",
                    GYRO_ENCODE_FLOAT(
                        gyro_cal->gyro_stillness_detect.win_var_x, 8),
                    GYRO_ENCODE_FLOAT(
                        gyro_cal->gyro_stillness_detect.win_var_y, 8),
                    GYRO_ENCODE_FLOAT(
                        gyro_cal->gyro_stillness_detect.win_var_z, 8));
    GYRO_INFO_PRINT(
        "   Accel Variance = {%s%d.%08d, %s%d.%08d, %s%d.%08d} [m/sec^2]^2\n",
                    GYRO_ENCODE_FLOAT(
                        gyro_cal->accel_stillness_detect.win_var_x, 8),
                    GYRO_ENCODE_FLOAT(
                        gyro_cal->accel_stillness_detect.win_var_y, 8),
                    GYRO_ENCODE_FLOAT(
                        gyro_cal->accel_stillness_detect.win_var_z, 8));
    GYRO_INFO_PRINT(
        "   Mag Variance = {%s%d.%06d, %s%d.%06d, %s%d.%06d} [uT]^2\n",
                    GYRO_ENCODE_FLOAT(
                        gyro_cal->mag_stillness_detect.win_var_x, 6),
                    GYRO_ENCODE_FLOAT(
                        gyro_cal->mag_stillness_detect.win_var_y, 6),
                    GYRO_ENCODE_FLOAT(
                        gyro_cal->mag_stillness_detect.win_var_z, 6));
    GYRO_INFO_PRINT(
        "   Stillness = {G%s%d.%06d, A%s%d.%06d, M%s%d.%06d}\n",
                    GYRO_ENCODE_FLOAT(
                        gyro_cal->
                        gyro_stillness_detect.stillness_confidence, 6),
                    GYRO_ENCODE_FLOAT(
                        gyro_cal->
                        accel_stillness_detect.stillness_confidence, 6),
                    GYRO_ENCODE_FLOAT(
                        gyro_cal->
                        mag_stillness_detect.stillness_confidence, 6));
    GYRO_INFO_PRINT(
        "   Temperature = %s%d.%06d [C]\n",
                    GYRO_ENCODE_FLOAT(
                        gyro_cal->latest_temperature_celcius, 6));
    GYRO_INFO_PRINT("Total Gyro Calibrations: %lu\n",
                    gyro_cal->debug_calibration_count);
    counter = sample_time; //reset
  }
}

// Print Debug data report.
void gyroCalDebugPrint(struct gyroCal_t* gyro_cal,
                       int* state,
                       uint64_t sample_time) {

  static int head_index;
  static int i, j;

  if (gyro_cal->debug_num_cals == 0) {
    *state = -1; // idle state.
  }

  // State machine to control reporting out debug print data.
  switch (*state) {

    case 0:
      // STATE 0 : Print out header
      GYRO_INFO_PRINT("");
      GYRO_INFO_PRINT(
          "---DEBUG REPORT-------------------------------------\n");
      GYRO_INFO_PRINT("Gyro Calibrations To Report (newest to oldest): %d\n",
                      gyro_cal->debug_num_cals);
      head_index = gyro_cal->debug_head;
      i = 1; // initialize report count
      *state = 1; // set next state.
      break;

    case 1:
      // STATE 1: Print out past N recent calibrations.
      if (i <= gyro_cal->debug_num_cals) {

        // Print the data.
        gyroCalDebugPrintData(i, &gyro_cal->debug_cal_data[head_index]);

        // Move to the previous calibration data set.
        head_index--;
        if (head_index < 0) {
          head_index = DEBUG_GYRO_SHORTTERM_NUM_CAL-1;
        }

        // Increment the report count.
        i++;

        // Go to wait state.
        j = 0;
        *state = 2;

      } else {
        // Print out the footer data.
        *state = 3;
      }
      break;

    case 2:
      // STATE 2 : Wait state.
      // Wait for N cycles of gyro samples before printing out more data.
      // This helps throttle the print statements.
      if (j <= 50) {
        j++;
      } else {
        *state = 1;
      }
      break;

    case 3:
      // STATE 3 : Print the end of the debug report.
      GYRO_INFO_PRINT("Total Gyro Calibrations: %lu\n",
                      gyro_cal->debug_calibration_count);

      GYRO_INFO_PRINT("Total Watchdog Timeouts: %lu\n",
                      gyro_cal->debug_watchdog_count);

      GYRO_INFO_PRINT(
          "---END DEBUG REPORT---------------------------------\n");
      *state = 4;
      break;

    case 4:
      // STATE 4 : Print out header (historic data)
      GYRO_INFO_PRINT("");
      GYRO_INFO_PRINT(
          "---DEBUG REPORT (HISTORY)---------------------------\n");
      GYRO_INFO_PRINT("Gyro Calibrations To Report (newest to oldest): %d\n",
                      gyro_cal->debug_num_cals_hist);
      head_index = gyro_cal->debug_head_hist;
      i = 1; // initialize report count
      *state = 5; // set next state.
      break;

    case 5:
      // STATE 5: Print out past N historic calibrations.
      if (i <= gyro_cal->debug_num_cals_hist) {

        // Print the data.
        gyroCalDebugPrintData(i, &gyro_cal->debug_cal_data_hist[head_index]);

        // Move to the previous calibration data set.
        head_index--;
        if (head_index < 0) {
          head_index = DEBUG_GYRO_LONGTERM_NUM_CAL-1;
        }

        // Increment the report count.
        i++;

        // Go to wait state.
        j = 0;
        *state = 6;

      } else {
        // Print out the footer data.
        *state = 7;
      }
      break;

    case 6:
      // STATE 6 : Wait state.
      // Wait for N cycles of gyro samples before printing out more data.
      // This helps throttle the print statements.
      if (j <= 50) {
        j++;
      } else {
        *state = 5;
      }
      break;

    case 7:
      // STATE 3 : Print the end of the debug report.
      GYRO_INFO_PRINT("Total Gyro Calibrations: %lu\n",
                      gyro_cal->debug_calibration_count);

      GYRO_INFO_PRINT("Total Watchdog Timeouts: %lu\n",
                      gyro_cal->debug_watchdog_count);

      GYRO_INFO_PRINT(
          "---END DEBUG REPORT---------------------------------\n");
      *state = -1;
      break;

    default :
      // Idle state.
      *state = -1;

      // Report periodic data useful for tuning the stillness detectors.
      //gyroCalTuneDebugPrint(gyro_cal, sample_time);
    }
}
#endif