/*
 * Copyright (C) 2017 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/over_temp/over_temp_cal.h"

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

#include "calibration/util/cal_log.h"
#include "common/math/vec.h"
#include "util/nano_assert.h"

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

// Rate-limits the check of old data to every 2 hours.
#define OTC_STALE_CHECK_TIME_NANOS (7200000000000)

// Time duration in which to enforce using the last offset estimate for
// compensation (30 seconds).
#define OTC_USE_RECENT_OFFSET_TIME_NANOS (30000000000)

// Value used to check whether OTC model data is near zero.
#define OTC_MODELDATA_NEAR_ZERO_TOL (1e-7f)

#ifdef OVERTEMPCAL_DBG_ENABLED
// A debug version label to help with tracking results.
#define OTC_DEBUG_VERSION_STRING "[May 15, 2017]"

// The time value used to throttle debug messaging.
#define OTC_WAIT_TIME_NANOS (100000000)

// Sensor axis label definition with index correspondence: 0=X, 1=Y, 2=Z.
static const char  kDebugAxisLabel[3] = "XYZ";
#endif  // OVERTEMPCAL_DBG_ENABLED

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

// Updates the latest received model estimate data.
static void setLatestEstimate(struct OverTempCal *over_temp_cal,
                              const float *offset, float offset_temp_celsius,
                              uint64_t timestamp_nanos);

/*
 * Determines if a new over-temperature model fit should be performed, and then
 * updates the model as needed.
 *
 * INPUTS:
 *   over_temp_cal:    Over-temp data structure.
 *   timestamp_nanos:  Current timestamp for the model update.
 */
static void computeModelUpdate(struct OverTempCal *over_temp_cal,
                               uint64_t timestamp_nanos);

/*
 * Searches 'model_data' for the sensor offset estimate closest to the specified
 * temperature. Sets the 'nearest_offset' pointer to the result.
 */
static void findNearestEstimate(struct OverTempCal *over_temp_cal,
                                float temperature_celsius);

/*
 * Removes the "old" offset estimates from 'model_data' (i.e., eliminates the
 * drift-compromised data).
 */
static void removeStaleModelData(struct OverTempCal *over_temp_cal,
                                 uint64_t timestamp_nanos);

/*
 * Removes the offset estimates from 'model_data' at index, 'model_index'.
 * Returns 'true' if data was removed.
 */
static bool removeModelDataByIndex(struct OverTempCal *over_temp_cal,
                                   size_t model_index);

/*
 * Since it may take a while for an empty model to build up enough data to start
 * producing new model parameter updates, the model collection can be
 * jump-started by using the new model parameters to insert "fake" data in place
 * of actual sensor offset data. 'timestamp_nanos' sets the timestamp for the
 * new model data.
 */
static bool jumpStartModelData(struct OverTempCal *over_temp_cal,
                               uint64_t timestamp_nanos);

/*
 * Computes a new model fit and provides updated model parameters for the
 * over-temperature model data.
 *
 * INPUTS:
 *   over_temp_cal:    Over-temp data structure.
 * OUTPUTS:
 *   temp_sensitivity: Updated modeled temperature sensitivity (array).
 *   sensor_intercept: Updated model intercept (array).
 *
 * NOTE: Arrays are all 3-dimensional with indices: 0=x, 1=y, 2=z.
 *
 * Reference: "Comparing two ways to fit a line to data", John D. Cook.
 * http://www.johndcook.com/blog/2008/10/20/comparing-two-ways-to-fit-a-line-to-data/
 */
static void updateModel(const struct OverTempCal *over_temp_cal,
                        float *temp_sensitivity, float *sensor_intercept);

/*
 * Computes a new over-temperature compensated offset estimate based on the
 * temperature specified by, 'compensated_offset.offset_temp_celsius'.
 *
 * INPUTS:
 *   over_temp_cal:        Over-temp data structure.
 *   timestamp_nanos:      The current system timestamp.
 */
static void updateCalOffset(struct OverTempCal *over_temp_cal,
                            uint64_t timestamp_nanos);

/*
 * Sets the new over-temperature compensated offset estimate vector and
 * timestamp.
 *
 * INPUTS:
 *   over_temp_cal:        Over-temp data structure.
 *   compensated_offset:   The new temperature compensated offset array.
 *   timestamp_nanos:      The current system timestamp.
 */
static void setCompensatedOffset(struct OverTempCal *over_temp_cal,
                                 const float *compensated_offset,
                                 uint64_t timestamp_nanos);

/*
 * Checks new offset estimates to determine if they could be an outlier that
 * should be rejected. Operates on a per-axis basis determined by 'axis_index'.
 *
 * INPUTS:
 *   over_temp_cal:    Over-temp data structure.
 *   offset:           Offset array.
 *   axis_index:       Index of the axis to check (0=x, 1=y, 2=z).
 *
 * Returns 'true' if the deviation of the offset value from the linear model
 * exceeds 'outlier_limit'.
 */
static bool outlierCheck(struct OverTempCal *over_temp_cal, const float *offset,
                         size_t axis_index, float temperature_celsius);

// Sets the OTC model parameters to an "initialized" state.
static void resetOtcLinearModel(struct OverTempCal *over_temp_cal) {
  ASSERT_NOT_NULL(over_temp_cal);

  // Sets the temperature sensitivity model parameters to
  // OTC_INITIAL_SENSITIVITY to indicate that the model is in an "initial"
  // state.
  over_temp_cal->temp_sensitivity[0] = OTC_INITIAL_SENSITIVITY;
  over_temp_cal->temp_sensitivity[1] = OTC_INITIAL_SENSITIVITY;
  over_temp_cal->temp_sensitivity[2] = OTC_INITIAL_SENSITIVITY;
  memset(over_temp_cal->sensor_intercept, 0, 3 * sizeof(float));
}

// Checks that the input temperature value is within the valid range. If outside
// of range, then 'temperature_celsius' is coerced to within the limits.
static bool checkAndEnforceTemperatureRange(float *temperature_celsius) {
  if (*temperature_celsius > OTC_TEMP_MAX_CELSIUS) {
    *temperature_celsius = OTC_TEMP_MAX_CELSIUS;
    return false;
  }
  if (*temperature_celsius < OTC_TEMP_MIN_CELSIUS) {
    *temperature_celsius = OTC_TEMP_MIN_CELSIUS;
    return false;
  }
  return true;
}

// Returns "true" if the candidate linear model parameters are within the valid
// range, and not all zeros.
static bool isValidOtcLinearModel(const struct OverTempCal *over_temp_cal,
                   float temp_sensitivity, float sensor_intercept) {
  ASSERT_NOT_NULL(over_temp_cal);

  return NANO_ABS(temp_sensitivity) < over_temp_cal->temp_sensitivity_limit &&
         NANO_ABS(sensor_intercept) < over_temp_cal->sensor_intercept_limit &&
         NANO_ABS(temp_sensitivity) > OTC_MODELDATA_NEAR_ZERO_TOL &&
         NANO_ABS(sensor_intercept) > OTC_MODELDATA_NEAR_ZERO_TOL;
}

// Returns "true" if 'offset' and 'offset_temp_celsius' is valid.
static bool isValidOtcOffset(const float *offset, float offset_temp_celsius) {
  ASSERT_NOT_NULL(offset);

  // Simple check to ensure that:
  //   1. All of the input data is non "zero".
  //   2. The offset temperature is within the valid range.
  if (NANO_ABS(offset[0]) < OTC_MODELDATA_NEAR_ZERO_TOL &&
      NANO_ABS(offset[1]) < OTC_MODELDATA_NEAR_ZERO_TOL &&
      NANO_ABS(offset[2]) < OTC_MODELDATA_NEAR_ZERO_TOL &&
      NANO_ABS(offset_temp_celsius) < OTC_MODELDATA_NEAR_ZERO_TOL) {
    return false;
  }

  // Only returns the "check" result. Don't care about coercion.
  return checkAndEnforceTemperatureRange(&offset_temp_celsius);
}

