/*
 * 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 "calibration/gyroscope/gyro_cal.h"

#include <float.h>
#include <math.h>
#include <string.h>

#include "calibration/util/cal_log.h"
#include "common/math/vec.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.2f)  // [rad/sec]

// Converts units of radians to milli-degrees.
#define RAD_TO_MILLI_DEGREES (float)(1e3f * 180.0f / NANO_PI)

// Watchdog timeout value (5 seconds). Monitors dropouts in sensor data and
// resets when exceeded.
#define GYRO_WATCHDOG_TIMEOUT_NANOS (5000000000)

#ifdef GYRO_CAL_DBG_ENABLED
// The time value used to throttle debug messaging.
#define GYROCAL_WAIT_TIME_NANOS (100000000)

// Unit conversion: nanoseconds to seconds.
#define NANOS_TO_SEC (1.0e-9f)

// A debug version label to help with tracking results.
#define GYROCAL_DEBUG_VERSION_STRING "[July 05, 2017]"

// Debug log tag string used to identify debug report output data.
#define GYROCAL_REPORT_TAG "[GYRO_CAL:REPORT]"

// Debug log tag string used to identify debug tuning output data.
#define GYROCAL_TUNE_TAG "[GYRO_CAL:TUNE]"
#endif  // GYRO_CAL_DBG_ENABLED

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

static void deviceStillnessCheck(struct GyroCal* gyro_cal,
                                 uint64_t sample_time_nanos);

static void computeGyroCal(struct GyroCal* gyro_cal,
                           uint64_t calibration_time_nanos);

static void checkWatchdog(struct GyroCal* gyro_cal, uint64_t sample_time_nanos);

// Data tracker command enumeration.
enum GyroCalTrackerCommand {
  DO_RESET = 0,    // Resets the local data used for data tracking.
  DO_UPDATE_DATA,  // Updates the local tracking data.
  DO_STORE_DATA,   // Stores intermediate results for later recall.
  DO_EVALUATE      // Computes and provides the results of the gate function.
};

/*
 * Updates the temperature min/max and mean during the stillness period. Returns
 * 'true' if the min and max temperature values exceed the range set by
 * 'temperature_delta_limit_celsius'.
 *
 * INPUTS:
 *   gyro_cal:     Pointer to the GyroCal data structure.
 *   temperature_celsius:  New temperature sample to include.
 *   do_this:      Command enumerator that controls function behavior:
 */
static bool gyroTemperatureStatsTracker(struct GyroCal* gyro_cal,
                                        float temperature_celsius,
                                        enum GyroCalTrackerCommand do_this);

/*
 * Tracks the minimum and maximum gyroscope stillness window means.
 * Returns 'true' when the difference between gyroscope min and max window
 * means are outside the range set by 'stillness_mean_delta_limit'.
 *
 * INPUTS:
 *   gyro_cal:     Pointer to the GyroCal data structure.
 *   do_this:      Command enumerator that controls function behavior.
 */
static bool gyroStillMeanTracker(struct GyroCal* gyro_cal,
                                 enum GyroCalTrackerCommand do_this);

#ifdef GYRO_CAL_DBG_ENABLED
// Defines the type of debug data to print.
enum DebugPrintData {
  OFFSET = 0,
  STILLNESS_DATA,
  SAMPLE_RATE_AND_TEMPERATURE,
  GYRO_MINMAX_STILLNESS_MEAN,
  ACCEL_STATS,
  GYRO_STATS,
  MAG_STATS,
  ACCEL_STATS_TUNING,
  GYRO_STATS_TUNING,
  MAG_STATS_TUNING
};

/*
 * Updates running calculation of the gyro's mean sampling rate.
 *
 * Behavior:
 *   1)  If 'debug_mean_sampling_rate_hz' pointer is not NULL then the local
 *       calculation of the sampling rate is copied, and the function returns.
 *   2)  Else, if 'reset_stats' is 'true' then the local estimate is reset and
 *       the function returns.
 *   3)  Otherwise, the local estimate of the mean sampling rates is updated.
 *
 * INPUTS:
 *   debug_mean_sampling_rate_hz:   Pointer to the mean sampling rate to update.
 *   timestamp_nanos:  Time stamp (nanoseconds).
 *   reset_stats:  Flag that signals a reset of the sampling rate estimate.
 */
static void gyroSamplingRateUpdate(float* debug_mean_sampling_rate_hz,
                                   uint64_t timestamp_nanos, bool reset_stats);

// Updates the information used for debug printouts.
static void gyroCalUpdateDebug(struct GyroCal* gyro_cal);

// Helper function for printing out common debug data.
static void gyroCalDebugPrintData(const struct GyroCal* gyro_cal,
                                  char* debug_tag,
                                  enum DebugPrintData print_data);

// This conversion function is necessary for Nanohub firmware compilation (i.e.,
// can't cast a uint64_t to a float directly). This conversion function was
// copied from: /third_party/contexthub/firmware/src/floatRt.c
static float floatFromUint64(uint64_t v)
{
    uint32_t hi = v >> 32, lo = v;

    if (!hi) //this is very fast for cases where we fit into a uint32_t
        return(float)lo;
    else {
        return ((float)hi) * 4294967296.0f + (float)lo;
    }
}

#ifdef GYRO_CAL_DBG_TUNE_ENABLED
// Prints debug information useful for tuning the GyroCal parameters.
static void gyroCalTuneDebugPrint(const struct GyroCal* gyro_cal,
                                  uint64_t timestamp_nanos);
#endif  // GYRO_CAL_DBG_TUNE_ENABLED
#endif  // GYRO_CAL_DBG_ENABLED

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

