/* Copyright 2020 The ChromiumOS Authors
 * Use of this source code is governed by a BSD-style license that can be
 * found in the LICENSE file.
 */

#include "common.h"
#include "console.h"
#include "accel_cal.h"

#define CPRINTS(format, args...) cprints(CC_MOTION_SENSE, format, ##args)

#define TEMP_RANGE (CONFIG_ACCEL_CAL_MAX_TEMP - CONFIG_ACCEL_CAL_MIN_TEMP)

void accel_cal_reset(struct accel_cal *cal)
{
	int i;

	for (i = 0; i < cal->num_temp_windows; ++i) {
		kasa_reset(&(cal->algos[i].kasa_fit));
		newton_fit_reset(&(cal->algos[i].newton_fit));
	}
}

static inline int compute_temp_gate(const struct accel_cal *cal, fp_t temp)
{
	int gate = (int)fp_div(fp_mul(temp - CONFIG_ACCEL_CAL_MIN_TEMP,
				      INT_TO_FP(cal->num_temp_windows)),
			       TEMP_RANGE);

	return gate < cal->num_temp_windows ? gate :
					      (cal->num_temp_windows - 1);
}

test_mockable bool accel_cal_accumulate(struct accel_cal *cal,
					uint32_t timestamp, fp_t x, fp_t y,
					fp_t z, fp_t temp)
{
	struct accel_cal_algo *algo;

	/* Test that we're within the temperature range. */
	if (temp >= CONFIG_ACCEL_CAL_MAX_TEMP ||
	    temp <= CONFIG_ACCEL_CAL_MIN_TEMP)
		return false;

	/* Test that we have a still sample. */
	if (!still_det_update(&cal->still_det, timestamp, x, y, z))
		return false;

	/* We have a still sample, update x, y, and z to the mean. */
	x = cal->still_det.mean_x;
	y = cal->still_det.mean_y;
	z = cal->still_det.mean_z;

	/* Compute the temp gate. */
	algo = &cal->algos[compute_temp_gate(cal, temp)];

	kasa_accumulate(&algo->kasa_fit, x, y, z);
	if (newton_fit_accumulate(&algo->newton_fit, x, y, z)) {
		fp_t radius;

		kasa_compute(&algo->kasa_fit, cal->bias, &radius);
		if (ABS(radius - FLOAT_TO_FP(1.0f)) <
		    CONFIG_ACCEL_CAL_KASA_RADIUS_THRES)
			goto accel_cal_accumulate_success;

		newton_fit_compute(&algo->newton_fit, cal->bias, &radius);
		if (ABS(radius - FLOAT_TO_FP(1.0f)) <
		    CONFIG_ACCEL_CAL_NEWTON_RADIUS_THRES)
			goto accel_cal_accumulate_success;
	}

	return false;

accel_cal_accumulate_success:
	accel_cal_reset(cal);

	return true;
}