#ifdef OVERTEMPCAL_DBG_ENABLED
// This helper function stores all of the debug tracking information necessary
// for printing log messages.
static void updateDebugData(struct OverTempCal* over_temp_cal);

// Helper function that creates tag strings useful for identifying specific
// debug output data (embedded system friendly; not all systems have 'sprintf').
// 'new_debug_tag' is any null-terminated string. Respect the total allowed
// length of the 'otc_debug_tag' string.
//   Constructs: "[" + <otc_debug_tag> + <new_debug_tag>
//   Example,
//     otc_debug_tag = "OVER_TEMP_CAL"
//     new_debug_tag = "INIT]"
//   Output: "[OVER_TEMP_CAL:INIT]"
static void createDebugTag(struct OverTempCal *over_temp_cal,
                           const char *new_debug_tag) {
  over_temp_cal->otc_debug_tag[0] = '[';
  memcpy(over_temp_cal->otc_debug_tag + 1, over_temp_cal->otc_sensor_tag,
         strlen(over_temp_cal->otc_sensor_tag));
  memcpy(
      over_temp_cal->otc_debug_tag + strlen(over_temp_cal->otc_sensor_tag) + 1,
      new_debug_tag, strlen(new_debug_tag) + 1);
}
#endif  // OVERTEMPCAL_DBG_ENABLED

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

void overTempCalInit(struct OverTempCal *over_temp_cal,
                     size_t min_num_model_pts,
                     uint64_t min_update_interval_nanos,
                     float delta_temp_per_bin, float max_error_limit,
                     float outlier_limit, uint64_t age_limit_nanos,
                     float temp_sensitivity_limit, float sensor_intercept_limit,
                     float significant_offset_change, bool over_temp_enable) {
  ASSERT_NOT_NULL(over_temp_cal);

  // Clears OverTempCal memory.
  memset(over_temp_cal, 0, sizeof(struct OverTempCal));

  // Initializes the pointers to important sensor offset estimates.
  over_temp_cal->nearest_offset = &over_temp_cal->model_data[0];
  over_temp_cal->latest_offset  = NULL;

  // Initializes the OTC linear model parameters.
  resetOtcLinearModel(over_temp_cal);

  // Initializes the model identification parameters.
  over_temp_cal->new_overtemp_model_available = false;
  over_temp_cal->new_overtemp_offset_available = false;
  over_temp_cal->min_num_model_pts = min_num_model_pts;
  over_temp_cal->min_update_interval_nanos = min_update_interval_nanos;
  over_temp_cal->delta_temp_per_bin = delta_temp_per_bin;
  over_temp_cal->max_error_limit = max_error_limit;
  over_temp_cal->outlier_limit = outlier_limit;
  over_temp_cal->age_limit_nanos = age_limit_nanos;
  over_temp_cal->temp_sensitivity_limit = temp_sensitivity_limit;
  over_temp_cal->sensor_intercept_limit = sensor_intercept_limit;
  over_temp_cal->significant_offset_change = significant_offset_change;
  over_temp_cal->over_temp_enable = over_temp_enable;

  // Initializes the over-temperature compensated offset temperature.
  over_temp_cal->compensated_offset.offset_temp_celsius =
      OTC_TEMP_INVALID_CELSIUS;

#ifdef OVERTEMPCAL_DBG_ENABLED
  // Sets the default sensor descriptors for debugging.
  overTempCalDebugDescriptors(over_temp_cal, "OVER_TEMP_CAL", "mDPS",
                              1e3f * 180.0f / NANO_PI);

  createDebugTag(over_temp_cal, ":INIT]");
  CAL_DEBUG_LOG(over_temp_cal->otc_debug_tag, "sizeof(struct OverTempCal): %lu",
                (unsigned long int)sizeof(struct OverTempCal));

  if (over_temp_cal->over_temp_enable) {
    CAL_DEBUG_LOG(over_temp_cal->otc_debug_tag,
                  "Over-temperature compensation ENABLED.");
  } else {
    CAL_DEBUG_LOG(over_temp_cal->otc_debug_tag,
                  "Over-temperature compensation DISABLED.");
  }
#endif  // OVERTEMPCAL_DBG_ENABLED
}

void overTempCalSetModel(struct OverTempCal *over_temp_cal, const float *offset,
                         float offset_temp_celsius, uint64_t timestamp_nanos,
                         const float *temp_sensitivity,
                         const float *sensor_intercept, bool jump_start_model) {
  ASSERT_NOT_NULL(over_temp_cal);
  ASSERT_NOT_NULL(offset);
  ASSERT_NOT_NULL(temp_sensitivity);
  ASSERT_NOT_NULL(sensor_intercept);

  // Initializes the OTC linear model parameters.
  resetOtcLinearModel(over_temp_cal);

  // Sets the model parameters if they are within the acceptable limits.
  // Includes a check to reject input model parameters that may have been passed
  // in as all zeros.
  size_t i;
  for (i = 0; i < 3; i++) {
    if (isValidOtcLinearModel(over_temp_cal, temp_sensitivity[i],
                              sensor_intercept[i])) {
      over_temp_cal->temp_sensitivity[i] = temp_sensitivity[i];
      over_temp_cal->sensor_intercept[i] = sensor_intercept[i];
    }
  }

  // Sets the model update time to the current timestamp.
  over_temp_cal->modelupdate_timestamp_nanos = timestamp_nanos;

  // Model "Jump-Start".
  const bool model_jump_started =
      (jump_start_model) ? jumpStartModelData(over_temp_cal, timestamp_nanos)
                         : false;

  if (!model_jump_started) {
    // Checks that the new offset data is valid.
    if (isValidOtcOffset(offset, offset_temp_celsius)) {
      // Sets the initial over-temp calibration estimate.
      memcpy(over_temp_cal->model_data[0].offset, offset, 3 * sizeof(float));
      over_temp_cal->model_data[0].offset_temp_celsius = offset_temp_celsius;
      over_temp_cal->model_data[0].timestamp_nanos = timestamp_nanos;
      over_temp_cal->num_model_pts = 1;
    } else {
      // No valid offset data to load.
      over_temp_cal->num_model_pts = 0;
#ifdef OVERTEMPCAL_DBG_ENABLED
      createDebugTag(over_temp_cal, ":RECALL]");
      CAL_DEBUG_LOG(over_temp_cal->otc_debug_tag,
                    "No valid sensor offset vector to load.");
#endif  // OVERTEMPCAL_DBG_ENABLED
    }
  }

  // If the new offset is valid, then use this as the current compensated
  // offset, otherwise the current value will be kept.
  if (isValidOtcOffset(offset, offset_temp_celsius)) {
    memcpy(over_temp_cal->compensated_offset.offset, offset, 3 * sizeof(float));
    over_temp_cal->compensated_offset.offset_temp_celsius = offset_temp_celsius;
    over_temp_cal->compensated_offset.timestamp_nanos = timestamp_nanos;
  }

  // Resets the latest offset pointer. There are no new offset estimates to
  // track yet.
  over_temp_cal->latest_offset = NULL;

#ifdef OVERTEMPCAL_DBG_ENABLED
  // Prints the recalled model data.
  createDebugTag(over_temp_cal, ":RECALL]");
  CAL_DEBUG_LOG(over_temp_cal->otc_debug_tag,
                "Temperature|Offset|Sensitivity|Intercept [C|units/C|units]: "
                "%s%d.%06d, | %s%d.%06d, %s%d.%06d, %s%d.%06d | %s%d.%06d, "
                "%s%d.%06d, %s%d.%06d | %s%d.%06d, %s%d.%06d, %s%d.%06d",
                CAL_ENCODE_FLOAT(offset_temp_celsius, 6),
                CAL_ENCODE_FLOAT(offset[0], 6), CAL_ENCODE_FLOAT(offset[1], 6),
                CAL_ENCODE_FLOAT(offset[2], 6),
                CAL_ENCODE_FLOAT(temp_sensitivity[0], 6),
                CAL_ENCODE_FLOAT(temp_sensitivity[1], 6),
                CAL_ENCODE_FLOAT(temp_sensitivity[2], 6),
                CAL_ENCODE_FLOAT(sensor_intercept[0], 6),
                CAL_ENCODE_FLOAT(sensor_intercept[1], 6),
                CAL_ENCODE_FLOAT(sensor_intercept[2], 6));

  // Resets the debug print machine to ensure that updateDebugData() can
  // produce a debug report and interupt any ongoing report.
  over_temp_cal->debug_state = OTC_IDLE;

  // Triggers a debug print out to view the new model parameters.
  updateDebugData(over_temp_cal);
#endif  // OVERTEMPCAL_DBG_ENABLED
}

