/*
 * Copyright (c) 2020 TDK Invensense
 *
 * SPDX-License-Identifier: Apache-2.0
 */

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

#include "icp20100.h"
#include "icp20100_reg.h"
#include "icp20100_spi.h"

LOG_MODULE_DECLARE(ICP20100, CONFIG_SENSOR_LOG_LEVEL);

int icp20100_set_mode(const struct device *dev, uint8_t mode, uint8_t enabled)
{
	const struct icp20100_config *cfg = dev->config;
	uint8_t databuf;
	int result;

	if (mode > 3) {
		LOG_ERR("Not supported mode");
		return -ENOTSUP;
	}

	result = inv20100_spi_read(&cfg->spi, REG_MODE_SELECT, &databuf, 1);

	if (result) {
		return result;
	}

	databuf &= 0x1F;

	if (mode == 3) {
		databuf |= BIT_MODE_SELECT_MODE3;
	} else if (mode == 2) {
		databuf |= BIT_MODE_SELECT_MODE2;
	} else if (mode == 1) {
		databuf |= BIT_MODE_SELECT_MODE1;
	} else if (mode == 0) {
		databuf |= BIT_MODE_SELECT_MODE0;
	}

	if (enabled) {
		databuf |= BIT_MODE_SELECT_ACTIVE;
	} else {
		databuf &= ~(BIT_MODE_SELECT_ACTIVE);
	}

	result = inv20100_spi_single_write(&cfg->spi, REG_MODE_SELECT, &databuf);

	if (result) {
		return result;
	}

	LOG_DBG("Write MODE_SELECT 0x%X", databuf);
}

int icp20100_sensor_init(const struct device *dev)
{
	const struct icp20100_config *cfg = dev->config;
	int result = 0;
	uint8_t v;

	result = inv20100_spi_read(&cfg->spi, REG_DEVICE_ID, &v, 1);

	if (result) {
		return result;
	}

	LOG_DBG("WHO AM I : 0x%X", v);

	result = inv20100_spi_read(&cfg->spi, REG_VERSION, &v, 1);

	if (result) {
		LOG_DBG("read REG_VERSION failed");
		return result;
	}

	if (v != 0xB2) {
		LOG_ERR("Requires version B of the sensor.");
		return -1;
	}
	return 0;
}

int icp20100_turn_on_fifo(const struct device *dev, uint8_t enable_interrupt)
{
	const struct icp20100_data *drv_data = dev->data;
	const struct icp20100_config *cfg = dev->config;

	int result;
	uint8_t v = 0;

	v = BIT_FIFO_CFG_WM_H_14 | BIT_FIFO_CFG_WM_L_DIS;
	result = inv20100_spi_single_write(&cfg->spi, REG_FIFO_CONFIG, &v);
	if (result) {
		return result;
	}

	if (enable_interrupt) {
		v = ~(BIT_INT_STATUS_WMK_H);
		result = inv20100_spi_single_write(&cfg->spi, REG_INTERRUPT_MASK, &v);
		if (result) {
			return result;
		}
	}

	LOG_DBG("turn on fifo done");
	return 0;
}

int icp20100_turn_off_fifo(const struct device *dev)
{
	const struct icp20100_data *drv_data = dev->data;
	const struct icp20100_config *cfg = dev->config;
	int result;
	uint8_t v = 0;

	v = BIT_FIFO_CFG_WM_H_DIS | BIT_FIFO_CFG_WM_L_DIS;
	result = inv20100_spi_single_write(&cfg->spi, REG_FIFO_CONFIG, &v);
	if (result) {
		return result;
	}

	v = ~(0);
	result = inv20100_spi_single_write(&cfg->spi, REG_INTERRUPT_MASK, &v);
	if (result) {
		return result;
	}

	return 0;
}

int icp20100_turn_on_sensor(const struct device *dev)
{
	struct icp20100_data *drv_data = dev->data;
	const struct icp20100_config *cfg = dev->config;
	uint8_t v = 0;
	int result = 0;


	if (drv_data->sensor_started) {
		LOG_ERR("Sensor already started");
		return -EALREADY;
	}

	icp20100_turn_on_fifo(dev, 0);
	icp20100_set_mode(dev, 0, 1);

	v = 0;
	while ((v & BIT_INT_STATUS_WMK_H) == 0) {
		result = inv20100_spi_read(&cfg->spi, REG_INTERRUPT_STATUS, &v, 1);
		if (result) {
			return result;
		}
	}

	icp20100_set_mode(dev, 0, 0);

	k_msleep(1);

	v = BIT_FIFO_FLUSH;
	result = inv20100_spi_single_write(&cfg->spi, REG_FIFO_FILL, &v);
	if (result) {
		return result;
	}

	icp20100_turn_on_fifo(dev, 1);
	icp20100_set_mode(dev, 0, 1);

	drv_data->sensor_started = true;

	return 0;
}

int icp20100_turn_off_sensor(const struct device *dev)
{
	struct icp20100_data *drv_data = dev->data;
	const struct icp20100_config *cfg = dev->config;
	uint8_t v = 0;
	int result = 0;

	icp20100_set_mode(dev, 0, 0);
	icp20100_turn_off_fifo(dev);

	k_msleep(1);

	v = BIT_FIFO_FLUSH;
	result = inv20100_spi_single_write(&cfg->spi, REG_FIFO_FILL, &v);
	if (result) {
		return result;
	}

	return 0;
}