// Initialize the gyro calibration data structure.
void gyroCalInit(struct GyroCal* gyro_cal, uint64_t min_still_duration_nanos,
                 uint64_t max_still_duration_nanos, float bias_x, float bias_y,
                 float bias_z, uint64_t calibration_time_nanos,
                 uint64_t window_time_duration_nanos, 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,
                 float stillness_mean_delta_limit,
                 float temperature_delta_limit_celsius,
                 bool gyro_calibration_enable) {
  // Clear gyro_cal structure memory.
  memset(gyro_cal, 0, sizeof(struct GyroCal));

  // 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_nanos = 0;

  // Set the min and max window stillness duration.
  gyro_cal->min_still_duration_nanos = min_still_duration_nanos;
  gyro_cal->max_still_duration_nanos = max_still_duration_nanos;

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

  // Set the watchdog timeout duration.
  gyro_cal->gyro_watchdog_timeout_duration_nanos = GYRO_WATCHDOG_TIMEOUT_NANOS;

  // 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_nanos = calibration_time_nanos;

  // 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_nanos = 0;

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

  // Sets the stability limit for the stillness window mean acceptable delta.
  gyro_cal->stillness_mean_delta_limit = stillness_mean_delta_limit;

  // Sets the min/max temperature delta limit for the stillness period.
  gyro_cal->temperature_delta_limit_celsius = temperature_delta_limit_celsius;

  // Ensures that the data tracking functionality is reset.
  gyroStillMeanTracker(gyro_cal, DO_RESET);
  gyroTemperatureStatsTracker(gyro_cal, 0.0f, DO_RESET);

#ifdef GYRO_CAL_DBG_ENABLED
  if (gyro_cal->gyro_calibration_enable) {
    CAL_DEBUG_LOG("[GYRO_CAL:INIT]", "Online gyroscope calibration ENABLED.");
  } else {
    CAL_DEBUG_LOG("[GYRO_CAL:INIT]", "Online gyroscope calibration DISABLED.");
  }

  // Ensures that the gyro sampling rate estimate is reset.
  gyroSamplingRateUpdate(NULL, 0, /*reset_stats=*/true);
#endif  // GYRO_CAL_DBG_ENABLED
}

// Void pointer in the gyro calibration data structure (doesn't do anything
// except prevent compiler warnings).
void gyroCalDestroy(struct GyroCal* gyro_cal) {
  (void)gyro_cal;
}

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

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

#ifdef GYRO_CAL_DBG_ENABLED
  CAL_DEBUG_LOG("[GYRO_CAL:SET BIAS]",
                "Gyro Bias Calibration [mDPS]: %s%d.%03d, %s%d.%03d, %s%d.%03d",
                CAL_ENCODE_FLOAT(gyro_cal->bias_x * RAD_TO_MILLI_DEGREES, 3),
                CAL_ENCODE_FLOAT(gyro_cal->bias_y * RAD_TO_MILLI_DEGREES, 3),
                CAL_ENCODE_FLOAT(gyro_cal->bias_z * RAD_TO_MILLI_DEGREES, 3));
#endif  // GYRO_CAL_DBG_ENABLED
}

// Remove bias from a gyro measurement [rad/sec].
void gyroCalRemoveBias(struct GyroCal* 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* 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* gyro_cal, uint64_t sample_time_nanos,
                       float x, float y, float z, float temperature_celsius) {
  // Make sure that a valid window end-time is set, and start the watchdog
  // timer.
  if (gyro_cal->stillness_win_endtime_nanos <= 0) {
    gyro_cal->stillness_win_endtime_nanos =
        sample_time_nanos + gyro_cal->window_time_duration_nanos;

    // Start the watchdog timer.
    gyro_cal->gyro_watchdog_start_nanos = sample_time_nanos;
  }

  // Update the temperature statistics.
  gyroTemperatureStatsTracker(gyro_cal, temperature_celsius, DO_UPDATE_DATA);

#ifdef GYRO_CAL_DBG_ENABLED
  // Update the gyro sampling rate estimate.
  gyroSamplingRateUpdate(NULL, sample_time_nanos, /*reset_stats=*/false);
#endif  // GYRO_CAL_DBG_ENABLED

  // Pass gyro data to stillness detector
  gyroStillDetUpdate(&gyro_cal->gyro_stillness_detect,
                     gyro_cal->stillness_win_endtime_nanos, sample_time_nanos,
                     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_nanos);
}

// Update the gyro calibration with mag data [micro Tesla].
void gyroCalUpdateMag(struct GyroCal* gyro_cal, uint64_t sample_time_nanos,
                      float x, float y, float z) {
  // Pass magnetometer data to stillness detector.
  gyroStillDetUpdate(&gyro_cal->mag_stillness_detect,
                     gyro_cal->stillness_win_endtime_nanos, sample_time_nanos,
                     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_nanos);
}

// Update the gyro calibration with accel data [m/sec^2].
void gyroCalUpdateAccel(struct GyroCal* gyro_cal, uint64_t sample_time_nanos,
                        float x, float y, float z) {
  // Pass accelerometer data to stillnesss detector.
  gyroStillDetUpdate(&gyro_cal->accel_stillness_detect,
                     gyro_cal->stillness_win_endtime_nanos, sample_time_nanos,
                     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_nanos);
}