void overTempCalGetModel(struct OverTempCal *over_temp_cal, float *offset,
                         float *offset_temp_celsius, uint64_t *timestamp_nanos,
                         float *temp_sensitivity, float *sensor_intercept) {
  ASSERT_NOT_NULL(over_temp_cal);
  ASSERT_NOT_NULL(offset);
  ASSERT_NOT_NULL(offset_temp_celsius);
  ASSERT_NOT_NULL(timestamp_nanos);
  ASSERT_NOT_NULL(temp_sensitivity);
  ASSERT_NOT_NULL(sensor_intercept);

  // Gets the latest over-temp calibration model data.
  memcpy(temp_sensitivity, over_temp_cal->temp_sensitivity, 3 * sizeof(float));
  memcpy(sensor_intercept, over_temp_cal->sensor_intercept, 3 * sizeof(float));
  *timestamp_nanos = over_temp_cal->modelupdate_timestamp_nanos;

  // Gets the latest temperature compensated offset estimate.
  overTempCalGetOffset(over_temp_cal, offset_temp_celsius, offset);

#ifdef OVERTEMPCAL_DBG_ENABLED
  // Prints the updated model data.
  createDebugTag(over_temp_cal, ":STORED]");
  CAL_DEBUG_LOG(over_temp_cal->otc_debug_tag,
                "Temperature|Offset|Sensitivity|Intercept [C|units/C|units]: "
                "%s%d.%06d, | %s%d.%06d, %s%d.%06d, %s%d.%06d | %s%d.%06d, "
                "%s%d.%06d, %s%d.%06d | %s%d.%06d, %s%d.%06d, %s%d.%06d",
                CAL_ENCODE_FLOAT(*offset_temp_celsius, 6),
                CAL_ENCODE_FLOAT(offset[0], 6),
                CAL_ENCODE_FLOAT(offset[1], 6),
                CAL_ENCODE_FLOAT(offset[2], 6),
                CAL_ENCODE_FLOAT(temp_sensitivity[0], 6),
                CAL_ENCODE_FLOAT(temp_sensitivity[1], 6),
                CAL_ENCODE_FLOAT(temp_sensitivity[2], 6),
                CAL_ENCODE_FLOAT(sensor_intercept[0], 6),
                CAL_ENCODE_FLOAT(sensor_intercept[1], 6),
                CAL_ENCODE_FLOAT(sensor_intercept[2], 6));
#endif  // OVERTEMPCAL_DBG_ENABLED
}

void overTempCalSetModelData(struct OverTempCal *over_temp_cal,
                             size_t data_length, uint64_t timestamp_nanos,
                             const struct OverTempCalDataPt *model_data) {
  ASSERT_NOT_NULL(over_temp_cal);
  ASSERT_NOT_NULL(model_data);

  // Load only "good" data from the input 'model_data'.
  over_temp_cal->num_model_pts = NANO_MIN(data_length, OTC_MODEL_SIZE);
  size_t i;
  size_t valid_data_count = 0;
  for (i = 0; i < over_temp_cal->num_model_pts; i++) {
    if (isValidOtcOffset(model_data[i].offset,
                         model_data[i].offset_temp_celsius)) {
      memcpy(&over_temp_cal->model_data[i], &model_data[i],
             sizeof(struct OverTempCalDataPt));

      // Updates the model time stamps to the current load time.
      over_temp_cal->model_data[i].timestamp_nanos = timestamp_nanos;

      valid_data_count++;
    }
  }
  over_temp_cal->num_model_pts = valid_data_count;

  // Initializes the OTC linear model parameters.
  resetOtcLinearModel(over_temp_cal);

  // Computes and replaces the model fit parameters.
  computeModelUpdate(over_temp_cal, timestamp_nanos);

  // Resets the latest offset pointer. There are no new offset estimates to
  // track yet.
  over_temp_cal->latest_offset = NULL;

  // Searches for the sensor offset estimate closest to the current temperature.
  findNearestEstimate(over_temp_cal,
                      over_temp_cal->compensated_offset.offset_temp_celsius);

  // Updates the current over-temperature compensated offset estimate.
  updateCalOffset(over_temp_cal, timestamp_nanos);

#ifdef OVERTEMPCAL_DBG_ENABLED
  // Prints the updated model data.
  createDebugTag(over_temp_cal, ":RECALL]");
  CAL_DEBUG_LOG(over_temp_cal->otc_debug_tag,
                "Over-temperature full model data set recalled.");

  // Resets the debug print machine to ensure that a new debug report will
  // interupt any ongoing report.
  over_temp_cal->debug_state = OTC_IDLE;

  // Triggers a log printout to show the updated sensor offset estimate.
  updateDebugData(over_temp_cal);
#endif  // OVERTEMPCAL_DBG_ENABLED
}

void overTempCalGetModelData(struct OverTempCal *over_temp_cal,
                             size_t *data_length,
                             struct OverTempCalDataPt *model_data) {
  ASSERT_NOT_NULL(over_temp_cal);
  *data_length = over_temp_cal->num_model_pts;
  memcpy(model_data, over_temp_cal->model_data,
         over_temp_cal->num_model_pts * sizeof(struct OverTempCalDataPt));
}

void overTempCalGetOffset(struct OverTempCal *over_temp_cal,
                          float *compensated_offset_temperature_celsius,
                          float *compensated_offset) {
  memcpy(compensated_offset, over_temp_cal->compensated_offset.offset,
         3 * sizeof(float));
  *compensated_offset_temperature_celsius =
      over_temp_cal->compensated_offset.offset_temp_celsius;
}

void overTempCalRemoveOffset(struct OverTempCal *over_temp_cal,
                             uint64_t timestamp_nanos, float xi, float yi,
                             float zi, float *xo, float *yo, float *zo) {
  ASSERT_NOT_NULL(over_temp_cal);
  ASSERT_NOT_NULL(xo);
  ASSERT_NOT_NULL(yo);
  ASSERT_NOT_NULL(zo);

  // Determines whether over-temp compensation will be applied.
  if (over_temp_cal->over_temp_enable) {
    // Removes the over-temperature compensated offset from the input sensor
    // data.
    *xo = xi - over_temp_cal->compensated_offset.offset[0];
    *yo = yi - over_temp_cal->compensated_offset.offset[1];
    *zo = zi - over_temp_cal->compensated_offset.offset[2];
  } else {
    *xo = xi;
    *yo = yi;
    *zo = zi;
  }
}

bool overTempCalNewModelUpdateAvailable(struct OverTempCal *over_temp_cal) {
  ASSERT_NOT_NULL(over_temp_cal);
  const bool update_available = over_temp_cal->new_overtemp_model_available &&
                                over_temp_cal->over_temp_enable;

  // The 'new_overtemp_model_available' flag is reset when it is read here.
  over_temp_cal->new_overtemp_model_available = false;

  return update_available;
}

bool overTempCalNewOffsetAvailable(struct OverTempCal *over_temp_cal) {
  ASSERT_NOT_NULL(over_temp_cal);
  const bool update_available = over_temp_cal->new_overtemp_offset_available &&
                                over_temp_cal->over_temp_enable;

  // The 'new_overtemp_offset_available' flag is reset when it is read here.
  over_temp_cal->new_overtemp_offset_available = false;

  return update_available;
}

