/*
 * Copyright (c) 2021, Nordic Semiconductor ASA
 *
 * SPDX-License-Identifier: Apache-2.0
 */

#include <zephyr/drivers/i2c.h>
#include <zephyr/drivers/sensor.h>
#include <zephyr/logging/log.h>

#include "mpu9250.h"
#include "ak8963.h"

LOG_MODULE_DECLARE(MPU9250, CONFIG_SENSOR_LOG_LEVEL);


#define I2C_READ_FLAG			BIT(7)

#define AK8963_I2C_ADDR			0x0C

#define AK8963_REG_ID			0x00
#define AK8963_REG_ID_VAL		0x48

#define AK8963_REG_DATA			0x03

#define AK8963_ST2_OVRFL_BIT		BIT(3)

#define AK8963_REG_CNTL1			0x0A
#define AK8963_REG_CNTL1_POWERDOWN_VAL		0x00
#define AK8963_REG_CNTL1_FUSE_ROM_VAL		0x0F
#define AK8963_REG_CNTL1_16BIT_100HZ_VAL	0x16
#define AK8963_SET_MODE_DELAY_MS		1

#define AK8963_REG_CNTL2			0x0B
#define AK8963_REG_CNTL2_RESET_VAL		0x01
#define AK8963_RESET_DELAY_MS			1

#define AK8963_REG_ADJ_DATA_X			0x10
#define AK8963_REG_ADJ_DATA_Y			0x11
#define AK8963_REG_ADJ_DATA_Z			0x12

#define AK9863_SCALE_TO_UG			1499

#define MPU9250_REG_I2C_MST_CTRL			0x24
#define MPU9250_REG_I2C_MST_CTRL_WAIT_MAG_400KHZ_VAL	0x4D

#define MPU9250_REG_I2C_SLV0_ADDR			0x25
#define MPU9250_REG_I2C_SLV0_REG			0x26
#define MPU9250_REG_I2C_SLV0_CTRL			0x27
#define MPU9250_REG_I2C_SLV0_DATA0			0x63
#define MPU9250_REG_READOUT_CTRL_VAL			(BIT(7) | 0x07)

#define MPU9250_REG_USER_CTRL				0x6A
#define MPU9250_REG_USER_CTRL_I2C_MASTERMODE_VAL	0x20

#define MPU9250_REG_EXT_DATA00				0x49

#define MPU9250_REG_I2C_SLV4_ADDR			0x31
#define MPU9250_REG_I2C_SLV4_REG			0x32
#define MPU9250_REG_I2C_SLV4_DO				0x33
#define MPU9250_REG_I2C_SLV4_CTRL			0x34
#define MPU9250_REG_I2C_SLV4_CTRL_VAL			0x80
#define MPU9250_REG_I2C_SLV4_DI				0x35

#define MPU9250_I2C_MST_STS				0x36
#define MPU9250_I2C_MST_STS_SLV4_DONE			BIT(6)


int ak8963_convert_magn(struct sensor_value *val, int16_t raw_val,
			 int16_t scale, uint8_t st2)
{
	/* The sensor device returns 10^-9 Teslas after scaling.
	 * Scale adjusts for calibration data and units
	 * So sensor instance returns Gauss units
	 */

	/* If overflow happens then value is invalid */
	if ((st2 & AK8963_ST2_OVRFL_BIT) != 0) {
		LOG_INF("Magnetometer value overflow.");
		return -EOVERFLOW;
	}

	int32_t scaled_val = (int32_t)raw_val * (int32_t)scale;

	val->val1 = scaled_val / 1000000;
	val->val2 = scaled_val % 1000000;
	return 0;
}


static int ak8963_execute_rw(const struct device *dev, uint8_t reg, bool write)
{
	/* Instruct the MPU9250 to access over its external i2c bus
	 * given device register with given details
	 */
	const struct mpu9250_config *cfg = dev->config;
	uint8_t mode_bit = 0x00;
	uint8_t status;
	int ret;

	if (!write) {
		mode_bit = I2C_READ_FLAG;
	}

	/* Set target i2c address */
	ret = i2c_reg_write_byte_dt(&cfg->i2c,
				    MPU9250_REG_I2C_SLV4_ADDR,
				    AK8963_I2C_ADDR | mode_bit);
	if (ret < 0) {
		LOG_ERR("Failed to write i2c target slave address.");
		return ret;
	}

	/* Set target i2c register */
	ret = i2c_reg_write_byte_dt(&cfg->i2c,
				    MPU9250_REG_I2C_SLV4_REG,
				    reg);
	if (ret < 0) {
		LOG_ERR("Failed to write i2c target slave register.");
		return ret;
	}

	/* Initiate transfer  */
	ret = i2c_reg_write_byte_dt(&cfg->i2c,
				    MPU9250_REG_I2C_SLV4_CTRL,
				    MPU9250_REG_I2C_SLV4_CTRL_VAL);
	if (ret < 0) {
		LOG_ERR("Failed to initiate i2c slave transfer.");
		return ret;
	}

	/* Wait for a transfer to be ready */
	do {
		ret = i2c_reg_read_byte_dt(&cfg->i2c,
					   MPU9250_I2C_MST_STS, &status);
		if (ret < 0) {
			LOG_ERR("Waiting for slave failed.");
			return ret;
		}
	} while (!(status & MPU9250_I2C_MST_STS_SLV4_DONE));

	return 0;
}