// TODO: Consider breaking this function up to improve readability.
// Checks the state of all stillness detectors to determine
// whether the device is "still".
void deviceStillnessCheck(struct GyroCal* gyro_cal,
                          uint64_t sample_time_nanos) {
  bool stillness_duration_exceeded = false;
  bool stillness_duration_too_short = false;
  bool min_max_temp_exceeded = false;
  bool mean_not_stable = 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_nanos);

  // 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_nanos =
      sample_time_nanos + gyro_cal->window_time_duration_nanos;

  // 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;
  }

  // Updates the mean tracker data.
  gyroStillMeanTracker(gyro_cal, DO_UPDATE_DATA);

  // 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;

  // Evaluate the mean and temperature gate functions.
  mean_not_stable = gyroStillMeanTracker(gyro_cal, DO_EVALUATE);
  min_max_temp_exceeded =
      gyroTemperatureStatsTracker(gyro_cal, 0.0f, DO_EVALUATE);

  // Determines if the device is currently still.
  device_is_still = (conf_still > gyro_cal->stillness_threshold) &&
      !mean_not_stable && !min_max_temp_exceeded;

  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_nanos =
          gyro_cal->gyro_stillness_detect.window_start_time;
    }

    // Check to see if current stillness period exceeds the desired limit.
    stillness_duration_exceeded =
        (gyro_cal->gyro_stillness_detect.last_sample_time >=
         gyro_cal->start_still_time_nanos + gyro_cal->max_still_duration_nanos);

    // Track the new stillness mean and temperature data.
    gyroStillMeanTracker(gyro_cal, DO_STORE_DATA);
    gyroTemperatureStatsTracker(gyro_cal, 0.0f, DO_STORE_DATA);

    if (stillness_duration_exceeded) {
      // The current stillness has gone too long. Do a calibration with the
      // current data and reset.

      // Updates the gyro bias estimate with the current window data and
      // resets the stats.
      gyroStillDetReset(&gyro_cal->accel_stillness_detect,
                        /*reset_stats=*/true);
      gyroStillDetReset(&gyro_cal->gyro_stillness_detect, /*reset_stats=*/true);
      gyroStillDetReset(&gyro_cal->mag_stillness_detect, /*reset_stats=*/true);

      // Resets the local calculations because the stillness period is over.
      gyroStillMeanTracker(gyro_cal, DO_RESET);
      gyroTemperatureStatsTracker(gyro_cal, 0.0f, DO_RESET);

      // Computes a new gyro offset estimate.
      computeGyroCal(gyro_cal,
                     gyro_cal->gyro_stillness_detect.last_sample_time);

#ifdef GYRO_CAL_DBG_ENABLED
      // Resets the sampling rate estimate.
      gyroSamplingRateUpdate(NULL, sample_time_nanos, /*reset_stats=*/true);
#endif  // GYRO_CAL_DBG_ENABLED

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

      // Extend the stillness period.
      gyroStillDetReset(&gyro_cal->accel_stillness_detect,
                        /*reset_stats=*/false);
      gyroStillDetReset(&gyro_cal->gyro_stillness_detect,
                        /*reset_stats=*/false);
      gyroStillDetReset(&gyro_cal->mag_stillness_detect, /*reset_stats=*/false);

      // Update the 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_nanos + gyro_cal->min_still_duration_nanos);

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

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

    // Resets the temperature and sensor mean data.
    gyroTemperatureStatsTracker(gyro_cal, 0.0f, DO_RESET);
    gyroStillMeanTracker(gyro_cal, DO_RESET);

#ifdef GYRO_CAL_DBG_ENABLED
    // Resets the sampling rate estimate.
    gyroSamplingRateUpdate(NULL, sample_time_nanos, /*reset_stats=*/true);
#endif  // GYRO_CAL_DBG_ENABLED

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

  // Reset the watchdog timer after we have processed data.
  gyro_cal->gyro_watchdog_start_nanos = sample_time_nanos;
}

// Calculates a new gyro bias offset calibration value.
void computeGyroCal(struct GyroCal* gyro_cal, uint64_t calibration_time_nanos) {
  // 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)) {
#ifdef GYRO_CAL_DBG_ENABLED
    CAL_DEBUG_LOG("[GYRO_CAL:REJECT]",
                  "Offset|Temp|Time [mDPS|C|nsec]: %s%d.%03d, %s%d.%03d, "
                  "%s%d.%03d, %s%d.%03d, %llu",
                  CAL_ENCODE_FLOAT(gyro_cal->gyro_stillness_detect.prev_mean_x *
                                       RAD_TO_MILLI_DEGREES,
                                   3),
                  CAL_ENCODE_FLOAT(gyro_cal->gyro_stillness_detect.prev_mean_y *
                                       RAD_TO_MILLI_DEGREES,
                                   3),
                  CAL_ENCODE_FLOAT(gyro_cal->gyro_stillness_detect.prev_mean_z *
                                       RAD_TO_MILLI_DEGREES,
                                   3),
                  CAL_ENCODE_FLOAT(gyro_cal->temperature_mean_celsius, 3),
                  (unsigned long long int)calibration_time_nanos);
#endif  // GYRO_CAL_DBG_ENABLED

    // Outside of range. Ignore, reset, and continue.
    return;
  }

  // Record the 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;

  // Store the calibration temperature (using the mean temperature over the
  // "stillness" period).
  gyro_cal->bias_temperature_celsius = gyro_cal->temperature_mean_celsius;

  // Store the calibration time stamp.
  gyro_cal->calibration_time_nanos = calibration_time_nanos;

  // Record the 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;

  // 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++;

  // Update the calibration debug information and trigger a printout.
  gyroCalUpdateDebug(gyro_cal);
#endif
}

// Check for a watchdog timeout condition.
void checkWatchdog(struct GyroCal* gyro_cal, uint64_t sample_time_nanos) {
  bool watchdog_timeout;

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

  // Checks for the following watchdog timeout conditions:
  //    i.  The current timestamp has exceeded the allowed watchdog duration.
  //    ii. A timestamp was received that has jumped backwards by more than the
  //        allowed watchdog duration (e.g., timestamp clock roll-over).
  watchdog_timeout =
      (sample_time_nanos > gyro_cal->gyro_watchdog_timeout_duration_nanos +
                               gyro_cal->gyro_watchdog_start_nanos) ||
      (sample_time_nanos + gyro_cal->gyro_watchdog_timeout_duration_nanos <
       gyro_cal->gyro_watchdog_start_nanos);

  // If a timeout occurred then reset to known good state.
  if (watchdog_timeout) {
#ifdef GYRO_CAL_DBG_ENABLED
    gyro_cal->debug_watchdog_count++;
    if (sample_time_nanos < gyro_cal->gyro_watchdog_start_nanos) {
      CAL_DEBUG_LOG(
          "[GYRO_CAL:WATCHDOG]",
          "Total#, Timestamp | Delta [nsec]: %lu, %llu, -%llu",
          (unsigned long int)gyro_cal->debug_watchdog_count,
          (unsigned long long int)sample_time_nanos,
          (unsigned long long int)(gyro_cal->gyro_watchdog_start_nanos -
                                   sample_time_nanos));
    } else {
      CAL_DEBUG_LOG(
          "[GYRO_CAL:WATCHDOG]",
          "Total#, Timestamp | Delta  [nsec]: %lu, %llu, %llu",
          (unsigned long int)gyro_cal->debug_watchdog_count,
          (unsigned long long int)sample_time_nanos,
          (unsigned long long int)(sample_time_nanos -
                                   gyro_cal->gyro_watchdog_start_nanos));
    }
#endif  // GYRO_CAL_DBG_ENABLED

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

    // Resets the temperature and sensor mean data.
    gyroTemperatureStatsTracker(gyro_cal, 0.0f, DO_RESET);
    gyroStillMeanTracker(gyro_cal, DO_RESET);

#ifdef GYRO_CAL_DBG_ENABLED
    // Resets the sampling rate estimate.
    gyroSamplingRateUpdate(NULL, sample_time_nanos, /*reset_stats=*/true);
#endif  // GYRO_CAL_DBG_ENABLED

    // Resets the stillness window end-time.
    gyro_cal->stillness_win_endtime_nanos = 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_nanos = 0;
  }
}