void overTempCalUpdateSensorEstimate(struct OverTempCal *over_temp_cal,
                                     uint64_t timestamp_nanos,
                                     const float *offset,
                                     float temperature_celsius) {
  ASSERT_NOT_NULL(over_temp_cal);
  ASSERT_NOT_NULL(offset);
  ASSERT(over_temp_cal->delta_temp_per_bin > 0);

  // Checks that the new offset data is valid, returns if bad.
  if (!isValidOtcOffset(offset, temperature_celsius)) {
    return;
  }

  // Prevent a divide by zero below.
  if (over_temp_cal->delta_temp_per_bin <= 0) {
    return;
  }

  // Checks whether this offset estimate is a likely outlier. A limit is placed
  // on 'num_outliers', the previous number of successive rejects, to prevent
  // too many back-to-back rejections.
  if (over_temp_cal->num_outliers < OTC_MAX_OUTLIER_COUNT) {
    if (outlierCheck(over_temp_cal, offset, 0, temperature_celsius) ||
        outlierCheck(over_temp_cal, offset, 1, temperature_celsius) ||
        outlierCheck(over_temp_cal, offset, 2, temperature_celsius)) {
      // Increments the count of rejected outliers.
      over_temp_cal->num_outliers++;

#ifdef OVERTEMPCAL_DBG_ENABLED
      createDebugTag(over_temp_cal, ":OUTLIER]");
      CAL_DEBUG_LOG(
          over_temp_cal->otc_debug_tag,
          "Offset|Temperature|Time [%s|C|nsec]: "
          "%s%d.%06d, %s%d.%06d, %s%d.%06d, %s%d.%03d, %llu",
          over_temp_cal->otc_unit_tag,
          CAL_ENCODE_FLOAT(offset[0] * over_temp_cal->otc_unit_conversion, 6),
          CAL_ENCODE_FLOAT(offset[1] * over_temp_cal->otc_unit_conversion, 6),
          CAL_ENCODE_FLOAT(offset[2] * over_temp_cal->otc_unit_conversion, 6),
          CAL_ENCODE_FLOAT(temperature_celsius, 3),
          (unsigned long long int)timestamp_nanos);
#endif  // OVERTEMPCAL_DBG_ENABLED

      return;  // Outlier detected: skips adding this offset to the model.
    } else {
      // Resets the count of rejected outliers.
      over_temp_cal->num_outliers = 0;
    }
  } else {
    // Resets the count of rejected outliers.
    over_temp_cal->num_outliers = 0;
  }

  // Computes the temperature bin range data.
  const int32_t bin_num =
      CAL_FLOOR(temperature_celsius / over_temp_cal->delta_temp_per_bin);
  const float temp_lo_check = bin_num * over_temp_cal->delta_temp_per_bin;
  const float temp_hi_check = (bin_num + 1) * over_temp_cal->delta_temp_per_bin;

  // The rules for accepting new offset estimates into the 'model_data'
  // collection:
  //    1) The temperature domain is divided into bins each spanning
  //       'delta_temp_per_bin'.
  //    2) Find and replace the i'th 'model_data' estimate data if:
  //          Let, bin_num = floor(temperature_celsius / delta_temp_per_bin)
  //          temp_lo_check = bin_num * delta_temp_per_bin
  //          temp_hi_check = (bin_num + 1) * delta_temp_per_bin
  //          Check condition:
  //          temp_lo_check <= model_data[i].offset_temp_celsius < temp_hi_check
  bool replaced_one = false;
  size_t i = 0;
  for (i = 0; i < over_temp_cal->num_model_pts; i++) {
    if (over_temp_cal->model_data[i].offset_temp_celsius < temp_hi_check &&
        over_temp_cal->model_data[i].offset_temp_celsius >= temp_lo_check) {
      // NOTE - The pointer to the new model data point is set here; the offset
      // data is set below in the call to 'setLatestEstimate'.
      over_temp_cal->latest_offset = &over_temp_cal->model_data[i];
      replaced_one = true;
      break;
    }
  }

  // NOTE - The pointer to the new model data point is set here; the offset
  // data is set below in the call to 'setLatestEstimate'.
  if (!replaced_one && over_temp_cal->num_model_pts < OTC_MODEL_SIZE) {
    if (over_temp_cal->num_model_pts < OTC_MODEL_SIZE) {
      // 3) If nothing was replaced, and the 'model_data' buffer is not full
      //    then add the estimate data to the array.
      over_temp_cal->latest_offset =
          &over_temp_cal->model_data[over_temp_cal->num_model_pts];
      over_temp_cal->num_model_pts++;
    } else {
      // 4) Otherwise (nothing was replaced and buffer is full), replace the
      //    oldest data with the incoming one.
      over_temp_cal->latest_offset = &over_temp_cal->model_data[0];
      for (i = 1; i < over_temp_cal->num_model_pts; i++) {
        if (over_temp_cal->latest_offset->timestamp_nanos <
            over_temp_cal->model_data[i].timestamp_nanos) {
          over_temp_cal->latest_offset = &over_temp_cal->model_data[i];
        }
      }
    }
  }

  // Updates the latest model estimate data.
  setLatestEstimate(over_temp_cal, offset, temperature_celsius,
                    timestamp_nanos);

  // The latest offset estimate is the nearest temperature offset.
  over_temp_cal->nearest_offset = over_temp_cal->latest_offset;

  // The rules for determining whether a new model fit is computed are:
  //    1) A minimum number of data points must have been collected:
  //          num_model_pts >= min_num_model_pts
  //       NOTE: Collecting 'num_model_pts' and given that only one point is
  //       kept per temperature bin (spanning a thermal range specified by
  //       'delta_temp_per_bin'), implies that model data covers at least,
  //          model_temperature_span >= 'num_model_pts' * delta_temp_per_bin
  //    2) New model updates will not occur for intervals less than:
  //          (current_timestamp_nanos - modelupdate_timestamp_nanos) <
  //            min_update_interval_nanos
  if (over_temp_cal->num_model_pts >= over_temp_cal->min_num_model_pts &&
      NANO_TIMER_CHECK_T1_GEQUAL_T2_PLUS_DELTA(
          timestamp_nanos, over_temp_cal->modelupdate_timestamp_nanos,
          over_temp_cal->min_update_interval_nanos)) {
    computeModelUpdate(over_temp_cal, timestamp_nanos);
  }

  // Updates the current over-temperature compensated offset estimate.
  updateCalOffset(over_temp_cal, timestamp_nanos);

#ifdef OVERTEMPCAL_DBG_ENABLED
  // Updates the total number of received sensor offset estimates.
  over_temp_cal->debug_num_estimates++;

  // Triggers a log printout to show the updated sensor offset estimate.
  updateDebugData(over_temp_cal);
#endif  // OVERTEMPCAL_DBG_ENABLED
}