static int ak8963_read_reg(const struct device *dev, uint8_t reg, uint8_t *data)
{
	const struct mpu9250_config *cfg = dev->config;
	int ret;

	/* Execute transfer */
	ret = ak8963_execute_rw(dev, reg, false);
	if (ret < 0) {
		LOG_ERR("Failed to prepare transfer.");
		return ret;
	}

	/* Read the result */
	ret = i2c_reg_read_byte_dt(&cfg->i2c,
				   MPU9250_REG_I2C_SLV4_DI, data);
	if (ret < 0) {
		LOG_ERR("Failed to read data from slave.");
		return ret;
	}

	return 0;
}

static int ak8963_write_reg(const struct device *dev, uint8_t reg, uint8_t data)
{
	const struct mpu9250_config *cfg = dev->config;
	int ret;

	/* Set the data to write */
	ret = i2c_reg_write_byte_dt(&cfg->i2c,
				    MPU9250_REG_I2C_SLV4_DO, data);
	if (ret < 0) {
		LOG_ERR("Failed to write data to slave.");
		return ret;
	}

	/* Execute transfer */
	ret = ak8963_execute_rw(dev, reg, true);
	if (ret < 0) {
		LOG_ERR("Failed to transfer write to slave.");
		return ret;
	}

	return 0;
}


static int ak8963_set_mode(const struct device *dev, uint8_t mode)
{
	int ret;

	ret = ak8963_write_reg(dev, AK8963_REG_CNTL1, mode);
	if (ret < 0) {
		LOG_ERR("Failed to set AK8963 mode.");
		return ret;
	}

	/* Wait for mode to change */
	k_msleep(AK8963_SET_MODE_DELAY_MS);
	return 0;
}

static int16_t ak8963_calc_adj(int16_t val)
{

	/** Datasheet says the actual register value is in 16bit output max
	 *  value of 32760 that corresponds to 4912 uT flux, yielding factor
	 *  of 0.149938.
	 *
	 *  Now Zephyr unit is Gauss, and conversion is 1T = 10^4G
	 *  -> 0.1499 * 10^4 = 1499
	 *  So if we multiply with scaling with 1499 the unit is uG.
	 *
	 *  Calculation from MPU-9250 Register Map and Descriptions
	 *  adj = (((val-128)*0.5)/128)+1
	 */
	return ((AK9863_SCALE_TO_UG * (val - 128)) / 256) + AK9863_SCALE_TO_UG;
}

static int ak8963_fetch_adj(const struct device *dev)
{
	/* Read magnetometer adjustment data from the AK8963 chip */
	struct mpu9250_data *drv_data = dev->data;
	uint8_t buf;
	int ret;

	/* Change to FUSE access mode to access adjustment registers */
	ret = ak8963_set_mode(dev, AK8963_REG_CNTL1_FUSE_ROM_VAL);
	if (ret < 0) {
		LOG_ERR("Failed to set chip in fuse access mode.");
		return ret;
	}

	ret = ak8963_read_reg(dev, AK8963_REG_ADJ_DATA_X, &buf);
	if (ret < 0) {
		LOG_ERR("Failed to read adjustment data.");
		return ret;
	}
	drv_data->magn_scale_x = ak8963_calc_adj(buf);

	ret = ak8963_read_reg(dev, AK8963_REG_ADJ_DATA_Y, &buf);
	if (ret < 0) {
		LOG_ERR("Failed to read adjustment data.");
		return ret;
	}
	drv_data->magn_scale_y = ak8963_calc_adj(buf);

	ret = ak8963_read_reg(dev, AK8963_REG_ADJ_DATA_Z, &buf);
	if (ret < 0) {
		LOG_ERR("Failed to read adjustment data.");
		return ret;
	}
	drv_data->magn_scale_z = ak8963_calc_adj(buf);

	/* Change back to the powerdown mode */
	ret = ak8963_set_mode(dev, AK8963_REG_CNTL1_POWERDOWN_VAL);
	if (ret < 0) {
		LOG_ERR("Failed to set chip in power down mode.");
		return ret;
	}

	LOG_DBG("Adjustment values %d %d %d", drv_data->magn_scale_x,
		drv_data->magn_scale_y, drv_data->magn_scale_z);

	return 0;
}