// TODO -- Combine the following two functions into one or consider
// implementing a separate helper module for tracking the temperature and mean
// statistics.
bool gyroTemperatureStatsTracker(struct GyroCal* gyro_cal,
                                 float temperature_celsius,
                                 enum GyroCalTrackerCommand do_this) {
  bool min_max_temp_exceeded = false;

  switch (do_this) {
    case DO_RESET:
      // Resets the mean accumulator.
      gyro_cal->temperature_mean_tracker.num_points = 0;
      gyro_cal->temperature_mean_tracker.mean_accumulator = 0.0f;

      // Initializes the min/max temperatures values.
      gyro_cal->temperature_mean_tracker.temperature_min_max_celsius[0] =
          FLT_MAX;
      gyro_cal->temperature_mean_tracker.temperature_min_max_celsius[1] =
          -FLT_MAX;
      break;

    case DO_UPDATE_DATA:
      // Does the mean accumulation.
      gyro_cal->temperature_mean_tracker.mean_accumulator +=
          temperature_celsius;
      gyro_cal->temperature_mean_tracker.num_points++;

      // Tracks the min, max, and latest temperature values.
      gyro_cal->temperature_mean_tracker.latest_temperature_celsius =
          temperature_celsius;
      if (gyro_cal->temperature_mean_tracker.temperature_min_max_celsius[0] >
          temperature_celsius) {
        gyro_cal->temperature_mean_tracker.temperature_min_max_celsius[0] =
            temperature_celsius;
      }
      if (gyro_cal->temperature_mean_tracker.temperature_min_max_celsius[1] <
          temperature_celsius) {
        gyro_cal->temperature_mean_tracker.temperature_min_max_celsius[1] =
            temperature_celsius;
      }
      break;

    case DO_STORE_DATA:
      // Store the most recent temperature statistics data to the GyroCal data
      // structure. This functionality allows previous results to be recalled
      // when the device suddenly becomes "not still".
      if (gyro_cal->temperature_mean_tracker.num_points > 0) {
        gyro_cal->temperature_mean_celsius =
            gyro_cal->temperature_mean_tracker.mean_accumulator /
            gyro_cal->temperature_mean_tracker.num_points;
      } else {
        gyro_cal->temperature_mean_celsius =
            gyro_cal->temperature_mean_tracker.latest_temperature_celsius;
#ifdef GYRO_CAL_DBG_ENABLED
        CAL_DEBUG_LOG("[GYRO_CAL:TEMP_GATE]",
                      "Insufficient statistics (num_points = 0), using latest "
                      "measured temperature as the mean value.");
#endif  // GYRO_CAL_DBG_ENABLED
      }
#ifdef GYRO_CAL_DBG_ENABLED
      // Records the min/max and mean temperature values for debug purposes.
      gyro_cal->debug_gyro_cal.temperature_mean_celsius =
          gyro_cal->temperature_mean_celsius;
      memcpy(gyro_cal->debug_gyro_cal.temperature_min_max_celsius,
             gyro_cal->temperature_mean_tracker.temperature_min_max_celsius,
             2 * sizeof(float));
#endif
      break;

    case DO_EVALUATE:
      // Determines if the min/max delta exceeded the set limit.
      if (gyro_cal->temperature_mean_tracker.num_points > 0) {
        min_max_temp_exceeded =
            (gyro_cal->temperature_mean_tracker.temperature_min_max_celsius[1] -
             gyro_cal->temperature_mean_tracker
                 .temperature_min_max_celsius[0]) >
            gyro_cal->temperature_delta_limit_celsius;

#ifdef GYRO_CAL_DBG_ENABLED
        if (min_max_temp_exceeded) {
          CAL_DEBUG_LOG(
              "[GYRO_CAL:TEMP_GATE]",
              "Exceeded the max temperature variation during stillness.");
        }
#endif  // GYRO_CAL_DBG_ENABLED
      }
      break;

    default:
      break;
  }

  return min_max_temp_exceeded;
}

bool gyroStillMeanTracker(struct GyroCal* gyro_cal,
                          enum GyroCalTrackerCommand do_this) {
  static float gyro_winmean_min[3] = {0.0f, 0.0f, 0.0f};
  static float gyro_winmean_max[3] = {0.0f, 0.0f, 0.0f};
  bool mean_not_stable = false;
  size_t i;

  switch (do_this) {
    case DO_RESET:
      // Resets the min/max window mean values to a default value.
      for (i = 0; i < 3; i++) {
        gyro_winmean_min[i] = FLT_MAX;
        gyro_winmean_max[i] = -FLT_MAX;
      }
      break;

    case DO_UPDATE_DATA:
      // Computes the min/max window mean values.
      if (gyro_winmean_min[0] > gyro_cal->gyro_stillness_detect.win_mean_x) {
        gyro_winmean_min[0] = gyro_cal->gyro_stillness_detect.win_mean_x;
      }
      if (gyro_winmean_max[0] < gyro_cal->gyro_stillness_detect.win_mean_x) {
        gyro_winmean_max[0] = gyro_cal->gyro_stillness_detect.win_mean_x;
      }

      if (gyro_winmean_min[1] > gyro_cal->gyro_stillness_detect.win_mean_y) {
        gyro_winmean_min[1] = gyro_cal->gyro_stillness_detect.win_mean_y;
      }
      if (gyro_winmean_max[1] < gyro_cal->gyro_stillness_detect.win_mean_y) {
        gyro_winmean_max[1] = gyro_cal->gyro_stillness_detect.win_mean_y;
      }

      if (gyro_winmean_min[2] > gyro_cal->gyro_stillness_detect.win_mean_z) {
        gyro_winmean_min[2] = gyro_cal->gyro_stillness_detect.win_mean_z;
      }
      if (gyro_winmean_max[2] < gyro_cal->gyro_stillness_detect.win_mean_z) {
        gyro_winmean_max[2] = gyro_cal->gyro_stillness_detect.win_mean_z;
      }
      break;

    case DO_STORE_DATA:
      // Store the most recent "stillness" mean data to the GyroCal data
      // structure. This functionality allows previous results to be recalled
      // when the device suddenly becomes "not still".
      memcpy(gyro_cal->gyro_winmean_min, gyro_winmean_min, 3 * sizeof(float));
      memcpy(gyro_cal->gyro_winmean_max, gyro_winmean_max, 3 * sizeof(float));
    break;

    case DO_EVALUATE:
      // Performs the stability check and returns the 'true' if the difference
      // between min/max window mean value is outside the stable range.
      for (i = 0; i < 3; i++) {
        mean_not_stable |= (gyro_winmean_max[i] - gyro_winmean_min[i]) >
                           gyro_cal->stillness_mean_delta_limit;
      }
#ifdef GYRO_CAL_DBG_ENABLED
      if (mean_not_stable) {
        CAL_DEBUG_LOG(
            "[GYRO_CAL:MEAN_STABILITY_GATE]",
            "Variation Limit|Delta [mDPS]: %s%d.%03d | %s%d.%03d, %s%d.%03d, "
            "%s%d.%03d",
            CAL_ENCODE_FLOAT(
                gyro_cal->stillness_mean_delta_limit * RAD_TO_MILLI_DEGREES, 3),
            CAL_ENCODE_FLOAT((gyro_winmean_max[0] - gyro_winmean_min[0]) *
                                 RAD_TO_MILLI_DEGREES,
                             3),
            CAL_ENCODE_FLOAT((gyro_winmean_max[1] - gyro_winmean_min[1]) *
                                 RAD_TO_MILLI_DEGREES,
                             3),
            CAL_ENCODE_FLOAT((gyro_winmean_max[2] - gyro_winmean_min[2]) *
                                 RAD_TO_MILLI_DEGREES,
                             3));
      }
#endif  // GYRO_CAL_DBG_ENABLED
      break;

    default:
      break;
  }

  return mean_not_stable;
}