void overTempCalSetTemperature(struct OverTempCal *over_temp_cal,
                               uint64_t timestamp_nanos,
                               float temperature_celsius) {
  ASSERT_NOT_NULL(over_temp_cal);

#ifdef OVERTEMPCAL_DBG_ENABLED
#ifdef OVERTEMPCAL_DBG_LOG_TEMP
  static uint64_t wait_timer = 0;
  // Prints the sensor temperature trajectory for debugging purposes.
  // This throttles the print statements.
  if (NANO_TIMER_CHECK_T1_GEQUAL_T2_PLUS_DELTA(timestamp_nanos, wait_timer,
                                               1000000000)) {
    wait_timer = timestamp_nanos;  // Starts the wait timer.

    // Prints out temperature and the current timestamp.
    createDebugTag(over_temp_cal, ":TEMP]");
    CAL_DEBUG_LOG(over_temp_cal->otc_debug_tag,
                  "Temperature|Time [C|nsec] = %s%d.%06d, %llu",
                  CAL_ENCODE_FLOAT(temperature_celsius, 6),
                  (unsigned long long int)timestamp_nanos);
  }
#endif  // OVERTEMPCAL_DBG_LOG_TEMP
#endif  // OVERTEMPCAL_DBG_ENABLED

  // Checks that the offset temperature is within a valid range, saturates if
  // outside.
  checkAndEnforceTemperatureRange(&temperature_celsius);

  // Updates the offset compensation temperature.
  over_temp_cal->compensated_offset.offset_temp_celsius = temperature_celsius;

  // Searches for the sensor offset estimate closest to the current temperature
  // when the temperature has changed by more than +/-10% of
  // 'delta_temp_per_bin'.
  if (over_temp_cal->num_model_pts > 0) {
    if (NANO_ABS(over_temp_cal->last_temp_check_celsius - temperature_celsius) >
        0.1f * over_temp_cal->delta_temp_per_bin) {
      findNearestEstimate(over_temp_cal, temperature_celsius);
      over_temp_cal->last_temp_check_celsius = temperature_celsius;
    }
  }

  // Updates the current over-temperature compensated offset estimate.
  updateCalOffset(over_temp_cal, timestamp_nanos);
}

void overTempGetModelError(const struct OverTempCal *over_temp_cal,
                   const float *temp_sensitivity, const float *sensor_intercept,
                   float *max_error) {
  ASSERT_NOT_NULL(over_temp_cal);
  ASSERT_NOT_NULL(temp_sensitivity);
  ASSERT_NOT_NULL(sensor_intercept);
  ASSERT_NOT_NULL(max_error);

  size_t i;
  size_t j;
  float max_error_test;
  memset(max_error, 0, 3 * sizeof(float));

  for (i = 0; i < over_temp_cal->num_model_pts; i++) {
    for (j = 0; j < 3; j++) {
      max_error_test =
          NANO_ABS(over_temp_cal->model_data[i].offset[j] -
                   (temp_sensitivity[j] *
                        over_temp_cal->model_data[i].offset_temp_celsius +
                    sensor_intercept[j]));
      if (max_error_test > max_error[j]) {
        max_error[j] = max_error_test;
      }
    }
  }
}

/////// LOCAL HELPER FUNCTION DEFINITIONS /////////////////////////////////////

void updateCalOffset(struct OverTempCal *over_temp_cal,
                     uint64_t timestamp_nanos) {
  ASSERT_NOT_NULL(over_temp_cal);
  ASSERT_NOT_NULL(over_temp_cal->nearest_offset);

  float temperature_celsius =
      over_temp_cal->compensated_offset.offset_temp_celsius;

  // If 'temperature_celsius' is invalid, then do not update the compensated
  // offset (keeps the current values).
  if (temperature_celsius <= OTC_TEMP_INVALID_CELSIUS) {
    return;
  }

  // Removes very old data from the collected model estimates (i.e., eliminates
  // drift-compromised data). Only does this when there is more than one
  // estimate in the model (i.e., don't want to remove all data, even if it is
  // very old [something is likely better than nothing]).
  if ((timestamp_nanos >=
       OTC_STALE_CHECK_TIME_NANOS + over_temp_cal->stale_data_timer) &&
      over_temp_cal->num_model_pts > 1) {
    over_temp_cal->stale_data_timer = timestamp_nanos;  // Resets timer.
    removeStaleModelData(over_temp_cal, timestamp_nanos);
  }

  // If there are points in the model data set, then a 'nearest_offset' has been
  // defined.
  bool nearest_offset_defined = (over_temp_cal->num_model_pts > 0);

  // Uses the most recent offset estimate for offset compensation if it is
  // defined and within a time delta specified by
  // OTC_USE_RECENT_OFFSET_TIME_NANOS.
  if (over_temp_cal->latest_offset && nearest_offset_defined &&
      timestamp_nanos < over_temp_cal->latest_offset->timestamp_nanos +
                            OTC_USE_RECENT_OFFSET_TIME_NANOS) {
    setCompensatedOffset(over_temp_cal, over_temp_cal->latest_offset->offset,
                         timestamp_nanos);
    return;  // Exits early.
  }

  // Sets the default compensated offset vector.
  float compensated_offset[3];
  if (nearest_offset_defined) {
    // Uses the nearest-temperature estimate to perform the compensation.
    memcpy(compensated_offset, over_temp_cal->nearest_offset->offset,
           3 * sizeof(float));
  } else {
    // Uses the current compensated offset value.
    memcpy(compensated_offset, over_temp_cal->compensated_offset.offset,
           3 * sizeof(float));
  }

  // Provides per-axis checks to complete the offset compensation vector update.
  size_t index;
  for (index = 0; index < 3; index++) {
    if (over_temp_cal->temp_sensitivity[index] < OTC_INITIAL_SENSITIVITY) {
      // If a valid axis model is defined then the default compensation will use
      // the linear model:
      //   compensated_offset = (temp_sensitivity * temperature +
      //   sensor_intercept)
      compensated_offset[index] =
          (over_temp_cal->temp_sensitivity[index] * temperature_celsius +
           over_temp_cal->sensor_intercept[index]);

      if (nearest_offset_defined &&
          NANO_ABS(temperature_celsius -
                   over_temp_cal->nearest_offset->offset_temp_celsius) <
              over_temp_cal->delta_temp_per_bin &&
          NANO_ABS(compensated_offset[index] -
                   over_temp_cal->nearest_offset->offset[index]) <
              over_temp_cal->max_error_limit) {
        // Uses the nearest-temperature estimate to perform the compensation if
        // the sensor's temperature is within a small neighborhood of the
        // 'nearest_offset', and the delta between the nearest-temperature
        // estimate and the model fit is less than 'max_error_limit'.
        compensated_offset[index] =
            over_temp_cal->nearest_offset->offset[index];
      }
    }
  }

  // Finalizes the update to the offset compensation vector, and timestamp.
  setCompensatedOffset(over_temp_cal, compensated_offset, timestamp_nanos);
}

void setCompensatedOffset(struct OverTempCal *over_temp_cal,
                          const float *compensated_offset,
                          uint64_t timestamp_nanos) {
  ASSERT_NOT_NULL(over_temp_cal);
  ASSERT_NOT_NULL(compensated_offset);

  // If the 'compensated_offset' value has changed significantly, then set
  // 'new_overtemp_offset_available' true.
  size_t i;
  bool new_overtemp_offset_available = false;
  for (i = 0; i < 3; i++) {
    if (NANO_ABS(over_temp_cal->compensated_offset.offset[i] -
                 compensated_offset[i]) >=
        over_temp_cal->significant_offset_change) {
      new_overtemp_offset_available |= true;
      break;
    }
  }
  over_temp_cal->new_overtemp_offset_available |= new_overtemp_offset_available;

  // If the offset has changed significantly, then the offset compensation
  // vector and timestamp are updated.
  if (new_overtemp_offset_available) {
    memcpy(over_temp_cal->compensated_offset.offset, compensated_offset,
           3 * sizeof(float));
    over_temp_cal->compensated_offset.timestamp_nanos = timestamp_nanos;
  }
}

void setLatestEstimate(struct OverTempCal *over_temp_cal, const float *offset,
                       float offset_temp_celsius, uint64_t timestamp_nanos) {
  ASSERT_NOT_NULL(over_temp_cal);
  ASSERT_NOT_NULL(offset);

  if (over_temp_cal->latest_offset) {
    // Sets the latest over-temp calibration estimate.
    memcpy(over_temp_cal->latest_offset->offset, offset, 3 * sizeof(float));
    over_temp_cal->latest_offset->offset_temp_celsius = offset_temp_celsius;
    over_temp_cal->latest_offset->timestamp_nanos = timestamp_nanos;
  }
}

