/* 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 "motion_sense.h"
#include "newton_fit.h"
#include "test_util.h"

#include <stdio.h>

/*
 * Need to define motion sensor globals just to compile.
 * We include motion task to force the inclusion of math_util.c
 */
struct motion_sensor_t motion_sensors[] = {};
const unsigned int motion_sensor_count = ARRAY_SIZE(motion_sensors);

#define ACC(FIT, X, Y, Z, EXPECTED) \
	TEST_EQ(newton_fit_accumulate(FIT, X, Y, Z), EXPECTED, "%d")

static int test_newton_fit_reset(void)
{
	struct newton_fit fit = NEWTON_FIT(4, 15, 0.01f, 0.25f, 1.0e-8f, 100);

	newton_fit_reset(&fit);
	newton_fit_accumulate(&fit, 1.0f, 0.0f, 0.0f);
	TEST_EQ(queue_count(fit.orientations), (size_t)1, "%zu");
	newton_fit_reset(&fit);

	TEST_EQ(queue_count(fit.orientations), (size_t)0, "%zu");

	return EC_SUCCESS;
}

static int test_newton_fit_accumulate(void)
{
	struct newton_fit fit = NEWTON_FIT(4, 15, 0.01f, 0.25f, 1.0e-8f, 100);
	struct queue_iterator it;

	newton_fit_reset(&fit);
	newton_fit_accumulate(&fit, 1.0f, 0.0f, 0.0f);

	TEST_EQ(queue_count(fit.orientations), (size_t)1, "%zu");
	queue_begin(fit.orientations, &it);
	TEST_EQ(((struct newton_fit_orientation *)it.ptr)->nsamples, 1, "%u");

	return EC_SUCCESS;
}

static int test_newton_fit_accumulate_merge(void)
{
	struct newton_fit fit = NEWTON_FIT(4, 15, 0.01f, 0.25f, 1.0e-8f, 100);
	struct queue_iterator it;

	newton_fit_reset(&fit);
	newton_fit_accumulate(&fit, 1.0f, 0.0f, 0.0f);
	newton_fit_accumulate(&fit, 1.05f, 0.0f, 0.0f);

	TEST_EQ(queue_count(fit.orientations), (size_t)1, "%zu");
	queue_begin(fit.orientations, &it);
	TEST_EQ(((struct newton_fit_orientation *)it.ptr)->nsamples, 2, "%u");

	return EC_SUCCESS;
}

static int test_newton_fit_accumulate_prune(void)
{
	struct newton_fit fit = NEWTON_FIT(4, 15, 0.01f, 0.25f, 1.0e-8f, 100);
	struct queue_iterator it;

	newton_fit_reset(&fit);
	newton_fit_accumulate(&fit, 1.0f, 0.0f, 0.0f);
	newton_fit_accumulate(&fit, -1.0f, 0.0f, 0.0f);
	newton_fit_accumulate(&fit, 0.0f, 1.0f, 0.0f);
	newton_fit_accumulate(&fit, 0.0f, -1.0f, 0.0f);

	TEST_EQ(queue_is_full(fit.orientations), 1, "%d");
	queue_begin(fit.orientations, &it);
	TEST_EQ(((struct newton_fit_orientation *)it.ptr)->nsamples, 1, "%u");
	queue_next(fit.orientations, &it);
	TEST_EQ(((struct newton_fit_orientation *)it.ptr)->nsamples, 1, "%u");
	queue_next(fit.orientations, &it);
	TEST_EQ(((struct newton_fit_orientation *)it.ptr)->nsamples, 1, "%u");
	queue_next(fit.orientations, &it);
	TEST_EQ(((struct newton_fit_orientation *)it.ptr)->nsamples, 1, "%u");

	newton_fit_accumulate(&fit, 0.0f, 0.0f, 1.0f);
	TEST_EQ(queue_is_full(fit.orientations), 0, "%d");

	return EC_SUCCESS;
}

static int test_newton_fit_calculate(void)
{
	struct newton_fit fit = NEWTON_FIT(4, 3, 0.01f, 0.25f, 1.0e-8f, 100);
	floatv3_t bias;
	float radius;

	newton_fit_reset(&fit);

	ACC(&fit, 1.01f, 0.01f, 0.01f, false);
	ACC(&fit, 1.01f, 0.01f, 0.01f, false);
	ACC(&fit, 1.01f, 0.01f, 0.01f, false);

	ACC(&fit, -0.99f, 0.01f, 0.01f, false);
	ACC(&fit, -0.99f, 0.01f, 0.01f, false);
	ACC(&fit, -0.99f, 0.01f, 0.01f, false);

	ACC(&fit, 0.01f, 1.01f, 0.01f, false);
	ACC(&fit, 0.01f, 1.01f, 0.01f, false);
	ACC(&fit, 0.01f, 1.01f, 0.01f, false);

	ACC(&fit, 0.01f, 0.01f, 1.01f, false);
	ACC(&fit, 0.01f, 0.01f, 1.01f, false);
	ACC(&fit, 0.01f, 0.01f, 1.01f, true);

	fpv3_init(bias, 0.0f, 0.0f, 0.0f);
	newton_fit_compute(&fit, bias, &radius);

	TEST_NEAR(bias[0], 0.01f, 0.0001f, "%f");
	TEST_NEAR(bias[1], 0.01f, 0.0001f, "%f");
	TEST_NEAR(bias[2], 0.01f, 0.0001f, "%f");
	TEST_NEAR(radius, 1.0f, 0.0001f, "%f");

	return EC_SUCCESS;
}

void run_test(int argc, const char **argv)
{
	test_reset();

	RUN_TEST(test_newton_fit_reset);
	RUN_TEST(test_newton_fit_accumulate);
	RUN_TEST(test_newton_fit_accumulate_merge);
	RUN_TEST(test_newton_fit_accumulate_prune);
	RUN_TEST(test_newton_fit_calculate);

	test_print_result();
}