#ifdef GYRO_CAL_DBG_ENABLED
void gyroSamplingRateUpdate(float* debug_mean_sampling_rate_hz,
                            uint64_t timestamp_nanos, bool reset_stats) {
  // This is used for local calculations of average sampling rate.
  static uint64_t last_timestamp_nanos = 0;
  static uint64_t time_delta_accumulator = 0;
  static size_t num_samples = 0;

  // If 'debug_mean_sampling_rate_hz' is not NULL then this function just reads
  // out the estimate of the sampling rate.
  if (debug_mean_sampling_rate_hz) {
    if (num_samples > 1 && time_delta_accumulator > 0) {
      // Computes the final mean calculation.
      *debug_mean_sampling_rate_hz =
          num_samples /
          (floatFromUint64(time_delta_accumulator) * NANOS_TO_SEC);
    } else {
      // Not enough samples to compute a valid sample rate estimate. Indicate
      // this with a -1 value.
      *debug_mean_sampling_rate_hz = -1.0f;
    }
    reset_stats = true;
  }

  // Resets the sampling rate mean estimator data.
  if (reset_stats) {
    last_timestamp_nanos = 0;
    time_delta_accumulator = 0;
    num_samples = 0;
    return;
  }

  // Skip adding this data to the accumulator if:
  //   1. A bad timestamp was received (i.e., time not monotonic).
  //   2. 'last_timestamp_nanos' is zero.
  if (timestamp_nanos <= last_timestamp_nanos || last_timestamp_nanos == 0) {
    last_timestamp_nanos = timestamp_nanos;
    return;
  }

  // Increments the number of samples.
  num_samples++;

  // Accumulate the time steps.
  time_delta_accumulator += timestamp_nanos - last_timestamp_nanos;
  last_timestamp_nanos = timestamp_nanos;
}

void gyroCalUpdateDebug(struct GyroCal* gyro_cal) {
  // Only update this data if debug printing is not currently in progress
  // (i.e., don't want to risk overwriting debug information that is actively
  // being reported).
  if (gyro_cal->debug_state != GYRO_IDLE) {
    return;
  }

  // Probability of stillness (acc, rot, still), duration, timestamp.
  gyro_cal->debug_gyro_cal.accel_stillness_conf =
      gyro_cal->accel_stillness_detect.prev_stillness_confidence;
  gyro_cal->debug_gyro_cal.gyro_stillness_conf =
      gyro_cal->gyro_stillness_detect.prev_stillness_confidence;
  gyro_cal->debug_gyro_cal.mag_stillness_conf =
      gyro_cal->mag_stillness_detect.prev_stillness_confidence;

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

  // Stillness start, stop, and duration times.
  gyro_cal->debug_gyro_cal.start_still_time_nanos =
      gyro_cal->start_still_time_nanos;
  gyro_cal->debug_gyro_cal.end_still_time_nanos =
      gyro_cal->calibration_time_nanos;
  gyro_cal->debug_gyro_cal.stillness_duration_nanos =
      gyro_cal->calibration_time_nanos - gyro_cal->start_still_time_nanos;

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

  // Records the mean gyroscope sampling rate.
  gyroSamplingRateUpdate(&gyro_cal->debug_gyro_cal.mean_sampling_rate_hz, 0,
                         /*reset_stats=*/true);

  // Records the min/max gyroscope window stillness mean values.
  memcpy(gyro_cal->debug_gyro_cal.gyro_winmean_min, gyro_cal->gyro_winmean_min,
         3 * sizeof(float));
  memcpy(gyro_cal->debug_gyro_cal.gyro_winmean_max, gyro_cal->gyro_winmean_max,
         3 * sizeof(float));

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

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

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

  // Records the variance data.
  // NOTE: These statistics include the final captured window, which may be
  // outside of the "stillness" period. Therefore, these values may exceed the
  // stillness thresholds.
  gyro_cal->debug_gyro_cal.accel_var[0] =
      gyro_cal->accel_stillness_detect.win_var_x;
  gyro_cal->debug_gyro_cal.accel_var[1] =
      gyro_cal->accel_stillness_detect.win_var_y;
  gyro_cal->debug_gyro_cal.accel_var[2] =
      gyro_cal->accel_stillness_detect.win_var_z;

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

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

  // Trigger a printout of the debug information.
  gyro_cal->debug_print_trigger = true;
}