void computeModelUpdate(struct OverTempCal *over_temp_cal,
                        uint64_t timestamp_nanos) {
  ASSERT_NOT_NULL(over_temp_cal);

  // Ensures that the minimum number of points required for a model fit has been
  // satisfied.
  if (over_temp_cal->num_model_pts < over_temp_cal->min_num_model_pts)
      return;

  // Updates the linear model fit.
  float temp_sensitivity[3];
  float sensor_intercept[3];
  updateModel(over_temp_cal, temp_sensitivity, sensor_intercept);

#ifdef OVERTEMPCAL_DBG_ENABLED
  // Computes the maximum error over all of the model data.
  float max_error[3];
  overTempGetModelError(over_temp_cal, temp_sensitivity, sensor_intercept,
                        max_error);
#endif  // OVERTEMPCAL_DBG_ENABLED

  //    3) A new set of model parameters are accepted if:
  //         i. The model fit parameters must be within certain absolute bounds:
  //              a. NANO_ABS(temp_sensitivity) < temp_sensitivity_limit
  //              b. NANO_ABS(sensor_intercept) < sensor_intercept_limit
  // NOTE: Model parameter updates are not qualified against model fit error
  // here to protect against the case where there is large change in the
  // temperature characteristic either during runtime (e.g., temperature
  // conditioning due to hysteresis) or as a result of loading a poor model data
  // set. Otherwise, a lockout condition could occur where the entire model
  // data set would need to be replaced in order to bring the model fit error
  // below the error limit and allow a successful model update.
  size_t i;
  bool updated_one = false;
  for (i = 0; i < 3; i++) {
    if (isValidOtcLinearModel(over_temp_cal, temp_sensitivity[i],
                              sensor_intercept[i])) {
      over_temp_cal->temp_sensitivity[i] = temp_sensitivity[i];
      over_temp_cal->sensor_intercept[i] = sensor_intercept[i];
      updated_one = true;
    } else {
#ifdef OVERTEMPCAL_DBG_ENABLED
      createDebugTag(over_temp_cal, ":REJECT]");
      CAL_DEBUG_LOG(
          over_temp_cal->otc_debug_tag,
          "%c-Axis Parameters|Max Error|Time [%s/C|%s|%s|nsec]: "
          "%s%d.%06d, %s%d.%06d, %s%d.%06d, %llu",
          kDebugAxisLabel[i], over_temp_cal->otc_unit_tag,
          over_temp_cal->otc_unit_tag, over_temp_cal->otc_unit_tag,
          CAL_ENCODE_FLOAT(
              temp_sensitivity[i] * over_temp_cal->otc_unit_conversion, 6),
          CAL_ENCODE_FLOAT(
              sensor_intercept[i] * over_temp_cal->otc_unit_conversion, 6),
          CAL_ENCODE_FLOAT(max_error[i] * over_temp_cal->otc_unit_conversion,
                           6),
          (unsigned long long int)timestamp_nanos);
#endif  // OVERTEMPCAL_DBG_ENABLED
    }
  }

  // If at least one of the axes updated, then consider this a valid model
  // update.
  if (updated_one) {
    // Resets the timer and sets the update flag.
    over_temp_cal->modelupdate_timestamp_nanos = timestamp_nanos;
    over_temp_cal->new_overtemp_model_available = true;

#ifdef OVERTEMPCAL_DBG_ENABLED
    // Updates the total number of model updates, the debug data package, and
    // triggers a log printout.
    over_temp_cal->debug_num_model_updates++;
#endif  // OVERTEMPCAL_DBG_ENABLED
  }
}

void findNearestEstimate(struct OverTempCal *over_temp_cal,
                         float temperature_celsius) {
  ASSERT_NOT_NULL(over_temp_cal);

  // If 'temperature_celsius' is invalid, then do not search.
  if (temperature_celsius <= OTC_TEMP_INVALID_CELSIUS) {
    return;
  }

  // Performs a brute force search for the estimate nearest
  // 'temperature_celsius'.
  size_t i = 0;
  float dtemp_new = 0.0f;
  float dtemp_old = FLT_MAX;
  over_temp_cal->nearest_offset = &over_temp_cal->model_data[0];
  for (i = 0; i < over_temp_cal->num_model_pts; i++) {
    dtemp_new = NANO_ABS(over_temp_cal->model_data[i].offset_temp_celsius -
                         temperature_celsius);
    if (dtemp_new < dtemp_old) {
      over_temp_cal->nearest_offset = &over_temp_cal->model_data[i];
      dtemp_old = dtemp_new;
    }
  }
}

void removeStaleModelData(struct OverTempCal *over_temp_cal,
                          uint64_t timestamp_nanos) {
  ASSERT_NOT_NULL(over_temp_cal);

  size_t i;
  bool removed_one = false;
  for (i = 0; i < over_temp_cal->num_model_pts; i++) {
    if (timestamp_nanos > over_temp_cal->model_data[i].timestamp_nanos &&
        timestamp_nanos > over_temp_cal->age_limit_nanos +
                              over_temp_cal->model_data[i].timestamp_nanos) {
      // If the latest offset was removed, then indicate this by setting it to
      // NULL.
      if (over_temp_cal->latest_offset == &over_temp_cal->model_data[i]) {
        over_temp_cal->latest_offset = NULL;
      }
      removed_one |= removeModelDataByIndex(over_temp_cal, i);
    }
  }

  if (removed_one) {
    // If anything was removed, then this attempts to recompute the model.
    computeModelUpdate(over_temp_cal, timestamp_nanos);

    // Searches for the sensor offset estimate closest to the current
    // temperature.
    findNearestEstimate(over_temp_cal,
                        over_temp_cal->compensated_offset.offset_temp_celsius);
  }
}

bool removeModelDataByIndex(struct OverTempCal *over_temp_cal,
                            size_t model_index) {
  ASSERT_NOT_NULL(over_temp_cal);

  // This function will not remove all of the model data. At least one model
  // sample will be left.
  if (over_temp_cal->num_model_pts <= 1) {
    return false;
  }

#ifdef OVERTEMPCAL_DBG_ENABLED
  createDebugTag(over_temp_cal, ":REMOVE]");
  CAL_DEBUG_LOG(
      over_temp_cal->otc_debug_tag,
      "Offset|Temp|Time [%s|C|nsec]: %s%d.%06d, %s%d.%06d, %s%d.%06d, "
      "%s%d.%03d, %llu",
      over_temp_cal->otc_unit_tag,
      CAL_ENCODE_FLOAT(over_temp_cal->model_data[model_index].offset[0] *
                           over_temp_cal->otc_unit_conversion,
                       6),
      CAL_ENCODE_FLOAT(over_temp_cal->model_data[model_index].offset[1] *
                           over_temp_cal->otc_unit_conversion,
                       6),
      CAL_ENCODE_FLOAT(over_temp_cal->model_data[model_index].offset[1] *
                           over_temp_cal->otc_unit_conversion,
                       6),
      CAL_ENCODE_FLOAT(
          over_temp_cal->model_data[model_index].offset_temp_celsius, 3),
      (unsigned long long int)over_temp_cal->model_data[model_index]
          .timestamp_nanos);
#endif  // OVERTEMPCAL_DBG_ENABLED

  // Remove the model data at 'model_index'.
  size_t i;
  for (i = model_index; i < over_temp_cal->num_model_pts - 1; i++) {
    memcpy(&over_temp_cal->model_data[i], &over_temp_cal->model_data[i + 1],
           sizeof(struct OverTempCalDataPt));
  }
  over_temp_cal->num_model_pts--;

  return true;
}

