/*
 * ________________________________________________________________________________________________________
 * Copyright (c) 2017 InvenSense Inc. All rights reserved.
 *
 * This software, related documentation and any modifications thereto (collectively “Software”) is subject
 * to InvenSense and its licensors' intellectual property rights under U.S. and international copyright
 * and other intellectual property rights laws.
 *
 * InvenSense and its licensors retain all intellectual property and proprietary rights in and to the Software
 * and any use, reproduction, disclosure or distribution of the Software without an express license agreement
 * from InvenSense is strictly prohibited.
 *
 * EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, THE SOFTWARE IS
 * PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED
 * TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT.
 * EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, IN NO EVENT SHALL
 * INVENSENSE BE LIABLE FOR ANY DIRECT, SPECIAL, INDIRECT, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, OR ANY
 * DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT,
 * NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE
 * OF THE SOFTWARE.
 * ________________________________________________________________________________________________________
 */

#include "example_selftest.h"

/* InvenSense drivers and utils */
#include "Invn/EmbUtils/Message.h"

/* board driver */
#include "common.h"

/* --------------------------------------------------------------------------------------
 *  Static and extern variables
 * -------------------------------------------------------------------------------------- */

/* Just a handy variable to handle the IMU object */
static struct inv_imu_device imu_dev;

/* --------------------------------------------------------------------------------------
 *  Functions definition
 * -------------------------------------------------------------------------------------- */

int setup_imu_device(const struct inv_imu_serif *icm_serif)
{
	int     rc = 0;
	uint8_t who_am_i;

	/* Init device */
	rc = inv_imu_init(&imu_dev, icm_serif, NULL);
	if (rc != INV_ERROR_SUCCESS) {
		INV_MSG(INV_MSG_LEVEL_ERROR, "Failed to initialize IMU!");
		return rc;
	}

	/* Check WHOAMI */
	rc = inv_imu_get_who_am_i(&imu_dev, &who_am_i);
	if (rc != INV_ERROR_SUCCESS) {
		INV_MSG(INV_MSG_LEVEL_ERROR, "Failed to read whoami!");
		return rc;
	}

	if (who_am_i != ICM_WHOAMI) {
		INV_MSG(INV_MSG_LEVEL_ERROR, "Bad WHOAMI value!");
		INV_MSG(INV_MSG_LEVEL_ERROR, "Read 0x%02x, expected 0x%02x", who_am_i, ICM_WHOAMI);
		return INV_ERROR;
	}

	return rc;
}

int run_self_test(void)
{
	inv_imu_selftest_output_t     out;
	inv_imu_selftest_parameters_t params;
	int                           rc   = INV_ERROR_SUCCESS;
	static int                    iter = 0;

	rc |= inv_imu_init_selftest_parameters_struct(&imu_dev, &params);
	/* Update `params` if needed here */

	rc |= inv_imu_run_selftest(&imu_dev, params, &out);

	if (rc != INV_ERROR_SUCCESS) {
		INV_MSG(INV_MSG_LEVEL_ERROR, "[%u] An error occurred while running selftest (rc=%d)", iter,
		        rc);
	} else {
		/* Print self-test status */
		INV_MSG(INV_MSG_LEVEL_INFO, "[%u] Accel self-test %s", iter,
		        out.accel_status == 1 ? "OK" : "KO");
		if (out.accel_status != 1) {
			INV_MSG(INV_MSG_LEVEL_VERBOSE, "  - Accel X: %s", out.ax_status == 1 ? "OK" : "KO");
			INV_MSG(INV_MSG_LEVEL_VERBOSE, "  - Accel Y: %s", out.ay_status == 1 ? "OK" : "KO");
			INV_MSG(INV_MSG_LEVEL_VERBOSE, "  - Accel Z: %s", out.az_status == 1 ? "OK" : "KO");
			rc |= INV_ERROR;
		}

#if ICM_IS_GYRO_SUPPORTED
		INV_MSG(INV_MSG_LEVEL_INFO, "[%u] Gyro self-test %s", iter,
		        out.gyro_status == 1 ? "OK" : "KO");
		if (out.gyro_status != 1) {
			INV_MSG(INV_MSG_LEVEL_VERBOSE, "  - Gyro X: %s", out.gx_status == 1 ? "OK" : "KO");
			INV_MSG(INV_MSG_LEVEL_VERBOSE, "  - Gyro Y: %s", out.gy_status == 1 ? "OK" : "KO");
			INV_MSG(INV_MSG_LEVEL_VERBOSE, "  - Gyro Z: %s", out.gz_status == 1 ? "OK" : "KO");
			rc |= INV_ERROR;
		}

		/* Check incomplete state */
		if (out.gyro_status & 0x2) {
			INV_MSG(INV_MSG_LEVEL_ERROR, "[%u] Gyro self-test are incomplete.", iter);
			rc |= INV_ERROR;
		}
#endif
	}

	iter++;

	/* Print empty line to ease readability */
	INV_MSG(INV_MSG_LEVEL_INFO, "");

	return rc;
}