void gyroCalDebugPrintData(const struct GyroCal* gyro_cal, char* debug_tag,
                           enum DebugPrintData print_data) {
  // Prints out the desired debug data.
  float mag_data;
  switch (print_data) {
    case OFFSET:
      CAL_DEBUG_LOG(debug_tag,
                    "Cal#|Offset|Temp|Time [mDPS|C|nsec]: %lu, %s%d.%03d, "
                    "%s%d.%03d, %s%d.%03d, %s%d.%03d, %llu",
                    (unsigned long int)gyro_cal->debug_calibration_count,
                    CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.calibration[0] *
                                         RAD_TO_MILLI_DEGREES,
                                     3),
                    CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.calibration[1] *
                                         RAD_TO_MILLI_DEGREES,
                                     3),
                    CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.calibration[2] *
                                         RAD_TO_MILLI_DEGREES,
                                     3),
                    CAL_ENCODE_FLOAT(
                        gyro_cal->debug_gyro_cal.temperature_mean_celsius, 3),
                    (unsigned long long int)
                        gyro_cal->debug_gyro_cal.end_still_time_nanos);
      break;

    case STILLNESS_DATA:
      mag_data = (gyro_cal->debug_gyro_cal.using_mag_sensor)
                     ? gyro_cal->debug_gyro_cal.mag_stillness_conf
                     : -1.0f;  // Signals that magnetometer was not used.
      CAL_DEBUG_LOG(
          debug_tag,
          "Cal#|Stillness|Confidence [nsec]: %lu, %llu, %s%d.%03d, %s%d.%03d, "
          "%s%d.%03d",
          (unsigned long int)gyro_cal->debug_calibration_count,
          (unsigned long long int)(gyro_cal->debug_gyro_cal
                                       .end_still_time_nanos -
                                   gyro_cal->debug_gyro_cal
                                       .start_still_time_nanos),
          CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.gyro_stillness_conf, 3),
          CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_stillness_conf, 3),
          CAL_ENCODE_FLOAT(mag_data, 3));
      break;

    case SAMPLE_RATE_AND_TEMPERATURE:
      CAL_DEBUG_LOG(
          debug_tag,
          "Cal#|Mean|Min|Max|Delta|Sample Rate [C|Hz]: %lu, %s%d.%03d, "
          "%s%d.%03d, %s%d.%03d, %s%d.%04d, %s%d.%03d",
          (unsigned long int)gyro_cal->debug_calibration_count,
          CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.temperature_mean_celsius,
                           3),
          CAL_ENCODE_FLOAT(
              gyro_cal->debug_gyro_cal.temperature_min_max_celsius[0], 3),
          CAL_ENCODE_FLOAT(
              gyro_cal->debug_gyro_cal.temperature_min_max_celsius[1], 3),
          CAL_ENCODE_FLOAT(
              gyro_cal->debug_gyro_cal.temperature_min_max_celsius[1] -
                  gyro_cal->debug_gyro_cal.temperature_min_max_celsius[0],
              4),
          CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mean_sampling_rate_hz, 3));
      break;

    case GYRO_MINMAX_STILLNESS_MEAN:
      CAL_DEBUG_LOG(
          debug_tag,
          "Cal#|Gyro Peak Stillness Variation [mDPS]: %lu, %s%d.%03d, "
          "%s%d.%03d, %s%d.%03d",
          (unsigned long int)gyro_cal->debug_calibration_count,
          CAL_ENCODE_FLOAT((gyro_cal->debug_gyro_cal.gyro_winmean_max[0] -
                            gyro_cal->debug_gyro_cal.gyro_winmean_min[0]) *
                               RAD_TO_MILLI_DEGREES,
                           3),
          CAL_ENCODE_FLOAT((gyro_cal->debug_gyro_cal.gyro_winmean_max[1] -
                            gyro_cal->debug_gyro_cal.gyro_winmean_min[1]) *
                               RAD_TO_MILLI_DEGREES,
                           3),
          CAL_ENCODE_FLOAT((gyro_cal->debug_gyro_cal.gyro_winmean_max[2] -
                            gyro_cal->debug_gyro_cal.gyro_winmean_min[2]) *
                               RAD_TO_MILLI_DEGREES,
                           3));
      break;

    case ACCEL_STATS:
      CAL_DEBUG_LOG(
          debug_tag,
          "Cal#|Accel Mean|Var [m/sec^2|(m/sec^2)^2]: %lu, "
          "%s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%06d, %s%d.%06d, %s%d.%06d",
          (unsigned long int)gyro_cal->debug_calibration_count,
          CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_mean[0], 3),
          CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_mean[1], 3),
          CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_mean[2], 3),
          CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_var[0], 6),
          CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_var[1], 6),
          CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_var[2], 6));
      break;

    case GYRO_STATS:
      CAL_DEBUG_LOG(
          debug_tag,
          "Cal#|Gyro Mean|Var [mDPS|mDPS^2]: %lu, %s%d.%03d, "
          "%s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d",
          (unsigned long int)gyro_cal->debug_calibration_count,
          CAL_ENCODE_FLOAT(
              gyro_cal->debug_gyro_cal.gyro_mean[0] * RAD_TO_MILLI_DEGREES, 3),
          CAL_ENCODE_FLOAT(
              gyro_cal->debug_gyro_cal.gyro_mean[1] * RAD_TO_MILLI_DEGREES, 3),
          CAL_ENCODE_FLOAT(
              gyro_cal->debug_gyro_cal.gyro_mean[2] * RAD_TO_MILLI_DEGREES, 3),
          CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.gyro_var[0] *
                               RAD_TO_MILLI_DEGREES * RAD_TO_MILLI_DEGREES,
                           3),
          CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.gyro_var[1] *
                               RAD_TO_MILLI_DEGREES * RAD_TO_MILLI_DEGREES,
                           3),
          CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.gyro_var[2] *
                               RAD_TO_MILLI_DEGREES * RAD_TO_MILLI_DEGREES,
                           3));
      break;

    case MAG_STATS:
      if (gyro_cal->debug_gyro_cal.using_mag_sensor) {
        CAL_DEBUG_LOG(debug_tag,
                      "Cal#|Mag Mean|Var [uT|uT^2]: %lu, %s%d.%03d, "
                      "%s%d.%03d, %s%d.%03d, %s%d.%06d, %s%d.%06d, %s%d.%06d",
                      (unsigned long int)gyro_cal->debug_calibration_count,
                      CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_mean[0], 3),
                      CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_mean[1], 3),
                      CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_mean[2], 3),
                      CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_var[0], 6),
                      CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_var[1], 6),
                      CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_var[2], 6));
      } else {
        CAL_DEBUG_LOG(debug_tag,
                      "Cal#|Mag Mean|Var [uT|uT^2]: %lu, 0, 0, 0, -1.0, -1.0, "
                      "-1.0",
                      (unsigned long int)gyro_cal->debug_calibration_count);
      }
      break;