bool jumpStartModelData(struct OverTempCal *over_temp_cal,
                        uint64_t timestamp_nanos) {
  ASSERT_NOT_NULL(over_temp_cal);
  ASSERT(over_temp_cal->delta_temp_per_bin > 0);

  // Prevent a divide by zero below.
  if (over_temp_cal->delta_temp_per_bin <= 0) {
    return false;
  }

  // In normal operation the offset estimates enter into the 'model_data' array
  // complete (i.e., x, y, z values are all provided). Therefore, the jumpstart
  // data produced here requires that the model parameters have all been fully
  // defined and are all within the valid range.
  size_t i;
  for (i = 0; i < 3; i++) {
    if (!isValidOtcLinearModel(over_temp_cal,
                               over_temp_cal->temp_sensitivity[i],
                               over_temp_cal->sensor_intercept[i])) {
      return false;
    }
  }

  // Any pre-existing model data points will be overwritten.
  over_temp_cal->num_model_pts = 0;

  // This defines the minimum contiguous set of points to allow a model update
  // when the next offset estimate is received. They are placed at a common
  // temperature range that is likely to get replaced with actual data soon.
  const int32_t start_bin_num = CAL_FLOOR(JUMPSTART_START_TEMP_CELSIUS /
                                          over_temp_cal->delta_temp_per_bin);
  float offset_temp_celsius =
      (start_bin_num + 0.5f) * over_temp_cal->delta_temp_per_bin;

  size_t j;
  for (i = 0; i < over_temp_cal->min_num_model_pts; i++) {
    for (j = 0; j < 3; j++) {
      over_temp_cal->model_data[i].offset[j] =
          over_temp_cal->temp_sensitivity[j] * offset_temp_celsius +
          over_temp_cal->sensor_intercept[j];
    }
    over_temp_cal->model_data[i].offset_temp_celsius = offset_temp_celsius;
    over_temp_cal->model_data[i].timestamp_nanos = timestamp_nanos;

    offset_temp_celsius += over_temp_cal->delta_temp_per_bin;
    over_temp_cal->num_model_pts++;
  }

#ifdef OVERTEMPCAL_DBG_ENABLED
  createDebugTag(over_temp_cal, ":INIT]");
  if (over_temp_cal->num_model_pts > 0) {
    CAL_DEBUG_LOG(over_temp_cal->otc_debug_tag,
                  "Model Jump-Start:  #Points = %lu.",
                  (unsigned long int)over_temp_cal->num_model_pts);
  }
#endif  // OVERTEMPCAL_DBG_ENABLED

  return (over_temp_cal->num_model_pts > 0);
}

void updateModel(const struct OverTempCal *over_temp_cal,
                 float *temp_sensitivity, float *sensor_intercept) {
  ASSERT_NOT_NULL(over_temp_cal);
  ASSERT_NOT_NULL(temp_sensitivity);
  ASSERT_NOT_NULL(sensor_intercept);
  ASSERT(over_temp_cal->num_model_pts > 0);

  float st = 0.0f, stt = 0.0f;
  float sx = 0.0f, stsx = 0.0f;
  float sy = 0.0f, stsy = 0.0f;
  float sz = 0.0f, stsz = 0.0f;
  const size_t n = over_temp_cal->num_model_pts;
  size_t i = 0;

  // First pass computes the mean values.
  for (i = 0; i < n; ++i) {
    st += over_temp_cal->model_data[i].offset_temp_celsius;
    sx += over_temp_cal->model_data[i].offset[0];
    sy += over_temp_cal->model_data[i].offset[1];
    sz += over_temp_cal->model_data[i].offset[2];
  }

  // Second pass computes the mean corrected second moment values.
  const float inv_n = 1.0f / n;
  for (i = 0; i < n; ++i) {
    const float t =
        over_temp_cal->model_data[i].offset_temp_celsius - st * inv_n;
    stt += t * t;
    stsx += t * over_temp_cal->model_data[i].offset[0];
    stsy += t * over_temp_cal->model_data[i].offset[1];
    stsz += t * over_temp_cal->model_data[i].offset[2];
  }

  // Calculates the linear model fit parameters.
  ASSERT(stt > 0);
  const float inv_stt = 1.0f / stt;
  temp_sensitivity[0] = stsx * inv_stt;
  sensor_intercept[0] = (sx - st * temp_sensitivity[0]) * inv_n;
  temp_sensitivity[1] = stsy * inv_stt;
  sensor_intercept[1] = (sy - st * temp_sensitivity[1]) * inv_n;
  temp_sensitivity[2] = stsz * inv_stt;
  sensor_intercept[2] = (sz - st * temp_sensitivity[2]) * inv_n;
}

bool outlierCheck(struct OverTempCal *over_temp_cal, const float *offset,
                  size_t axis_index, float temperature_celsius) {
  ASSERT_NOT_NULL(over_temp_cal);
  ASSERT_NOT_NULL(offset);

  // If a model has been defined, then check to see if this offset could be a
  // potential outlier:
  if (over_temp_cal->temp_sensitivity[axis_index] < OTC_INITIAL_SENSITIVITY) {
    const float outlier_test = NANO_ABS(
        offset[axis_index] -
        (over_temp_cal->temp_sensitivity[axis_index] * temperature_celsius +
         over_temp_cal->sensor_intercept[axis_index]));

    if (outlier_test > over_temp_cal->outlier_limit) {
      return true;
    }
  }

  return false;
}

/////// DEBUG FUNCTION DEFINITIONS ////////////////////////////////////////////

