/*
 * 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.
 */

/*
 * This module contains the algorithms for producing a gyroscope offset
 * calibration. The algorithm looks for periods of stillness as indicated by
 * accelerometer, magnetometer and gyroscope, and computes a bias estimate by
 * taking the average of the gyroscope during the stillness times.
 *
 * Currently, this algorithm is tuned such that the device is only considered
 * still when the device is on a stationary surface (e.g., not on a person).
 *
 * NOTE - Time units are agnostic (i.e., determined by the user's application
 * and usage). However, typical time units are nanoseconds.
 *
 * Required Sensors and Units:
 *       - Gyroscope     [rad/sec]
 *       - Accelerometer [m/sec^2]
 *
 * Optional Sensors and Units:
 *       - Magnetometer  [micro-Tesla, uT]
 *       - Temperature   [Celsius]
 *
 * #define GYRO_CAL_DBG_ENABLED to enable debug printout statements.
 */

#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_GYROSCOPE_GYRO_CAL_H_
#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_GYROSCOPE_GYRO_CAL_H_

#include "calibration/gyroscope/gyro_stillness_detect.h"

#ifdef GYRO_CAL_DBG_ENABLED
#include "calibration/sample_rate_estimator/sample_rate_estimator.h"
#endif  // GYRO_CAL_DBG_ENABLED