#ifdef GYRO_CAL_DBG_TUNE_ENABLED
    case ACCEL_STATS_TUNING:
      CAL_DEBUG_LOG(
          debug_tag,
          "Accel Mean|Var [m/sec^2|(m/sec^2)^2]: %s%d.%03d, "
          "%s%d.%03d, %s%d.%03d, %s%d.%06d, %s%d.%06d, %s%d.%06d",
          CAL_ENCODE_FLOAT(gyro_cal->accel_stillness_detect.prev_mean_x, 3),
          CAL_ENCODE_FLOAT(gyro_cal->accel_stillness_detect.prev_mean_y, 3),
          CAL_ENCODE_FLOAT(gyro_cal->accel_stillness_detect.prev_mean_z, 3),
          CAL_ENCODE_FLOAT(gyro_cal->accel_stillness_detect.win_var_x, 6),
          CAL_ENCODE_FLOAT(gyro_cal->accel_stillness_detect.win_var_y, 6),
          CAL_ENCODE_FLOAT(gyro_cal->accel_stillness_detect.win_var_z, 6));
      break;

    case GYRO_STATS_TUNING:
      CAL_DEBUG_LOG(
          debug_tag,
          "Gyro Mean|Var [mDPS|mDPS^2]: %s%d.%03d, %s%d.%03d, %s%d.%03d, "
          "%s%d.%06d, %s%d.%06d, %s%d.%06d",
          CAL_ENCODE_FLOAT(gyro_cal->gyro_stillness_detect.prev_mean_x *
                               RAD_TO_MILLI_DEGREES,
                           3),
          CAL_ENCODE_FLOAT(gyro_cal->gyro_stillness_detect.prev_mean_y *
                               RAD_TO_MILLI_DEGREES,
                           3),
          CAL_ENCODE_FLOAT(gyro_cal->gyro_stillness_detect.prev_mean_z *
                               RAD_TO_MILLI_DEGREES,
                           3),
          CAL_ENCODE_FLOAT(gyro_cal->gyro_stillness_detect.win_var_x *
                               RAD_TO_MILLI_DEGREES * RAD_TO_MILLI_DEGREES,
                           6),
          CAL_ENCODE_FLOAT(gyro_cal->gyro_stillness_detect.win_var_y *
                               RAD_TO_MILLI_DEGREES * RAD_TO_MILLI_DEGREES,
                           6),
          CAL_ENCODE_FLOAT(gyro_cal->gyro_stillness_detect.win_var_z *
                               RAD_TO_MILLI_DEGREES * RAD_TO_MILLI_DEGREES,
                           6));
      break;

    case MAG_STATS_TUNING:
      if (gyro_cal->using_mag_sensor) {
        CAL_DEBUG_LOG(
            debug_tag,
            "Mag Mean|Var [uT|uT^2]: %s%d.%03d, %s%d.%03d, %s%d.%03d, "
            "%s%d.%06d, %s%d.%06d, %s%d.%06d",
            CAL_ENCODE_FLOAT(gyro_cal->mag_stillness_detect.prev_mean_x, 3),
            CAL_ENCODE_FLOAT(gyro_cal->mag_stillness_detect.prev_mean_y, 3),
            CAL_ENCODE_FLOAT(gyro_cal->mag_stillness_detect.prev_mean_z, 3),
            CAL_ENCODE_FLOAT(gyro_cal->mag_stillness_detect.win_var_x, 6),
            CAL_ENCODE_FLOAT(gyro_cal->mag_stillness_detect.win_var_y, 6),
            CAL_ENCODE_FLOAT(gyro_cal->mag_stillness_detect.win_var_z, 6));
      } else {
        CAL_DEBUG_LOG(GYROCAL_TUNE_TAG,
                      "Mag Mean|Var [uT|uT^2]: 0, 0, 0, -1.0, -1.0, -1.0");
      }
      break;
#endif  // GYRO_CAL_DBG_TUNE_ENABLED

    default:
      break;
  }
}