#ifdef OVERTEMPCAL_DBG_ENABLED
void updateDebugData(struct OverTempCal* over_temp_cal) {
  ASSERT_NOT_NULL(over_temp_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 (over_temp_cal->debug_state != OTC_IDLE) {
    return;
  }

  // Triggers a debug log printout.
  over_temp_cal->debug_print_trigger = true;

  // Initializes the debug data structure.
  memset(&over_temp_cal->debug_overtempcal, 0, sizeof(struct DebugOverTempCal));

  // Copies over the relevant data.
  size_t i;
  for (i = 0; i < 3; i++) {
    if (isValidOtcLinearModel(over_temp_cal, over_temp_cal->temp_sensitivity[i],
                              over_temp_cal->sensor_intercept[i])) {
      over_temp_cal->debug_overtempcal.temp_sensitivity[i] =
          over_temp_cal->temp_sensitivity[i];
      over_temp_cal->debug_overtempcal.sensor_intercept[i] =
          over_temp_cal->sensor_intercept[i];
    } else {
      // If the model is not valid then just set the debug information so that
      // zeros are printed.
      over_temp_cal->debug_overtempcal.temp_sensitivity[i] = 0.0f;
      over_temp_cal->debug_overtempcal.sensor_intercept[i] = 0.0f;
    }
  }

  // If 'latest_offset' is defined the copy the data for debug printing.
  // Otherwise, the current compensated offset will be printed.
  if (over_temp_cal->latest_offset) {
    memcpy(&over_temp_cal->debug_overtempcal.latest_offset,
           over_temp_cal->latest_offset, sizeof(struct OverTempCalDataPt));
  } else {
    memcpy(&over_temp_cal->debug_overtempcal.latest_offset,
           &over_temp_cal->compensated_offset,
           sizeof(struct OverTempCalDataPt));
  }

  over_temp_cal->debug_overtempcal.num_model_pts = over_temp_cal->num_model_pts;
  over_temp_cal->debug_overtempcal.modelupdate_timestamp_nanos =
      over_temp_cal->modelupdate_timestamp_nanos;

  // Computes the maximum error over all of the model data.
  overTempGetModelError(over_temp_cal,
                over_temp_cal->debug_overtempcal.temp_sensitivity,
                over_temp_cal->debug_overtempcal.sensor_intercept,
                over_temp_cal->debug_overtempcal.max_error);
}

void overTempCalDebugPrint(struct OverTempCal *over_temp_cal,
                           uint64_t timestamp_nanos) {
  ASSERT_NOT_NULL(over_temp_cal);

  static enum OverTempCalDebugState next_state = 0;
  static uint64_t wait_timer = 0;
  static size_t i = 0;  // Counter.
  createDebugTag(over_temp_cal, ":REPORT]");

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

    case OTC_WAIT_STATE:
      // This helps throttle the print statements.
      if (NANO_TIMER_CHECK_T1_GEQUAL_T2_PLUS_DELTA(timestamp_nanos, wait_timer,
                                                   OTC_WAIT_TIME_NANOS)) {
        over_temp_cal->debug_state = next_state;
      }
      break;

    case OTC_PRINT_OFFSET:
      // Prints out the latest offset estimate (input data).
      CAL_DEBUG_LOG(
          over_temp_cal->otc_debug_tag,
          "Cal#|Offset|Temp|Time [%s|C|nsec]: %lu, %s%d.%06d, "
          "%s%d.%06d, %s%d.%06d, %s%d.%03d, %llu",
          over_temp_cal->otc_unit_tag,
          (unsigned long int)over_temp_cal->debug_num_estimates,
          CAL_ENCODE_FLOAT(
              over_temp_cal->debug_overtempcal.latest_offset.offset[0] *
                  over_temp_cal->otc_unit_conversion,
              6),
          CAL_ENCODE_FLOAT(
              over_temp_cal->debug_overtempcal.latest_offset.offset[1] *
                  over_temp_cal->otc_unit_conversion,
              6),
          CAL_ENCODE_FLOAT(
              over_temp_cal->debug_overtempcal.latest_offset.offset[2] *
                  over_temp_cal->otc_unit_conversion,
              6),
          CAL_ENCODE_FLOAT(over_temp_cal->debug_overtempcal.latest_offset
                               .offset_temp_celsius,
                           6),
          (unsigned long long int)
              over_temp_cal->debug_overtempcal.latest_offset.timestamp_nanos);

      wait_timer = timestamp_nanos;                 // Starts the wait timer.
      next_state = OTC_PRINT_MODEL_PARAMETERS;      // Sets the next state.
      over_temp_cal->debug_state = OTC_WAIT_STATE;  // First, go to wait state.
      break;

    case OTC_PRINT_MODEL_PARAMETERS:
      // Prints out the model parameters.
      CAL_DEBUG_LOG(over_temp_cal->otc_debug_tag,
                    "Cal#|Sensitivity|Intercept [%s/C|%s]: %lu, %s%d.%06d, "
                    "%s%d.%06d, %s%d.%06d, %s%d.%06d, %s%d.%06d, %s%d.%06d",
                    over_temp_cal->otc_unit_tag, over_temp_cal->otc_unit_tag,
                    (unsigned long int)over_temp_cal->debug_num_estimates,
                    CAL_ENCODE_FLOAT(
                        over_temp_cal->debug_overtempcal.temp_sensitivity[0] *
                            over_temp_cal->otc_unit_conversion,
                        6),
                    CAL_ENCODE_FLOAT(
                        over_temp_cal->debug_overtempcal.temp_sensitivity[1] *
                            over_temp_cal->otc_unit_conversion,
                        6),
                    CAL_ENCODE_FLOAT(
                        over_temp_cal->debug_overtempcal.temp_sensitivity[2] *
                            over_temp_cal->otc_unit_conversion,
                        6),
                    CAL_ENCODE_FLOAT(
                        over_temp_cal->debug_overtempcal.sensor_intercept[0] *
                            over_temp_cal->otc_unit_conversion,
                        6),
                    CAL_ENCODE_FLOAT(
                        over_temp_cal->debug_overtempcal.sensor_intercept[1] *
                            over_temp_cal->otc_unit_conversion,
                        6),
                    CAL_ENCODE_FLOAT(
                        over_temp_cal->debug_overtempcal.sensor_intercept[2] *
                            over_temp_cal->otc_unit_conversion,
                        6));

      wait_timer = timestamp_nanos;                 // Starts the wait timer.
      next_state = OTC_PRINT_MODEL_ERROR;           // Sets the next state.
      over_temp_cal->debug_state = OTC_WAIT_STATE;  // First, go to wait state.
      break;

    case OTC_PRINT_MODEL_ERROR:
      // Computes the maximum error over all of the model data.
      CAL_DEBUG_LOG(
          over_temp_cal->otc_debug_tag,
          "Cal#|#Updates|#ModelPts|Model Error|Update Time [%s|nsec]: %lu, "
          "%lu, %lu, %s%d.%06d, %s%d.%06d, %s%d.%06d, %llu",
          over_temp_cal->otc_unit_tag,
          (unsigned long int)over_temp_cal->debug_num_estimates,
          (unsigned long int)over_temp_cal->debug_num_model_updates,
          (unsigned long int)over_temp_cal->debug_overtempcal.num_model_pts,
          CAL_ENCODE_FLOAT(over_temp_cal->debug_overtempcal.max_error[0] *
                               over_temp_cal->otc_unit_conversion,
                           6),
          CAL_ENCODE_FLOAT(over_temp_cal->debug_overtempcal.max_error[1] *
                               over_temp_cal->otc_unit_conversion,
                           6),
          CAL_ENCODE_FLOAT(over_temp_cal->debug_overtempcal.max_error[2] *
                               over_temp_cal->otc_unit_conversion,
                           6),
          (unsigned long long int)
              over_temp_cal->debug_overtempcal.modelupdate_timestamp_nanos);

      i = 0;                          // Resets the model data printer counter.
      wait_timer = timestamp_nanos;       // Starts the wait timer.
      next_state = OTC_PRINT_MODEL_DATA;  // Sets the next state.
      over_temp_cal->debug_state = OTC_WAIT_STATE;  // First, go to wait state.
      break;

    case OTC_PRINT_MODEL_DATA:
      // Prints out all of the model data.
      if (i < over_temp_cal->num_model_pts) {
        CAL_DEBUG_LOG(
            over_temp_cal->otc_debug_tag,
            "  Model[%lu] [%s|C|nsec] = %s%d.%06d, %s%d.%06d, %s%d.%06d, "
            "%s%d.%03d, %llu",
            (unsigned long int)i, over_temp_cal->otc_unit_tag,
            CAL_ENCODE_FLOAT(over_temp_cal->model_data[i].offset[0] *
                                 over_temp_cal->otc_unit_conversion,
                             6),
            CAL_ENCODE_FLOAT(over_temp_cal->model_data[i].offset[1] *
                                 over_temp_cal->otc_unit_conversion,
                             6),
            CAL_ENCODE_FLOAT(over_temp_cal->model_data[i].offset[2] *
                                 over_temp_cal->otc_unit_conversion,
                             6),
            CAL_ENCODE_FLOAT(over_temp_cal->model_data[i].offset_temp_celsius,
                             3),
            (unsigned long long int)over_temp_cal->model_data[i]
                .timestamp_nanos);

        i++;
        wait_timer = timestamp_nanos;       // Starts the wait timer.
        next_state = OTC_PRINT_MODEL_DATA;  // Sets the next state.
        over_temp_cal->debug_state =
            OTC_WAIT_STATE;                 // First, go to wait state.
      } else {
        // Sends this state machine to its idle state.
        wait_timer = timestamp_nanos;       // Starts the wait timer.
        next_state = OTC_IDLE;              // Sets the next state.
        over_temp_cal->debug_state =
            OTC_WAIT_STATE;                 // First, go to wait state.
      }
      break;

    default:
      // Sends this state machine to its idle state.
      wait_timer = timestamp_nanos;                 // Starts the wait timer.
      next_state = OTC_IDLE;                        // Sets the next state.
      over_temp_cal->debug_state = OTC_WAIT_STATE;  // First, go to wait state.
  }
}

void overTempCalDebugDescriptors(struct OverTempCal *over_temp_cal,
                                 const char *otc_sensor_tag,
                                 const char *otc_unit_tag,
                                 float otc_unit_conversion) {
  ASSERT_NOT_NULL(over_temp_cal);
  ASSERT_NOT_NULL(otc_sensor_tag);
  ASSERT_NOT_NULL(otc_unit_tag);

  // Sets the sensor descriptor, displayed units, and unit conversion factor.
  strcpy(over_temp_cal->otc_sensor_tag, otc_sensor_tag);
  strcpy(over_temp_cal->otc_unit_tag, otc_unit_tag);
  over_temp_cal->otc_unit_conversion = otc_unit_conversion;
}

#endif  // OVERTEMPCAL_DBG_ENABLED