#ifdef __cplusplus
extern "C" {
#endif

#ifdef GYRO_CAL_DBG_ENABLED
// Debug printout state enumeration.
enum GyroCalDebugState {
  GYRO_IDLE = 0,
  GYRO_WAIT_STATE,
  GYRO_PRINT_OFFSET,
  GYRO_PRINT_STILLNESS_DATA,
  GYRO_PRINT_SAMPLE_RATE_AND_TEMPERATURE,
  GYRO_PRINT_GYRO_MINMAX_STILLNESS_MEAN,
  GYRO_PRINT_ACCEL_STATS,
  GYRO_PRINT_GYRO_STATS,
  GYRO_PRINT_MAG_STATS
};

// Gyro Cal debug information/data tracking structure.
struct DebugGyroCal {
  struct SampleRateEstimator sample_rate_estimator;
  uint64_t start_still_time_nanos;
  uint64_t end_still_time_nanos;
  uint64_t stillness_duration_nanos;
  float accel_stillness_conf;
  float gyro_stillness_conf;
  float mag_stillness_conf;
  float calibration[3];
  float accel_mean[3];
  float gyro_mean[3];
  float mag_mean[3];
  float accel_var[3];
  float gyro_var[3];
  float mag_var[3];
  float gyro_winmean_min[3];
  float gyro_winmean_max[3];
  float temperature_min_celsius;
  float temperature_max_celsius;
  float temperature_mean_celsius;
  bool using_mag_sensor;
};
#endif  // GYRO_CAL_DBG_ENABLED

// GyroCal algorithm parameters (see GyroCal and GyroStillDet for details).
struct GyroCalParameters {
  uint64_t min_still_duration_nanos;
  uint64_t max_still_duration_nanos;
  uint64_t calibration_time_nanos;
  uint64_t window_time_duration_nanos;
  float bias_x;  // units: radians per second
  float bias_y;
  float bias_z;
  float stillness_threshold;         // units: (radians per second)^2
  float stillness_mean_delta_limit;  // units: radians per second
  float gyro_var_threshold;          // units: (radians per second)^2
  float gyro_confidence_delta;       // units: (radians per second)^2
  float accel_var_threshold;         // units: (meters per second)^2
  float accel_confidence_delta;      // units: (meters per second)^2
  float mag_var_threshold;           // units: micro-tesla^2
  float mag_confidence_delta;        // units: micro-tesla^2
  float temperature_delta_limit_celsius;
  bool gyro_calibration_enable;
};

// Data structure for tracking min/max window mean during device stillness.
struct MinMaxWindowMeanData {
  float gyro_winmean_min[3];
  float gyro_winmean_max[3];
};

// Data structure for tracking temperature data during device stillness.
struct TemperatureMeanData {
  float temperature_min_celsius;
  float temperature_max_celsius;
  float latest_temperature_celsius;
  float mean_accumulator;
  size_t num_points;
};

struct GyroCal {
  // Stillness detectors.
  struct GyroStillDet accel_stillness_detect;
  struct GyroStillDet mag_stillness_detect;
  struct GyroStillDet gyro_stillness_detect;

  // Data for tracking temperature mean during periods of device stillness.
  struct TemperatureMeanData temperature_mean_tracker;

  // Data for tracking gyro mean during periods of device stillness.
  struct MinMaxWindowMeanData window_mean_tracker;

  // Aggregated sensor stillness threshold required for gyro bias calibration.
  float stillness_threshold;

  // Min and max durations for gyro bias calibration.
  uint64_t min_still_duration_nanos;
  uint64_t max_still_duration_nanos;

  // Duration of the stillness processing windows.
  uint64_t window_time_duration_nanos;

  // Timestamp when device started a still period.
  uint64_t start_still_time_nanos;

  // Gyro offset estimate, and the associated calibration temperature,
  // timestamp, and stillness confidence values.
  float bias_x, bias_y, bias_z;  // [rad/sec]
  float bias_temperature_celsius;
  float stillness_confidence;
  uint64_t calibration_time_nanos;

  // Current window end-time for all sensors. Used to assist in keeping
  // sensor data collection in sync. On initialization this will be set to
  // zero indicating that sensor data will be dropped until a valid end-time
  // is set from the first gyro timestamp received.
  uint64_t stillness_win_endtime_nanos;

  // Watchdog timer to reset to a known good state when data capture stalls.
  uint64_t gyro_watchdog_start_nanos;
  uint64_t gyro_watchdog_timeout_duration_nanos;

  // Flag is "true" when the magnetometer is used.
  bool using_mag_sensor;

  // Flag set by user to control whether calibrations are used (default:
  // "true").
  bool gyro_calibration_enable;

  // Flag is 'true' when a new calibration update is ready.
  bool new_gyro_cal_available;

  // Flag to indicate if device was previously still.
  bool prev_still;

  // Min and maximum stillness window mean. This is used to check the stability
  // of the mean values computed for the gyroscope (i.e., provides further
  // validation for stillness).
  float gyro_winmean_min[3];
  float gyro_winmean_max[3];
  float stillness_mean_delta_limit;

  // The mean temperature over the stillness period. The limit is used to check
  // for temperature stability and provide a gate for when temperature is
  // rapidly changing.
  float temperature_mean_celsius;
  float temperature_delta_limit_celsius;

  //----------------------------------------------------------------

#ifdef GYRO_CAL_DBG_ENABLED
  // Debug info.
  struct DebugGyroCal debug_gyro_cal;  // Debug data structure.
  enum GyroCalDebugState debug_state;  // Debug printout state machine.
  enum GyroCalDebugState next_state;   // Debug state machine next state.
  uint64_t wait_timer_nanos;           // Debug message throttle timer.
  size_t debug_calibration_count;      // Total number of cals performed.
  size_t debug_watchdog_count;         // Total number of watchdog timeouts.
  bool debug_print_trigger;            // Flag used to trigger data printout.
#endif                                 // GYRO_CAL_DBG_ENABLED
};

/////// FUNCTION PROTOTYPES //////////////////////////////////////////

// Initialize the gyro calibration data structure.
void gyroCalInit(struct GyroCal* gyro_cal,
                 const struct GyroCalParameters* parameters);

// 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,
                    uint64_t* calibration_time_nanos);

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

// Remove gyro bias from the calibration [rad/sec].
void gyroCalRemoveBias(struct GyroCal* gyro_cal, float xi, float yi, float zi,
                       float* xo, float* yo, float* zo);

// Returns true when a new gyro calibration is available.
bool gyroCalNewBiasAvailable(struct GyroCal* gyro_cal);

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

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

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

#ifdef GYRO_CAL_DBG_ENABLED
// Print debug data report.
void gyroCalDebugPrint(struct GyroCal* gyro_cal,
                       uint64_t timestamp_nanos_nanos);
#endif

#ifdef __cplusplus
}
#endif

#endif  // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_GYROSCOPE_GYRO_CAL_H_