void gyroCalDebugPrint(struct GyroCal* gyro_cal, uint64_t timestamp_nanos) {
  static enum GyroCalDebugState next_state = GYRO_IDLE;
  static uint64_t wait_timer_nanos = 0;

  // This is a state machine that controls the reporting out of debug data.
  switch (gyro_cal->debug_state) {
    case GYRO_IDLE:
      // Wait for a trigger and start the debug printout sequence.
      if (gyro_cal->debug_print_trigger) {
        CAL_DEBUG_LOG(GYROCAL_REPORT_TAG, "");
        CAL_DEBUG_LOG(GYROCAL_REPORT_TAG, "Debug Version: %s",
                      GYROCAL_DEBUG_VERSION_STRING);
        gyro_cal->debug_print_trigger = false;  // Resets trigger.
        gyro_cal->debug_state = GYRO_PRINT_OFFSET;
      } else {
        gyro_cal->debug_state = GYRO_IDLE;
      }
      break;

    case GYRO_WAIT_STATE:
      // This helps throttle the print statements.
      if (NANO_TIMER_CHECK_T1_GEQUAL_T2_PLUS_DELTA(
              timestamp_nanos, wait_timer_nanos, GYROCAL_WAIT_TIME_NANOS)) {
        gyro_cal->debug_state = next_state;
      }
      break;

    case GYRO_PRINT_OFFSET:
      gyroCalDebugPrintData(gyro_cal, GYROCAL_REPORT_TAG, OFFSET);
      wait_timer_nanos = timestamp_nanos;       // Starts the wait timer.
      next_state = GYRO_PRINT_STILLNESS_DATA;   // Sets the next state.
      gyro_cal->debug_state = GYRO_WAIT_STATE;  // First, go to wait state.
      break;

    case GYRO_PRINT_STILLNESS_DATA:
      gyroCalDebugPrintData(gyro_cal, GYROCAL_REPORT_TAG, STILLNESS_DATA);
      wait_timer_nanos = timestamp_nanos;       // Starts the wait timer.
      next_state = GYRO_PRINT_SAMPLE_RATE_AND_TEMPERATURE;  // Sets next state.
      gyro_cal->debug_state = GYRO_WAIT_STATE;  // First, go to wait state.
      break;

    case GYRO_PRINT_SAMPLE_RATE_AND_TEMPERATURE:
      gyroCalDebugPrintData(gyro_cal, GYROCAL_REPORT_TAG,
                            SAMPLE_RATE_AND_TEMPERATURE);
      wait_timer_nanos = timestamp_nanos;       // Starts the wait timer.
      next_state = GYRO_PRINT_GYRO_MINMAX_STILLNESS_MEAN;  // Sets next state.
      gyro_cal->debug_state = GYRO_WAIT_STATE;  // First, go to wait state.
      break;

    case GYRO_PRINT_GYRO_MINMAX_STILLNESS_MEAN:
      gyroCalDebugPrintData(gyro_cal, GYROCAL_REPORT_TAG,
                            GYRO_MINMAX_STILLNESS_MEAN);
      wait_timer_nanos = timestamp_nanos;       // Starts the wait timer.
      next_state = GYRO_PRINT_ACCEL_STATS;      // Sets the next state.
      gyro_cal->debug_state = GYRO_WAIT_STATE;  // First, go to wait state.
      break;

    case GYRO_PRINT_ACCEL_STATS:
      gyroCalDebugPrintData(gyro_cal, GYROCAL_REPORT_TAG, ACCEL_STATS);
      wait_timer_nanos = timestamp_nanos;       // Starts the wait timer.
      next_state = GYRO_PRINT_GYRO_STATS;       // Sets the next state.
      gyro_cal->debug_state = GYRO_WAIT_STATE;  // First, go to wait state.
      break;

    case GYRO_PRINT_GYRO_STATS:
      gyroCalDebugPrintData(gyro_cal, GYROCAL_REPORT_TAG, GYRO_STATS);
      wait_timer_nanos = timestamp_nanos;       // Starts the wait timer.
      next_state = GYRO_PRINT_MAG_STATS;        // Sets the next state.
      gyro_cal->debug_state = GYRO_WAIT_STATE;  // First, go to wait state.
      break;

    case GYRO_PRINT_MAG_STATS:
      gyroCalDebugPrintData(gyro_cal, GYROCAL_REPORT_TAG, MAG_STATS);
      wait_timer_nanos = timestamp_nanos;       // Starts the wait timer.
      next_state = GYRO_IDLE;                   // Sets the next state.
      gyro_cal->debug_state = GYRO_WAIT_STATE;  // First, go to wait state.
      break;

    default:
      // Sends this state machine to its idle state.
      wait_timer_nanos = timestamp_nanos;       // Starts the wait timer.
      gyro_cal->debug_state = GYRO_WAIT_STATE;  // First, go to wait state.
  }

#ifdef GYRO_CAL_DBG_TUNE_ENABLED
  if (gyro_cal->debug_state == GYRO_IDLE) {
    // This check keeps the tuning printout from interleaving with the above
    // debug print data.
    gyroCalTuneDebugPrint(gyro_cal, timestamp_nanos);
  }
#endif  // GYRO_CAL_DBG_TUNE_ENABLED
}

#ifdef GYRO_CAL_DBG_TUNE_ENABLED
void gyroCalTuneDebugPrint(const struct GyroCal* gyro_cal,
                           uint64_t timestamp_nanos) {
  static enum GyroCalDebugState debug_state = GYRO_IDLE;
  static enum GyroCalDebugState next_state = GYRO_IDLE;
  static uint64_t wait_timer_nanos = 0;

  // Output sensor variance levels to assist with tuning thresholds.
  //   i.  Within the first 300 seconds of boot: output interval = 5
  //       seconds.
  //   ii. Thereafter: output interval is 60 seconds.
  bool condition_i = ((timestamp_nanos <= 300000000000) &&
                      NANO_TIMER_CHECK_T1_GEQUAL_T2_PLUS_DELTA(
                          timestamp_nanos, wait_timer_nanos, 5000000000));
  bool condition_ii = NANO_TIMER_CHECK_T1_GEQUAL_T2_PLUS_DELTA(
      timestamp_nanos, wait_timer_nanos, 60000000000);

  // This is a state machine that controls the reporting out of tuning data.
  switch (debug_state) {
    case GYRO_IDLE:
      // Wait for a trigger and start the data tuning printout sequence.
      if (condition_i || condition_ii) {
        CAL_DEBUG_LOG(GYROCAL_TUNE_TAG, "Temp [C]: %s%d.%03d",
                      CAL_ENCODE_FLOAT(gyro_cal->temperature_mean_celsius, 3));
        wait_timer_nanos = timestamp_nanos;   // Starts the wait timer.
        next_state = GYRO_PRINT_ACCEL_STATS;  // Sets the next state.
        debug_state = GYRO_WAIT_STATE;        // First, go to wait state.
      } else {
        debug_state = GYRO_IDLE;
      }
      break;

    case GYRO_WAIT_STATE:
      // This helps throttle the print statements.
      if (NANO_TIMER_CHECK_T1_GEQUAL_T2_PLUS_DELTA(
              timestamp_nanos, wait_timer_nanos, GYROCAL_WAIT_TIME_NANOS)) {
        debug_state = next_state;
      }
      break;

    case GYRO_PRINT_ACCEL_STATS:
      gyroCalDebugPrintData(gyro_cal, GYROCAL_TUNE_TAG, ACCEL_STATS_TUNING);
      wait_timer_nanos = timestamp_nanos;  // Starts the wait timer.
      next_state = GYRO_PRINT_GYRO_STATS;  // Sets the next state.
      debug_state = GYRO_WAIT_STATE;       // First, go to wait state.
      break;

    case GYRO_PRINT_GYRO_STATS:
      gyroCalDebugPrintData(gyro_cal, GYROCAL_TUNE_TAG, GYRO_STATS_TUNING);
      wait_timer_nanos = timestamp_nanos;  // Starts the wait timer.
      next_state = GYRO_PRINT_MAG_STATS;   // Sets the next state.
      debug_state = GYRO_WAIT_STATE;       // First, go to wait state.
      break;

    case GYRO_PRINT_MAG_STATS:
      gyroCalDebugPrintData(gyro_cal, GYROCAL_TUNE_TAG, MAG_STATS_TUNING);
      wait_timer_nanos = timestamp_nanos;  // Starts the wait timer.
      next_state = GYRO_IDLE;              // Sets the next state.
      debug_state = GYRO_WAIT_STATE;       // First, go to wait state.
      break;

    default:
      // Sends this state machine to its idle state.
      wait_timer_nanos = timestamp_nanos;  // Starts the wait timer.
      debug_state = GYRO_IDLE;
  }
}
#endif  // GYRO_CAL_DBG_TUNE_ENABLED
#endif  // GYRO_CAL_DBG_ENABLED