static int ak8963_reset(const struct device *dev)
{
	int ret;

	/* Reset the chip -> reset all settings. */
	ret = ak8963_write_reg(dev, AK8963_REG_CNTL2,
			       AK8963_REG_CNTL2_RESET_VAL);
	if (ret < 0) {
		LOG_ERR("Failed to reset AK8963.");
		return ret;
	}

	/* Wait for reset */
	k_msleep(AK8963_RESET_DELAY_MS);

	return 0;
}

static int ak8963_init_master(const struct device *dev)
{
	const struct mpu9250_config *cfg = dev->config;
	int ret;

	/* Instruct MPU9250 to use its external I2C bus as master */
	ret = i2c_reg_write_byte_dt(&cfg->i2c,
				    MPU9250_REG_USER_CTRL,
				    MPU9250_REG_USER_CTRL_I2C_MASTERMODE_VAL);
	if (ret < 0) {
		LOG_ERR("Failed to set MPU9250 master i2c mode.");
		return ret;
	}

	/* Set MPU9250 I2C bus as 400kHz and issue interrupt at data ready. */
	ret = i2c_reg_write_byte_dt(&cfg->i2c,
				    MPU9250_REG_I2C_MST_CTRL,
				    MPU9250_REG_I2C_MST_CTRL_WAIT_MAG_400KHZ_VAL);
	if (ret < 0) {
		LOG_ERR("Failed to set MPU9250 master i2c speed.");
		return ret;
	}

	return 0;
}

static int ak8963_init_readout(const struct device *dev)
{
	const struct mpu9250_config *cfg = dev->config;
	int ret;

	/* Set target i2c address */
	ret = i2c_reg_write_byte_dt(&cfg->i2c,
				    MPU9250_REG_I2C_SLV0_ADDR,
				    AK8963_I2C_ADDR | I2C_READ_FLAG);
	if (ret < 0) {
		LOG_ERR("Failed to set AK8963 slave address.");
		return ret;
	}

	/* Set target as data registers */
	ret = i2c_reg_write_byte_dt(&cfg->i2c,
				    MPU9250_REG_I2C_SLV0_REG, AK8963_REG_DATA);
	if (ret < 0) {
		LOG_ERR("Failed to set AK8963 register address.");
		return ret;
	}

	/* Initiate readout at sample rate */
	ret = i2c_reg_write_byte_dt(&cfg->i2c,
				    MPU9250_REG_I2C_SLV0_CTRL,
				    MPU9250_REG_READOUT_CTRL_VAL);
	if (ret < 0) {
		LOG_ERR("Failed to init AK8963 value readout.");
		return ret;
	}

	return 0;
}

int ak8963_init(const struct device *dev)
{
	uint8_t buf;
	int ret;

	ret = ak8963_init_master(dev);
	if (ret < 0) {
		LOG_ERR("Initializing MPU9250 master mode failed.");
		return ret;
	}

	ret = ak8963_reset(dev);
	if (ret < 0) {
		LOG_ERR("Resetting AK8963 failed.");
		return ret;
	}

	/* First check that the chip says hello */
	ret = ak8963_read_reg(dev, AK8963_REG_ID, &buf);
	if (ret < 0) {
		LOG_ERR("Failed to read AK8963 chip id.");
		return ret;
	}

	if (buf != AK8963_REG_ID_VAL) {
		LOG_ERR("Invalid AK8963 chip id (0x%X).", buf);
		return -ENOTSUP;
	}

	/* Fetch calibration data */
	ret = ak8963_fetch_adj(dev);
	if (ret < 0) {
		LOG_ERR("Calibrating AK8963 failed.");
		return ret;
	}

	/* Set AK sample rate and resolution */
	ret = ak8963_set_mode(dev, AK8963_REG_CNTL1_16BIT_100HZ_VAL);
	if (ret < 0) {
		LOG_ERR("Failed set sample rate for AK8963.");
		return ret;
	}

	/* Init constant readouts at sample rate */
	ret = ak8963_init_readout(dev);
	if (ret < 0) {
		LOG_ERR("Initializing AK8963 readout failed.");
		return ret;
	}

	return 0;
}