// Copyright 2018 The Periph Authors. All rights reserved.
// Use of this source code is governed under the Apache License, Version 2.0
// that can be found in the LICENSE file.

// Package mpu9250 MPU-9250 is a 9-axis MotionTracking device that combines a
// 3-axis gyroscope, 3-axis accelerometer, 3-axis magnetometer and a Digital
// Motion Processor™ (DMP). Connections via SPI or I2C are supported.
//
// There is a sample program in periph.io/x/cmd/mpu9250 that you can run to
// test using the sensor.
//
// # Datasheet
//
// https://www.invensense.com/wp-content/uploads/2015/02/PS-MPU-9250A-01-v1.1.pdf
// https://www.invensense.com/wp-content/uploads/2015/02/MPU-9250-Register-Map.pdf
package mpu9250

import (
	"fmt"
	"math"
	"time"

	"periph.io/x/devices/v3/mpu9250/reg"
)

const (
	slaveNumberError = "slave number 0 .. 3"
	registers        = 12
	accelsenSitivity = 16384
)

// AccelerometerData the values for x/y/z axises.
type AccelerometerData struct {
	X, Y, Z int16
}

// GyroscopeData the values for x/y/z axises.
type GyroscopeData struct {
	X, Y, Z int16
}

// RotationData the rotation around X/Y/Z axises.
type RotationData struct {
	X, Y, Z int16
}

// Deviation defines the standard deviation for major axises.
type Deviation struct {
	X, Y, Z float64
}

// SelfTestResult defines the results for self-test for accelerometer, gyroscope.
type SelfTestResult struct {
	AccelDeviation Deviation
	GyroDeviation  Deviation
}

// MPU9250 defines the structure to keep reference to the transport.
type MPU9250 struct {
	transport Transport
	debug     func(string, ...interface{})
}

// New creates the new instance of the driver.
//
// transport the transport interface.
func New(transport Transport) (*MPU9250, error) {
	return &MPU9250{transport: transport, debug: noop}, nil
}

// Debug sets the debug logger implementation.
func (m *MPU9250) Debug(f DebugF) {
	m.debug = f
}

// Init initializes the device.
func (m *MPU9250) Init() error {
	return m.transferBatch(initSequence, "error initializing %d: [%x:%x] => %v")
}

// Calibrate calibrates the device using maximum precision for both Gyroscope
// and Accelerometer.
func (m *MPU9250) Calibrate() error {
	if err := m.transferBatch(calibrateSequence, "error calibrating %d: [%x:%x] => %v"); err != nil {
		return err
	}
	reads, err := m.GetFIFOCount() // read FIFO sample count
	if err != nil {
		return wrapf("can't get FIFO => %v", err)
	}

	m.debug("Read %d packets\n", reads)

	packets := reads / registers

	var buffer [registers]byte

	toUint16 := func(offset int) int16 {
		return int16(buffer[offset])<<8 | int16(buffer[offset+1])
	}

	writeGyroOffset := func(v int16, h, l byte) error {
		o := (-v) >> 2
		if err = m.transport.writeByte(h, byte(o>>8)); err != nil {
			return wrapf("can't write Gyro offset %x(h):%x => %w", h, o, err)
		}
		if err = m.transport.writeByte(l, byte(o&0xFF)); err != nil {
			return wrapf("can't write Gyro offset %x(l):%x => %w", l, o, err)
		}
		return nil
	}

	writeAccelOffset := func(o uint16, h, l byte) error {
		if err = m.transport.writeByte(h, byte(o>>8)); err != nil {
			return wrapf("can't write Accelerator %x(h):%x => %w", h, o, err)
		}
		if err = m.transport.writeByte(l, byte(o&0xFF)); err != nil {
			return wrapf("can't write Accelerator %x(l):%x => %w", l, o, err)
		}
		return nil
	}

	var (
		accelX, accelY, accelZ, gyroX, gyroY, gyroZ                         int64
		accelXBias, accelYBias, accelZBias, gyroXBias, gyroYBias, gyroZBias int16
	)

	for i := 0; i < int(packets); i++ {
		for j := 0; j < registers; j++ {
			b := byte(0)
			if b, err = m.GetFIFOByte(); err != nil {
				return wrapf("can't read data byte %d of packet %d => %w", j, i, err)
			}
			buffer[j] = b
		}
		accelX += int64(toUint16(0))
		accelY += int64(toUint16(2))
		accelZ += int64(toUint16(4))
		gyroX += int64(toUint16(6))
		gyroY += int64(toUint16(8))
		gyroZ += int64(toUint16(10))
	}

	accelXBias = int16(accelX / int64(packets))
	accelYBias = int16(accelY / int64(packets))
	accelZBias = int16(accelZ / int64(packets))
	gyroXBias = int16(gyroX / int64(packets))
	gyroYBias = int16(gyroY / int64(packets))
	gyroZBias = int16(gyroZ / int64(packets))

	m.debug("Raw accelerometer bias: X:%d, Y:%d, Z:%d\n", accelXBias, accelYBias, accelZBias)
	m.debug("Raw gyroscope bias X:%d, Y:%d, Z:%d\n", gyroXBias, gyroYBias, gyroZBias)

	if accelZBias > 0 {
		accelZBias -= accelsenSitivity
	} else {
		accelZBias += accelsenSitivity
	}

	var factoryGyroBiasX, factoryGyroBiasY, factoryGyroBiasZ int16
	factoryGyroBiasX, err = m.ReadSignedWord(reg.MPU9250_XG_OFFSET_H, reg.MPU9250_XG_OFFSET_L)
	if err != nil {
		return err
	}
	factoryGyroBiasY, err = m.ReadSignedWord(reg.MPU9250_YG_OFFSET_H, reg.MPU9250_YG_OFFSET_L)
	if err != nil {
		return err
	}
	factoryGyroBiasZ, err = m.ReadSignedWord(reg.MPU9250_ZG_OFFSET_H, reg.MPU9250_ZG_OFFSET_L)
	if err != nil {
		return err
	}
	m.debug("Factory gyroscope bias: X:%d, Y:%d, Z:%d\n", int16(factoryGyroBiasX), int16(factoryGyroBiasY), int16(factoryGyroBiasZ))

	if err = writeGyroOffset(gyroXBias, reg.MPU9250_GYRO_XOUT_H, reg.MPU9250_GYRO_XOUT_L); err != nil {
		return err
	}
	if err = writeGyroOffset(gyroYBias, reg.MPU9250_GYRO_YOUT_H, reg.MPU9250_GYRO_YOUT_L); err != nil {
		return err
	}
	if err = writeGyroOffset(gyroZBias, reg.MPU9250_GYRO_ZOUT_H, reg.MPU9250_GYRO_ZOUT_L); err != nil {
		return err
	}

	// Construct the accelerometer biases for push to the hardware accelerometer
	// bias registers. These registers contain factory trim values which must be
	// added to the calculated accelerometer biases; on boot up these registers
	// will hold non-zero values.
	var factoryBiasX, factoryBiasY, factoryBiasZ int16
	factoryBiasX, err = m.ReadSignedWord(reg.MPU9250_XA_OFFSET_H, reg.MPU9250_XA_OFFSET_L)
	if err != nil {
		return err
	}
	factoryBiasY, err = m.ReadSignedWord(reg.MPU9250_YA_OFFSET_H, reg.MPU9250_YA_OFFSET_L)
	if err != nil {
		return err
	}
	factoryBiasZ, err = m.ReadSignedWord(reg.MPU9250_ZA_OFFSET_H, reg.MPU9250_ZA_OFFSET_L)
	if err != nil {
		return err
	}

	// In addition, bit 0 of the lower byte must be preserved since it is used
	// for temperature compensation calculations.
	maskX := factoryBiasX & 1
	maskY := factoryBiasY & 1
	maskZ := factoryBiasZ & 1

	m.debug("Factory accelerometer bias: X:%d, Y:%d, Z:%d\n", int16(factoryBiasX), int16(factoryBiasY), int16(factoryBiasZ))

	// Accelerometer bias registers expect bias input as 2048 LSB per g, so that
	// the accelerometer biases calculated above must be divided by 8.
	factoryBiasX -= accelXBias >> 3
	factoryBiasY -= accelYBias >> 3
	factoryBiasZ -= accelZBias >> 3

	// restore the temperature preserve bit
	factoryBiasX |= maskX
	factoryBiasY |= maskY
	factoryBiasZ |= maskZ

	if err := writeAccelOffset(uint16(factoryBiasX), reg.MPU9250_XA_OFFSET_H, reg.MPU9250_XA_OFFSET_L); err != nil {
		return err
	}
	if err := writeAccelOffset(uint16(factoryBiasY), reg.MPU9250_YA_OFFSET_H, reg.MPU9250_YA_OFFSET_L); err != nil {
		return err
	}
	return writeAccelOffset(uint16(factoryBiasZ), reg.MPU9250_ZA_OFFSET_H, reg.MPU9250_ZA_OFFSET_L)
}

var selftTestSequence = [][]byte{
	{reg.MPU9250_SMPLRT_DIV, 0x00},
	{reg.MPU9250_CONFIG, 0x02},
	{reg.MPU9250_PWR_MGMT_2, 0x00},
	{reg.MPU9250_GYRO_CONFIG, 0x00},
	{reg.MPU9250_ACCEL_CONFIG2, 0x02},
	{reg.MPU9250_ACCEL_CONFIG, 0x00},
}

// SelfTest runs the self test on the device.
//
// Returns the accelerator and gyroscope deviations from the factory defaults.
func (m *MPU9250) SelfTest() (*SelfTestResult, error) {
	if err := m.transferBatch(selftTestSequence, "error initializing self-test sequence %d: [%x:%x] => %v"); err != nil {
		return nil, err
	}
	const iterations = 200
	var (
		avgAccelX, avgAccelY, avgAccelZ       int32
		avgGyroX, avgGyroY, avgGyroZ          int32
		avgSTAccelX, avgSTAccelY, avgSTAccelZ int32
		avgSTGyroX, avgSTGyroY, avgSTGyroZ    int32
		stGyroX, stGyroY, stGyroZ             byte
		stAccelX, stAccelY, stAccelZ          byte
	)

	add := func(f func() (int16, error), dest *int32) error {
		v, err := f()
		if err != nil {
			return err
		}
		*dest += int32(v)
		return nil
	}

	collectData := func(ax, ay, az, gx, gy, gz *int32) error {
		for i := 0; i < iterations; i++ {
			if err := add(m.GetAccelerationX, ax); err != nil {
				return err
			}
			if err := add(m.GetAccelerationY, ay); err != nil {
				return err
			}
			if err := add(m.GetAccelerationZ, az); err != nil {
				return err
			}
			if err := add(m.GetRotationX, gx); err != nil {
				return err
			}
			if err := add(m.GetRotationY, gy); err != nil {
				return err
			}
			if err := add(m.GetRotationZ, gz); err != nil {
				return err
			}
		}
		*ax /= iterations
		*ay /= iterations
		*az /= iterations
		*gx /= iterations
		*gy /= iterations
		*gz /= iterations
		return nil
	}

	// Collect the average measurement for the registers data
	if err := collectData(&avgAccelX, &avgAccelY, &avgAccelZ, &avgGyroX, &avgGyroY, &avgGyroZ); err != nil {
		return nil, wrapf("selftest: error reading register data %v", err)
	}

	m.debug("Avg accelerometer: X: %d, Y: %d, Z: %d\n    gyroscope:     X: %d, Y: %d, Z: %d\n", avgAccelX, avgAccelY, avgAccelZ, avgGyroX, avgGyroY, avgGyroZ)

	// Collect the self-test data.
	if err := m.transport.writeByte(reg.MPU9250_ACCEL_CONFIG, 0xe0); err != nil {
		return nil, wrapf("selftest: error setting selftest for accelerometer: %v", err)
	}
	if err := m.transport.writeByte(reg.MPU9250_GYRO_CONFIG, 0xe0); err != nil {
		return nil, wrapf("selftest: error setting selftest for gyroscope: %v", err)
	}
	if err := collectData(&avgSTAccelX, &avgSTAccelY, &avgSTAccelZ, &avgSTGyroX, &avgSTGyroY, &avgSTGyroZ); err != nil {
		return nil, wrapf("selftest: error reading self-test register data %v", err)
	}
	m.debug("Avg Trim accelerometer : X: %d, Y: %d, Z: %d\n         gyroscope:      X: %d, Y: %d, Z: %d\n", avgSTAccelX, avgSTAccelY, avgSTAccelZ, avgSTGyroX, avgSTGyroY, avgSTGyroZ)
	if err := m.transport.writeByte(reg.MPU9250_ACCEL_CONFIG, 0x00); err != nil {
		return nil, wrapf("selftest: error resetting accelerometer: %v", err)
	}
	if err := m.transport.writeByte(reg.MPU9250_GYRO_CONFIG, 0x00); err != nil {
		return nil, wrapf("selftest: error resetting gyroscope: %v", err)
	}
	time.Sleep(25 * time.Millisecond)

	// Get the self-test trim values.
	if stX, err := m.transport.readByte(reg.MPU9250_SELF_TEST_X_ACCEL); err == nil {
		stAccelX = stX
	} else {
		return nil, wrapf("selftest: error getting self-test accelerometer value X: %v", err)
	}
	if stY, err := m.transport.readByte(reg.MPU9250_SELF_TEST_Y_ACCEL); err == nil {
		stAccelY = stY
	} else {
		return nil, wrapf("selftest: error getting self-test accelerometer value Y: %v", err)
	}
	if stZ, err := m.transport.readByte(reg.MPU9250_SELF_TEST_Z_ACCEL); err == nil {
		stAccelZ = stZ
	} else {
		return nil, wrapf("selftest: error getting self-test accelerometer value Z: %v", err)
	}
	if gyX, err := m.transport.readByte(reg.MPU9250_SELF_TEST_X_GYRO); err == nil {
		stGyroX = gyX
	} else {
		return nil, wrapf("selftest: error getting self-test gyroscope value X: %v", err)
	}
	if gyY, err := m.transport.readByte(reg.MPU9250_SELF_TEST_Y_GYRO); err == nil {
		stGyroY = gyY
	} else {
		return nil, wrapf("selftest: error getting self-test gyroscope value Y: %v", err)
	}
	if gyZ, err := m.transport.readByte(reg.MPU9250_SELF_TEST_Z_GYRO); err == nil {
		stGyroZ = gyZ
	} else {
		return nil, wrapf("selftest: error getting self-test gyroscope value Z: %v", err)
	}

	m.debug("Self-Test accelerometer: X: %d, Y: %d, Z: %d\n          gyroscope:     X: %d, Y: %d, Z: %d\n", stAccelX, stAccelY, stAccelZ, stGyroX, stGyroY, stGyroZ)

	deviation := func(aSTAvg, aAvg int32, stV byte) float64 {
		factoryTrim := 2620.0 * (math.Pow(1.01, float64(stV)-1.0))
		return math.Abs(100.0*float64(aSTAvg-aAvg)/factoryTrim) - 100.0
	}

	return &SelfTestResult{
		AccelDeviation: Deviation{
			X: deviation(avgAccelX, avgSTAccelX, stAccelX),
			Y: deviation(avgAccelY, avgSTAccelY, stAccelY),
			Z: deviation(avgAccelZ, avgSTAccelZ, stAccelZ),
		},
		GyroDeviation: Deviation{
			X: deviation(avgGyroX, avgSTGyroX, stGyroX),
			Y: deviation(avgGyroY, avgSTGyroY, stGyroY),
			Z: deviation(avgGyroZ, avgSTGyroZ, stGyroZ),
		},
	}, nil
}

// SetClockSource sets clock source setting.
//
// An internal 8MHz oscillator, gyroscope based clock, or external sources can
// be selected as the MPU-60X0 clock source. When the internal 8 MHz oscillator
// or an external source is chosen as the clock source, the MPU-60X0 can operate
// in low power modes with the gyroscopes disabled.
//
// Upon power up, the MPU-60X0 clock source defaults to the internal oscillator.
// However, it is highly recommended that the device be configured to use one of
// the gyroscopes (or an external clock source) as the clock reference for
// improved stability. The clock source can be selected according to the
// following table:
//
//	CLK_SEL | Clock Source
//	--------+--------------------------------------
//	0       | Internal oscillator
//	1       | PLL with X Gyro reference
//	2       | PLL with Y Gyro reference
//	3       | PLL with Z Gyro reference
//	4       | PLL with external 32.768kHz reference
//	5       | PLL with external 19.2MHz reference
//	6       | Reserved
//	7       | Stops the clock and keeps the timing generator in reset
func (m *MPU9250) SetClockSource(src byte) error {
	if src > 7 {
		return wrapf("clock should be in range 0 .. 7")
	}
	return m.transport.writeMaskedReg(reg.MPU9250_PWR_MGMT_1, reg.MPU9250_CLKSEL_MASK, src)
}

// SetGyroRange sets the gyroscope range.
//
// The FS_SEL parameter allows setting the full-scale range of the gyro sensors,
// as described in the table below.
//
//	0 = +/- 250 degrees/sec
//	1 = +/- 500 degrees/sec
//	2 = +/- 1000 degrees/sec
//	3 = +/- 2000 degrees/sec
func (m *MPU9250) SetGyroRange(rangeVal byte) error {
	if rangeVal > 3 {
		return wrapf("accepted values are in the range 0 .. 3")
	}
	return m.transport.writeMaskedReg(reg.MPU9250_GYRO_CONFIG, reg.MPU9250_GYRO_FS_SEL_MASK, rangeVal<<3)
}

// GetGyroRange gets the gyroscope range.
func (m *MPU9250) GetGyroRange() (byte, error) {
	b, err := m.transport.readMaskedReg(reg.MPU9250_GYRO_CONFIG, reg.MPU9250_GYRO_FS_SEL_MASK)
	return b >> 3, err // the mask is 11000
}

// SetAccelRange sets the full-scale accelerometer range.
//
// The FS_SEL parameter allows setting the full-scale range of the accelerometer
// sensors, as described in the table below.
//
//	0 = +/- 2g
//	1 = +/- 4g
//	2 = +/- 8g
//	3 = +/- 16g
func (m *MPU9250) SetAccelRange(rangeVal byte) error {
	if (rangeVal >> 3) > 3 {
		return wrapf("accepted values are in the range 0 .. 3")
	}
	return m.transport.writeMaskedReg(reg.MPU9250_ACCEL_CONFIG, reg.MPU9250_ACCEL_FS_SEL_MASK, rangeVal)
}

// GetAccelRange gets the full-scale accelerometer range.
func (m *MPU9250) GetAccelRange() (byte, error) {
	b, err := m.transport.readMaskedReg(reg.MPU9250_ACCEL_CONFIG, reg.MPU9250_ACCEL_FS_SEL_MASK)
	return b >> 3, err
}

// GetTempFIFOEnabled gets the temperature FIFO enabled value.
func (m *MPU9250) GetTempFIFOEnabled() (bool, error) {
	response, err := m.transport.readMaskedReg(reg.MPU9250_FIFO_EN, reg.MPU9250_TEMP_FIFO_EN_MASK)
	if err != nil {
		return false, err
	}
	return response != 0, nil
}

// SetTempFIFOEnabled sets the temperature FIFO enabled value.
//
// When set, this bit enables TEMP_OUT_H and TEMP_OUT_L (Registers 65 and 66)
// to be written into the FIFO buffer.
func (m *MPU9250) SetTempFIFOEnabled(enabled bool) error {
	return m.transport.writeMaskedReg(reg.MPU9250_FIFO_EN, reg.MPU9250_TEMP_FIFO_EN_MASK, boolByte(enabled, reg.MPU9250_TEMP_FIFO_EN_MASK))
}

// GetXGyroFIFOEnabled gets the gyroscope X-axis FIFO enabled value.
func (m *MPU9250) GetXGyroFIFOEnabled() (bool, error) {
	return m.readMaskedRegBool(reg.MPU9250_FIFO_EN, reg.MPU9250_GYRO_XOUT_MASK)
}

// SetXGyroFIFOEnabled sets the gyroscope X-axis FIFO enabled value.
//
// When set, this bit enables GYRO_XOUT_H and GYRO_XOUT_L (Registers 67 and 68)
// to be written into the FIFO buffer.
func (m *MPU9250) SetXGyroFIFOEnabled(enabled bool) error {
	return m.transport.writeMaskedReg(reg.MPU9250_FIFO_EN, reg.MPU9250_GYRO_XOUT_MASK, boolByte(enabled, reg.MPU9250_GYRO_XOUT_MASK))
}

// GetYGyroFIFOEnabled gets the gyroscope Y-axis FIFO enabled value.
func (m *MPU9250) GetYGyroFIFOEnabled() (bool, error) {
	return m.readMaskedRegBool(reg.MPU9250_FIFO_EN, reg.MPU9250_GYRO_YOUT_MASK)
}

// SetYGyroFIFOEnabled sets the gyroscope Y-axis FIFO enabled value.
//
// When set, this bit enables GYRO_YOUT_H and GYRO_YOUT_L (Registers 69 and
// 70) to be written into the FIFO buffer.
func (m *MPU9250) SetYGyroFIFOEnabled(enabled bool) error {
	return m.transport.writeMaskedReg(reg.MPU9250_FIFO_EN, reg.MPU9250_GYRO_YOUT_MASK, boolByte(enabled, reg.MPU9250_GYRO_YOUT_MASK))
}

// GetZGyroFIFOEnabled gets the gyroscope Z-axis FIFO enabled value.
func (m *MPU9250) GetZGyroFIFOEnabled() (bool, error) {
	return m.readMaskedRegBool(reg.MPU9250_FIFO_EN, reg.MPU9250_GYRO_ZOUT_MASK)
}

// SetZGyroFIFOEnabled sets the gyroscope Z-axis FIFO enabled value.
//
// When set, this bit enables GYRO_ZOUT_H and GYRO_ZOUT_L (Registers 71 and 72)
// to be written into the FIFO buffer.
func (m *MPU9250) SetZGyroFIFOEnabled(enabled bool) error {
	return m.transport.writeMaskedReg(reg.MPU9250_FIFO_EN, reg.MPU9250_GYRO_ZOUT_MASK, boolByte(enabled, reg.MPU9250_GYRO_ZOUT_MASK))
}

// GetAccelFIFOEnabled gets accelerometer FIFO enabled value.
func (m *MPU9250) GetAccelFIFOEnabled() (bool, error) {
	return m.readMaskedRegBool(reg.MPU9250_FIFO_EN, reg.MPU9250_ACCEL_MASK)
}

// SetAccelFIFOEnabled sets accelerometer FIFO enabled value.
//
// When set, this bit enables ACCEL_XOUT_H, ACCEL_XOUT_L, ACCEL_YOUT_H,
// ACCEL_YOUT_L, ACCEL_ZOUT_H, and ACCEL_ZOUT_L (Registers 59 to 64) to be
// written into the FIFO buffer.
func (m *MPU9250) SetAccelFIFOEnabled(enabled bool) error {
	return m.transport.writeMaskedReg(reg.MPU9250_FIFO_EN, reg.MPU9250_ACCEL_MASK, boolByte(enabled, reg.MPU9250_ACCEL_MASK))
}

// GetSlave2FIFOEnabled gets Slave 2 FIFO enabled value.
func (m *MPU9250) GetSlave2FIFOEnabled() (bool, error) {
	return m.readMaskedRegBool(reg.MPU9250_FIFO_EN, reg.MPU9250_SLV2_MASK)
}

// SetSlave2FIFOEnabled sets Slave 2 FIFO enabled value.
//
// When set, this bit enables EXT_SENS_DATA registers (Registers 73 to 96)
// associated with Slave 2 to be written into the FIFO buffer.
func (m *MPU9250) SetSlave2FIFOEnabled(enabled bool) error {
	return m.transport.writeMaskedReg(reg.MPU9250_FIFO_EN, reg.MPU9250_SLV2_MASK, boolByte(enabled, reg.MPU9250_SLV2_MASK))
}

// GetSlave1FIFOEnabled gets Slave 1 FIFO enabled value.
func (m *MPU9250) GetSlave1FIFOEnabled() (bool, error) {
	return m.readMaskedRegBool(reg.MPU9250_FIFO_EN, reg.MPU9250_SLV1_MASK)
}

// SetSlave1FIFOEnabled sets Slave 1 FIFO enabled value.
//
// When set, this bit enables EXT_SENS_DATA registers (Registers 73 to 96)
// associated with Slave 1 to be written into the FIFO buffer.
func (m *MPU9250) SetSlave1FIFOEnabled(enabled bool) error {
	return m.transport.writeMaskedReg(reg.MPU9250_FIFO_EN, reg.MPU9250_SLV1_MASK, boolByte(enabled, reg.MPU9250_SLV1_MASK))
}

// GetSlave0FIFOEnabled gets Slave 0 FIFO enabled value.
func (m *MPU9250) GetSlave0FIFOEnabled() (bool, error) {
	return m.readMaskedRegBool(reg.MPU9250_FIFO_EN, reg.MPU9250_SLV0_MASK)
}

// SetSlave0FIFOEnabled sets Slave 0 FIFO enabled value.
//
// When set, this bit enables EXT_SENS_DATA registers (Registers 73 to 96)
// associated with Slave 0 to be written into the FIFO buffer.
func (m *MPU9250) SetSlave0FIFOEnabled(enabled bool) error {
	return m.transport.writeMaskedReg(reg.MPU9250_FIFO_EN, reg.MPU9250_SLV0_MASK, boolByte(enabled, reg.MPU9250_SLV0_MASK))
}

// I2C_MST_CTRL register

// GetMultiMasterEnabled gets multi-master enabled value.
func (m *MPU9250) GetMultiMasterEnabled() (bool, error) {
	return m.readMaskedRegBool(reg.MPU9250_I2C_MST_CTRL, reg.MPU9250_MULT_MST_EN_MASK)
}

// SetMultiMasterEnabled sets multi-master enabled value.
//
// Multi-master capability allows multiple I2C masters to operate on the same
// bus. In circuits where multi-master capability is required, set MULT_MST_EN
// to 1. This will increase current drawn by approximately 30uA.
//
// In circuits where multi-master capability is required, the state of the I2C
// bus must always be monitored by each separate I2C Master. Before an I2C
// Master can assume arbitration of the bus, it must first confirm that no other
// I2C Master has arbitration of the bus. When MULT_MST_EN is set to 1, the
// MPU-60X0's bus arbitration detection logic is turned on, enabling it to
// detect when the bus is available.
func (m *MPU9250) SetMultiMasterEnabled(enabled bool) error {
	return m.transport.writeMaskedReg(reg.MPU9250_I2C_MST_CTRL, reg.MPU9250_MULT_MST_EN_MASK, boolByte(enabled, reg.MPU9250_MULT_MST_EN_MASK))
}

// GetWaitForExternalSensorEnabled gets wait-for-external-sensor-data enabled
// value.
func (m *MPU9250) GetWaitForExternalSensorEnabled() (bool, error) {
	return m.readMaskedRegBool(reg.MPU9250_I2C_MST_CTRL, reg.MPU9250_WAIT_FOR_ES_MASK)
}

// SetWaitForExternalSensorEnabled enables wait-for-external-sensor-data.
//
// When the WAIT_FOR_ES bit is set to 1, the Data Ready interrupt will be
// delayed until External Sensor data from the Slave Devices are loaded into the
// EXT_SENS_DATA registers. This is used to ensure that both the internal sensor
// data (i.e. from gyro and accel) and external sensor data have been loaded to
// their respective data registers (i.e. the data is synced) when the Data Ready
// interrupt is triggered.
func (m *MPU9250) SetWaitForExternalSensorEnabled(enabled bool) error {
	return m.transport.writeMaskedReg(reg.MPU9250_I2C_MST_CTRL, reg.MPU9250_WAIT_FOR_ES_MASK, boolByte(enabled, reg.MPU9250_WAIT_FOR_ES_MASK))
}

// GetSlave3FIFOEnabled gets Slave 3 FIFO enabled value.
func (m *MPU9250) GetSlave3FIFOEnabled() (bool, error) {
	return m.readMaskedRegBool(reg.MPU9250_I2C_MST_CTRL, reg.MPU9250_SLV_3_FIFO_EN_MASK)
}

// SetSlave3FIFOEnabled sets Slave 3 FIFO enabled value.
//
// When set, this bit enables EXT_SENS_DATA registers (Registers 73 to 96)
// associated with Slave 3 to be written into the FIFO buffer.
func (m *MPU9250) SetSlave3FIFOEnabled(enabled bool) error {
	return m.transport.writeMaskedReg(reg.MPU9250_I2C_MST_CTRL, reg.MPU9250_SLV_3_FIFO_EN_MASK, boolByte(enabled, reg.MPU9250_SLV_3_FIFO_EN_MASK))
}

// GetSlaveReadWriteTransitionEnabled gets slave read/write transition enabled
// value.
func (m *MPU9250) GetSlaveReadWriteTransitionEnabled() (bool, error) {
	return m.readMaskedRegBool(reg.MPU9250_I2C_MST_CTRL, reg.MPU9250_I2C_MST_P_NSR_MASK)
}

// SetSlaveReadWriteTransitionEnabled enables slave read/write transition.
//
// The I2C_MST_P_NSR bit configures the I2C Master's transition from one slave
// read to the next slave read. If the bit equals 0, there will be a restart
// between reads. If the bit equals 1, there will be a stop followed by a start
// of the following read. When a write transaction follows a read transaction,
// the stop followed by a start of the successive write will be always used.
func (m *MPU9250) SetSlaveReadWriteTransitionEnabled(enabled bool) error {
	return m.transport.writeMaskedReg(reg.MPU9250_I2C_MST_CTRL, reg.MPU9250_I2C_MST_P_NSR_MASK, boolByte(enabled, reg.MPU9250_I2C_MST_P_NSR_MASK))
}

// GetMasterClockSpeed gets I2C master clock speed.
func (m *MPU9250) GetMasterClockSpeed() (byte, error) {
	return m.transport.readMaskedReg(reg.MPU9250_I2C_MST_CTRL, reg.MPU9250_I2C_MST_CLK_MASK)
}

// SetMasterClockSpeed sets I2C master clock speed.
//
// I2C_MST_CLK is a 4 bit unsigned value which configures a divider on the
// MPU-60X0 internal 8MHz clock. It sets the I2C master clock speed according to
// the following table:
//
//	I2C_MST_CLK | I2C Master Clock Speed | 8MHz Clock Divider
//	------------+------------------------+-------------------
//	0           | 348kHz                 | 23
//	1           | 333kHz                 | 24
//	2           | 320kHz                 | 25
//	3           | 308kHz                 | 26
//	4           | 296kHz                 | 27
//	5           | 286kHz                 | 28
//	6           | 276kHz                 | 29
//	7           | 267kHz                 | 30
//	8           | 258kHz                 | 31
//	9           | 500kHz                 | 16
//	10          | 471kHz                 | 17
//	11          | 444kHz                 | 18
//	12          | 421kHz                 | 19
//	13          | 400kHz                 | 20
//	14          | 381kHz                 | 21
//	15          | 364kHz                 | 22
func (m *MPU9250) SetMasterClockSpeed(speed byte) error {
	// TODO: Use physic.Frequency.
	if speed > 15 {
		return wrapf("speed range 0 .. 15")
	}
	return m.transport.writeMaskedReg(reg.MPU9250_I2C_MST_CTRL, reg.MPU9250_I2C_MST_CLK_MASK, speed)
}

// I2C_SLV* registers (Slave 0-3)

// GetSlaveAddress gets the I2C address of the specified slave (0-3).
func (m *MPU9250) GetSlaveAddress(num byte) (byte, error) {
	if num > 3 {
		return 0, wrapf(slaveNumberError)
	}
	return m.transport.readByte(reg.MPU9250_I2C_SLV0_ADDR + num*3)
}

// SetSlaveAddress sets the I2C address of the specified slave (0-3).
//
// Note that Bit 7 (MSB) controls read/write mode. If Bit 7 is set, it's a read
// operation, and if it is cleared, then it's a write operation. The remaining
// bits (6-0) are the 7-bit device address of the slave device.
//
// In read mode, the result of the read is placed in the lowest available
// EXT_SENS_DATA register. For further information regarding the allocation of
// read results, please refer to the EXT_SENS_DATA register description
// (Registers 73 - 96).
//
// The MPU-6050 supports a total of five slaves, but Slave 4 has unique
// characteristics, and so it has its own functions (getSlave4* and setSlave4*).
//
// I2C data transactions are performed at the Sample Rate, as defined in
// Register 25. The user is responsible for ensuring that I2C data transactions
// to and from each enabled Slave can be completed within a single period of the
// Sample Rate.
//
// The I2C slave access rate can be reduced relative to the Sample Rate. This
// reduced access rate is determined by I2C_MST_DLY (Register 52). Whether a
// slave's access rate is reduced relative to the Sample Rate is determined by
// I2C_MST_DELAY_CTRL (Register 103).
//
// The processing order for the slaves is fixed. The sequence followed for
// processing the slaves is Slave 0, Slave 1, Slave 2, Slave 3 and Slave 4. If a
// particular Slave is disabled it will be skipped.
//
// Each slave can either be accessed at the sample rate or at a reduced sample
// rate. In a case where some slaves are accessed at the Sample Rate and some
// slaves are accessed at the reduced rate, the sequence of accessing the slaves
// (Slave 0 to Slave 4) is still followed. However, the reduced rate slaves will
// be skipped if their access rate dictates that they should not be accessed
// during that particular cycle. For further information regarding the reduced
// access rate, please refer to Register 52. Whether a slave is accessed at the
// Sample Rate or at the reduced rate is determined by the Delay Enable bits in
// Register 103.
func (m *MPU9250) SetSlaveAddress(num, address byte) error {
	if num > 3 {
		return wrapf(slaveNumberError)
	}
	return m.transport.writeByte(reg.MPU9250_I2C_SLV0_ADDR+num*3, address)
}

// GetSlaveRegister gets the active internal register for the specified slave
// (0-3).
func (m *MPU9250) GetSlaveRegister(num byte) (byte, error) {
	if num > 3 {
		return 0, wrapf(slaveNumberError)
	}
	return m.transport.readByte(reg.MPU9250_I2C_SLV0_REG + num*3)
}

// SetSlaveRegister sets the active internal register for the specified slave
// (0-3).
//
// Read/write operations for this slave will be done to whatever internal
// register address is stored in this MPU register.
//
// The MPU-6050 supports a total of five slaves, but Slave 4 has unique
// characteristics, and so it has its own functions.
func (m *MPU9250) SetSlaveRegister(num, r byte) error {
	if num > 3 {
		return wrapf(slaveNumberError)
	}
	return m.transport.writeByte(reg.MPU9250_I2C_SLV0_REG+num*3, r)
}

// GetSlaveEnabled gets the enabled value for the specified slave (0-3).
func (m *MPU9250) GetSlaveEnabled(num byte) (bool, error) {
	if num > 3 {
		return false, wrapf(slaveNumberError)
	}
	return m.readMaskedRegBool(reg.MPU9250_I2C_SLV0_CTRL+num*3, reg.MPU9250_I2C_SLV0_EN_MASK)
}

// SetSlaveEnabled sets the enabled value for the specified slave (0-3).
//
// When set, this bit enables Slave 0 for data transfer operations. When
// cleared to 0, this bit disables Slave 0 from data transfer operations.
func (m *MPU9250) SetSlaveEnabled(num byte, enabled bool) error {
	if num > 3 {
		return wrapf(slaveNumberError)
	}
	return m.transport.writeMaskedReg(reg.MPU9250_I2C_SLV0_CTRL+num*3, reg.MPU9250_I2C_SLV0_EN_MASK, boolByte(enabled, reg.MPU9250_I2C_SLV0_EN_MASK))
}

// GetSlaveWordByteSwap gets the word pair byte-swapping enabled for the
// specified slave (0-3).
func (m *MPU9250) GetSlaveWordByteSwap(num byte) (bool, error) {
	if num > 3 {
		return false, wrapf(slaveNumberError)
	}
	return m.readMaskedRegBool(reg.MPU9250_I2C_SLV0_CTRL+num*3, reg.MPU9250_I2C_SLV0_BYTE_SW_MASK)
}

// SetSlaveWordByteSwap sets the word pair byte-swapping enabled for the
// specified slave (0-3).
//
// When set, this bit enables byte swapping. When byte swapping is enabled, the
// high and low bytes of a word pair are swapped. Please refer to I2C_SLV0_GRP
// for the pairing convention of the word pairs. When cleared to 0, bytes
// transferred to and from Slave 0 will be written to EXT_SENS_DATA registers
// in the order they were transferred.
func (m *MPU9250) SetSlaveWordByteSwap(num byte, enabled bool) error {
	if num > 3 {
		return wrapf(slaveNumberError)
	}
	return m.transport.writeMaskedReg(reg.MPU9250_I2C_SLV0_CTRL+num*3, reg.MPU9250_I2C_SLV0_BYTE_SW_MASK, boolByte(enabled, reg.MPU9250_I2C_SLV0_BYTE_SW_MASK))
}

// GetSlaveWriteMode gets write mode for the specified slave (0-3).
func (m *MPU9250) GetSlaveWriteMode(num byte) (bool, error) {
	if num > 3 {
		return false, wrapf(slaveNumberError)
	}
	return m.readMaskedRegBool(reg.MPU9250_I2C_SLV0_CTRL+num*3, reg.MPU9250_I2C_SLV0_REG_DIS_MASK)
}

// SetSlaveWriteMode sets write mode for the specified slave (0-3).
//
// When set, the transaction will read or write data only. When cleared to 0,
// the transaction will write a register address prior to reading or writing
// data. This should equal 0 when specifying the register address within the
// Slave device to/from which the ensuing data transaction will take place.
func (m *MPU9250) SetSlaveWriteMode(num byte, mode bool) error {
	if num > 3 {
		return wrapf(slaveNumberError)
	}
	return m.transport.writeMaskedReg(reg.MPU9250_I2C_SLV0_CTRL+num*3, reg.MPU9250_I2C_SLV0_REG_DIS_MASK, boolByte(mode, reg.MPU9250_I2C_SLV0_REG_DIS_MASK))
}

// GetSlaveWordGroupOffset gets word pair grouping order offset for the
// specified slave (0-3).
func (m *MPU9250) GetSlaveWordGroupOffset(num byte) (bool, error) {
	if num > 3 {
		return false, wrapf(slaveNumberError)
	}
	return m.readMaskedRegBool(reg.MPU9250_I2C_SLV0_CTRL+num*3, reg.MPU9250_I2C_SLV0_GRP_MASK)
}

// SetSlaveWordGroupOffset enables word pair grouping order offset for the
// specified slave (0-3).
//
// This sets specifies the grouping order of word pairs received from registers.
// When cleared to 0, bytes from register addresses 0 and 1, 2 and 3, etc (even,
// then odd register addresses) are paired to form a word. When set to 1, bytes
// from register addresses are paired 1 and 2, 3 and 4, etc. (odd, then even
// register addresses) are paired to form a word.
func (m *MPU9250) SetSlaveWordGroupOffset(num byte, enabled bool) error {
	if num > 3 {
		return wrapf(slaveNumberError)
	}
	return m.transport.writeMaskedReg(reg.MPU9250_I2C_SLV0_CTRL+num*3, reg.MPU9250_I2C_SLV0_GRP_MASK, boolByte(enabled, reg.MPU9250_I2C_SLV0_GRP_MASK))
}

// GetSlaveDataLength gets number of bytes to read for the specified slave
// (0-3).
func (m *MPU9250) GetSlaveDataLength(num byte) (byte, error) {
	if num > 3 {
		return 0, wrapf(slaveNumberError)
	}
	return m.transport.readMaskedReg(reg.MPU9250_I2C_SLV0_CTRL+num*3, reg.MPU9250_I2C_SLV0_LENG_MASK)
}

// SetSlaveDataLength sets number of bytes to read for the specified slave
// (0-3).
//
// Specifies the number of bytes transferred to and from Slave 0. Clearing this
// bit to 0 is equivalent to disabling the register by writing 0 to I2C_SLV0_EN.
func (m *MPU9250) SetSlaveDataLength(num byte, length byte) error {
	if num > 3 {
		return wrapf(slaveNumberError)
	}
	return m.transport.writeMaskedReg(reg.MPU9250_I2C_SLV0_CTRL+num*3, reg.MPU9250_I2C_SLV0_LENG_MASK, length)
}

// I2C_SLV* registers (Slave 4)

// GetSlave4Address gets the I2C address of Slave 4.
func (m *MPU9250) GetSlave4Address() (byte, error) {
	return m.transport.readByte(reg.MPU9250_I2C_SLV4_ADDR)
}

// SetSlave4Address sets the I2C address of Slave 4.
//
// Note that Bit 7 (MSB) controls read/write mode. If Bit 7 is set, it's a read
// operation, and if it is cleared, then it's a write operation. The remaining
// bits (6-0) are the 7-bit device address of the slave device.
func (m *MPU9250) SetSlave4Address(address byte) error {
	return m.transport.writeByte(reg.MPU9250_I2C_SLV4_ADDR, address)
}

// GetSlave4Register gets the active internal register for the Slave 4.
func (m *MPU9250) GetSlave4Register() (byte, error) {
	return m.transport.readByte(reg.MPU9250_I2C_SLV4_REG)
}

// SetSlave4Register sets the active internal register for Slave 4.
//
// Read/write operations for this slave will be done to whatever internal
// register address is stored in this MPU register.
func (m *MPU9250) SetSlave4Register(r byte) error {
	return m.transport.writeByte(reg.MPU9250_I2C_SLV4_REG, r)
}

// SetSlave4OutputByte sets new byte to write to Slave 4.
//
// This register stores the data to be written into the Slave 4. If I2C_SLV4_RW
// is set 1 (set to read), this register has no effect.
func (m *MPU9250) SetSlave4OutputByte(data byte) error {
	return m.transport.writeByte(reg.MPU9250_I2C_SLV4_DO, data)
}

// GetSlave4Enabled gets the enabled value for the Slave 4.
func (m *MPU9250) GetSlave4Enabled() (bool, error) {
	return m.readMaskedRegBool(reg.MPU9250_I2C_SLV4_CTRL, reg.MPU9250_I2C_SLV4_EN_MASK)
}

// SetSlave4Enabled sets the enabled value for Slave 4.
//
// When set, this bit enables Slave 4 for data transfer operations. When
// cleared to 0, this bit disables Slave 4 from data transfer operations.
func (m *MPU9250) SetSlave4Enabled(enabled bool) error {
	return m.transport.writeMaskedReg(reg.MPU9250_I2C_SLV4_CTRL, reg.MPU9250_I2C_SLV4_EN_MASK, boolByte(enabled, reg.MPU9250_I2C_SLV4_EN_MASK))
}

// GetSlave4InterruptEnabled gets the enabled value for Slave 4 transaction
// interrupts.
func (m *MPU9250) GetSlave4InterruptEnabled() (bool, error) {
	return m.readMaskedRegBool(reg.MPU9250_I2C_SLV4_CTRL, reg.MPU9250_SLV4_DONE_INT_EN_MASK)
}

// SetSlave4InterruptEnabled sets the enabled value for Slave 4 transaction
// interrupts.
//
// When set, this bit enables the generation of an interrupt signal upon
// completion of a Slave 4 transaction. When cleared to 0, this bit disables
// the generation of an interrupt signal upon completion of a Slave 4
// transaction. The interrupt status can be observed in Register 54.
func (m *MPU9250) SetSlave4InterruptEnabled(enabled bool) error {
	return m.transport.writeMaskedReg(reg.MPU9250_I2C_SLV4_CTRL, reg.MPU9250_SLV4_DONE_INT_EN_MASK, boolByte(enabled, reg.MPU9250_SLV4_DONE_INT_EN_MASK))
}

// GetSlave4WriteMode gets write mode for Slave 4.
func (m *MPU9250) GetSlave4WriteMode() (bool, error) {
	return m.readMaskedRegBool(reg.MPU9250_I2C_SLV4_CTRL, reg.MPU9250_I2C_SLV4_REG_DIS_MASK)
}

// SetSlave4WriteMode sets write mode for the Slave 4.
//
// When set, the transaction will read or write data only. When cleared to 0,
// the transaction will write a register address prior to reading or writing
// data. This should equal 0 when specifying the register address within the
// Slave device to/from which the ensuing data transaction will take place.
func (m *MPU9250) SetSlave4WriteMode(mode bool) error {
	return m.transport.writeMaskedReg(reg.MPU9250_I2C_SLV4_CTRL, reg.MPU9250_I2C_SLV4_REG_DIS_MASK, boolByte(mode, reg.MPU9250_I2C_SLV4_REG_DIS_MASK))
}

// GetSlave4MasterDelay gets Slave 4 master delay value.
func (m *MPU9250) GetSlave4MasterDelay() (byte, error) {
	return m.transport.readMaskedReg(reg.MPU9250_I2C_SLV4_CTRL, reg.MPU9250_I2C_MST_DLY_MASK)
}

// SetSlave4MasterDelay sets Slave 4 master delay value.
//
// This configures the reduced access rate of I2C slaves relative to the Sample
// Rate. When a slave's access rate is decreased relative to the Sample Rate,
// the slave is accessed every:
//
//	1 / (1 + I2C_MST_DLY) samples
//
// This base Sample Rate in turn is determined by SMPLRT_DIV (register 25) and
// DLPF_CFG (register 26). Whether a slave's access rate is reduced relative to
// the Sample Rate is determined by I2C_MST_DELAY_CTRL (register 103). For
// further information regarding the Sample Rate, please refer to register 25.
func (m *MPU9250) SetSlave4MasterDelay(delay byte) error {
	return m.transport.writeMaskedReg(reg.MPU9250_I2C_SLV4_CTRL, reg.MPU9250_I2C_MST_DLY_MASK, delay)
}

// GetSlave4InputByte gets last available byte read from Slave 4.
//
// This register stores the data read from Slave 4. This field is populated
// after a read transaction.
func (m *MPU9250) GetSlave4InputByte() (byte, error) {
	return m.transport.readByte(reg.MPU9250_I2C_SLV4_DI)
}

// I2C_MST_STATUS register

// GetPassthroughStatus gets FSYNC interrupt status.
//
// This bit reflects the status of the FSYNC interrupt from an external device
// into the MPU-60X0. This is used as a way to pass an external interrupt
// through the MPU-60X0 to the host application processor. When set to 1, this
// bit will cause an interrupt if FSYNC_INT_EN is asserted in INT_PIN_CFG
// (Register 55).
func (m *MPU9250) GetPassthroughStatus() (bool, error) {
	return m.readMaskedRegBool(reg.MPU9250_I2C_MST_STATUS, reg.MPU9250_PASS_THROUGH_MASK)
}

// GetSlave4IsDone gets Slave 4 transaction done status.
//
// Automatically sets to 1 when a Slave 4 transaction has completed. This
// triggers an interrupt if the I2C_MST_INT_EN bit in the INT_ENABLE register
// (Register 56) is asserted and if the SLV_4_DONE_INT bit is asserted in the
// I2C_SLV4_CTRL register (Register 52).
func (m *MPU9250) GetSlave4IsDone() (bool, error) {
	return m.readMaskedRegBool(reg.MPU9250_I2C_MST_STATUS, reg.MPU9250_I2C_SLV4_DONE_MASK)
}

// GetLostArbitration gets master arbitration lost status.
//
// This bit automatically sets to 1 when the I2C Master has lost arbitration of
// the auxiliary I2C bus (an error condition). This triggers an interrupt if the
// I2C_MST_INT_EN bit in the INT_ENABLE register (Register 56) is asserted.
func (m *MPU9250) GetLostArbitration() (bool, error) {
	return m.readMaskedRegBool(reg.MPU9250_I2C_MST_STATUS, reg.MPU9250_I2C_LOST_ARB_MASK)
}

// GetSlave4Nack gets Slave 4 NACK status.
//
// This bit automatically sets to 1 when the I2C Master receives a NACK in a
// transaction with Slave 4. This triggers an interrupt if the I2C_MST_INT_EN
// bit in the INT_ENABLE register (Register 56) is asserted.
func (m *MPU9250) GetSlave4Nack() (bool, error) {
	// Generalize all 4.
	return m.readMaskedRegBool(reg.MPU9250_I2C_MST_STATUS, reg.MPU9250_I2C_SLV4_NACK_MASK)
}

// GetSlave3Nack gets Slave 3 NACK status.
//
// This bit automatically sets to 1 when the I2C Master receives a NACK in a
// transaction with Slave 3. This triggers an interrupt if the I2C_MST_INT_EN
// bit in the INT_ENABLE register (Register 56) is asserted.
func (m *MPU9250) GetSlave3Nack() (bool, error) {
	return m.readMaskedRegBool(reg.MPU9250_I2C_MST_STATUS, reg.MPU9250_I2C_SLV3_NACK_MASK)
}

// GetSlave2Nack gets Slave 2 NACK status.
//
// This bit automatically sets to 1 when the I2C Master receives a NACK in a
// transaction with Slave 2. This triggers an interrupt if the I2C_MST_INT_EN
// bit in the INT_ENABLE register (Register 56) is asserted.
func (m *MPU9250) GetSlave2Nack() (bool, error) {
	return m.readMaskedRegBool(reg.MPU9250_I2C_MST_STATUS, reg.MPU9250_I2C_SLV2_NACK_MASK)
}

// GetSlave1Nack gets Slave 1 NACK status.
//
// This bit automatically sets to 1 when the I2C Master receives a NACK in a
// transaction with Slave 1. This triggers an interrupt if the I2C_MST_INT_EN
// bit in the INT_ENABLE register (Register 56) is asserted.
func (m *MPU9250) GetSlave1Nack() (bool, error) {
	return m.readMaskedRegBool(reg.MPU9250_I2C_MST_STATUS, reg.MPU9250_I2C_SLV1_NACK_MASK)
}

// GetSlave0Nack gets Slave 0 NACK status.
//
// This bit automatically sets to 1 when the I2C Master receives a NACK in a
// transaction with Slave 0. This triggers an interrupt if the I2C_MST_INT_EN
// bit in the INT_ENABLE register (Register 56) is asserted.
func (m *MPU9250) GetSlave0Nack() (bool, error) {
	return m.readMaskedRegBool(reg.MPU9250_I2C_MST_STATUS, reg.MPU9250_I2C_SLV0_NACK_MASK)
}

// INT_PIN_CFG register

// GetInterruptMode gets interrupt logic level mode.
//
// Is false for active-high, true for active-low.
func (m *MPU9250) GetInterruptMode() (bool, error) {
	return m.readMaskedRegBool(reg.MPU9250_INT_PIN_CFG, reg.MPU9250_ACTL_MASK)
}

// SetInterruptMode sets interrupt logic level mode.
//
// Is false for active-high, true for active-low.
func (m *MPU9250) SetInterruptMode(mode bool) error {
	return m.transport.writeMaskedReg(reg.MPU9250_INT_PIN_CFG, reg.MPU9250_ACTL_MASK, boolByte(mode, reg.MPU9250_ACTL_MASK))
}

// GetInterruptDrive gets interrupt drive mode.
//
// Will be set false for push-pull, true for open-drain.
func (m *MPU9250) GetInterruptDrive() (bool, error) {
	return m.readMaskedRegBool(reg.MPU9250_INT_PIN_CFG, reg.MPU9250_OPEN_MASK)
}

// SetInterruptDrive sets interrupt drive mode.
//
// New interrupt drive mode (false for push-pull, true for open-drain).
func (m *MPU9250) SetInterruptDrive(drive bool) error {
	return m.transport.writeMaskedReg(reg.MPU9250_INT_PIN_CFG, reg.MPU9250_OPEN_MASK, boolByte(drive, reg.MPU9250_OPEN_MASK))
}

// GetInterruptLatch gets interrupt latch mode.
//
// Is false for 50us-pulse, true for latch-until-int-cleared.
func (m *MPU9250) GetInterruptLatch() (bool, error) {
	return m.readMaskedRegBool(reg.MPU9250_INT_PIN_CFG, reg.MPU9250_LATCH_INT_EN_MASK)
}

// SetInterruptLatch sets interrupt latch mode.
//
// New latch mode (false is 50us-pulse, true is latch-until-int-cleared).
func (m *MPU9250) SetInterruptLatch(latch bool) error {
	return m.transport.writeMaskedReg(reg.MPU9250_INT_PIN_CFG, reg.MPU9250_LATCH_INT_EN_MASK, boolByte(latch, reg.MPU9250_LATCH_INT_EN_MASK))
}

// GetInterruptLatchClear gets the interrupt latch clear mode.
//
// Is false for status-read-only, true for any-register-read.
func (m *MPU9250) GetInterruptLatchClear() (bool, error) {
	return m.readMaskedRegBool(reg.MPU9250_INT_PIN_CFG, reg.MPU9250_INT_ANYRD_2CLEAR_MASK)
}

// SetInterruptLatchClear sets the interrupt latch clear mode.
//
// New latch clear mode (false is status-read-only, true is any-register-read).
func (m *MPU9250) SetInterruptLatchClear(clear bool) error {
	return m.transport.writeMaskedReg(reg.MPU9250_INT_PIN_CFG, reg.MPU9250_INT_ANYRD_2CLEAR_MASK, boolByte(clear, reg.MPU9250_INT_ANYRD_2CLEAR_MASK))
}

// GetFSyncInterruptLevel gets the FSYNC interrupt logic level mode.
func (m *MPU9250) GetFSyncInterruptLevel() (bool, error) {
	return m.readMaskedRegBool(reg.MPU9250_INT_PIN_CFG, reg.MPU9250_FSYNC_INT_MODE_EN_MASK)
}

// SetFSyncInterruptLevel sets the FSYNC interrupt logic level mode.
//
// New FSYNC interrupt mode (false is active-high, true is active-low).
func (m *MPU9250) SetFSyncInterruptLevel(level bool) error {
	return m.transport.writeMaskedReg(reg.MPU9250_INT_PIN_CFG, reg.MPU9250_FSYNC_INT_MODE_EN_MASK, boolByte(level, reg.MPU9250_FSYNC_INT_MODE_EN_MASK))
}

// GetFSyncInterruptEnabled gets the FSYNC pin interrupt enabled setting.
func (m *MPU9250) GetFSyncInterruptEnabled() (bool, error) {
	return m.readMaskedRegBool(reg.MPU9250_INT_ENABLE, reg.MPU9250_FSYNC_INT_EN_MASK)
}

// SetFSyncInterruptEnabled sets the FSYNC pin interrupt enabled setting.
func (m *MPU9250) SetFSyncInterruptEnabled(enabled bool) error {
	return m.transport.writeMaskedReg(reg.MPU9250_INT_ENABLE, reg.MPU9250_FSYNC_INT_EN_MASK, boolByte(enabled, reg.MPU9250_FSYNC_INT_EN_MASK))
}

// GetI2CBypassEnabled gets the I2C bypass enabled status.
func (m *MPU9250) GetI2CBypassEnabled() (bool, error) {
	return m.readMaskedRegBool(reg.MPU9250_INT_PIN_CFG, reg.MPU9250_BYPASS_EN_MASK)
}

// SetI2CBypassEnabled sets the I2C bypass enabled status.
//
// When this bit is equal to 1 and I2C_MST_EN (Register 106 bit[5]) is equal to
// 0, the host application processor will be able to directly access the
// auxiliary I2C bus of the MPU-60X0. When this bit is equal to 0, the host
// application processor will not be able to directly access the auxiliary I2C
// bus of the MPU-60X0 regardless of the state of I2C_MST_EN (Register 106
// bit[5]).
func (m *MPU9250) SetI2CBypassEnabled(enabled bool) error {
	return m.transport.writeMaskedReg(reg.MPU9250_INT_PIN_CFG, reg.MPU9250_BYPASS_EN_MASK, boolByte(enabled, reg.MPU9250_BYPASS_EN_MASK))
}

// INT_ENABLE register

// GetIntEnabled gets the full interrupt enabled status.
func (m *MPU9250) GetIntEnabled() (byte, error) {
	return m.transport.readByte(reg.MPU9250_INT_ENABLE)
}

// SetIntEnabled sets the full interrupt enabled status.
//
// Full register byte for all interrupts, for quick reading. Each bit will be
// set 0 for disabled, 1 for enabled.
func (m *MPU9250) SetIntEnabled(enabled byte) error {
	return m.transport.writeByte(reg.MPU9250_INT_ENABLE, enabled)
}

// GetIntFIFOBufferOverflowEnabled gets the FIFO Buffer Overflow interrupt
// enabled status.
func (m *MPU9250) GetIntFIFOBufferOverflowEnabled() (bool, error) {
	return m.readMaskedRegBool(reg.MPU9250_INT_ENABLE, reg.MPU9250_FIFO_OFLOW_EN_MASK)
}

// SetIntFIFOBufferOverflowEnabled sets the FIFO Buffer Overflow interrupt
// enabled status.
func (m *MPU9250) SetIntFIFOBufferOverflowEnabled(enabled bool) error {
	return m.transport.writeMaskedReg(reg.MPU9250_INT_ENABLE, reg.MPU9250_FIFO_OFLOW_EN_MASK, boolByte(enabled, reg.MPU9250_FIFO_OFLOW_EN_MASK))
}

// GetIntI2CMasterEnabled gets the I2C Master interrupt enabled status.
func (m *MPU9250) GetIntI2CMasterEnabled() (bool, error) {
	return m.readMaskedRegBool(reg.MPU9250_USER_CTRL, reg.MPU9250_I2C_MST_EN_MASK)
}

// SetIntI2CMasterEnabled sets the I2C Master interrupt enabled status.
//
// This enables any of the I2C Master interrupt sources to generate an
// interrupt.
func (m *MPU9250) SetIntI2CMasterEnabled(enabled bool) error {
	return m.transport.writeMaskedReg(reg.MPU9250_USER_CTRL, reg.MPU9250_I2C_MST_EN_MASK, boolByte(enabled, reg.MPU9250_I2C_MST_EN_MASK))
}

// INT_STATUS register

// GetIntStatus get the full set of interrupt status bits.
//
// These bits clear to 0 after the register has been read. Very useful
// for getting multiple INT statuses, since each single bit read clears
// all of them because it has to read the whole byte.
func (m *MPU9250) GetIntStatus() (byte, error) {
	return m.transport.readByte(reg.MPU9250_INT_STATUS)
}

// SIGNAL_PATH_RESET register

// ResetGyroscopePath resets the gyroscope signal path.
//
// The reset will revert the signal path analog to digital converters and
// filters to their power up configurations.
func (m *MPU9250) ResetGyroscopePath() error {
	// TODO: remove, only keep Reset().
	return m.transport.writeMaskedReg(reg.MPU9250_SIGNAL_PATH_RESET, reg.MPU9250_GYRO_RST_MASK, reg.MPU9250_GYRO_RST_MASK)
}

// ResetAccelerometerPath resets the accelerometer signal path.
//
// The reset will revert the signal path analog to digital converters and
// filters to their power up configurations.
func (m *MPU9250) ResetAccelerometerPath() error {
	// TODO: remove, only keep Reset().
	return m.transport.writeMaskedReg(reg.MPU9250_SIGNAL_PATH_RESET, reg.MPU9250_ACCEL_RST_MASK, reg.MPU9250_ACCEL_RST_MASK)
}

// ResetTemperaturePath resets the temperature sensor signal path.
//
// The reset will revert the signal path analog to digital converters and
// filters to their power up configurations.
func (m *MPU9250) ResetTemperaturePath() error {
	// TODO: remove, only keep Reset().
	return m.transport.writeMaskedReg(reg.MPU9250_SIGNAL_PATH_RESET, reg.MPU9250_TEMP_RST_MASK, reg.MPU9250_TEMP_RST_MASK)
}

// USER_CTRL register

// GetFIFOEnabled gets the FIFO enabled status.
func (m *MPU9250) GetFIFOEnabled() (bool, error) {
	// TODO: remove.
	return m.readMaskedRegBool(reg.MPU9250_USER_CTRL, reg.MPU9250_FIFO_EN_MASK)
}

// SetFIFOEnabled sets the FIFO enabled status.
//
// When this bit is set to 0, the FIFO buffer is disabled. The FIFO buffer
// cannot be written to or read from while disabled. The FIFO buffer's state
// does not change unless the MPU-60X0 is power cycled.
func (m *MPU9250) SetFIFOEnabled(enabled bool) error {
	// TODO: remove, should always be set.
	return m.transport.writeMaskedReg(reg.MPU9250_USER_CTRL, reg.MPU9250_FIFO_EN_MASK, boolByte(enabled, reg.MPU9250_FIFO_EN_MASK))
}

// GetI2CMasterModeEnabled gets the I2C Master Mode enabled status.
func (m *MPU9250) GetI2CMasterModeEnabled() (bool, error) {
	// TODO: remove.
	return m.readMaskedRegBool(reg.MPU9250_USER_CTRL, reg.MPU9250_I2C_MST_EN_MASK)
}

// SetI2CMasterModeEnabled sets the I2C Master Mode enabled status.
//
// When this mode is enabled, the MPU-60X0 acts as the I2C Master to the
// external sensor slave devices on the auxiliary I2C bus. When this bit is
// cleared to 0, the auxiliary I2C bus lines (AUX_DA and AUX_CL) are logically
// driven by the primary I2C bus (SDA and SCL). This is a precondition to
// enabling Bypass Mode. For further information regarding Bypass Mode, please
// refer to Register 55.
func (m *MPU9250) SetI2CMasterModeEnabled(enabled bool) error {
	// TODO: Should be an argument to New().
	return m.transport.writeMaskedReg(reg.MPU9250_USER_CTRL, reg.MPU9250_I2C_MST_EN_MASK, boolByte(enabled, reg.MPU9250_I2C_MST_EN_MASK))
}

// SwitchSPIEnabled switches from I2C to SPI mode (MPU-6000 only)
//
// If this is set, the primary SPI interface will be enabled in place of the
// disabled primary I2C interface.
func (m *MPU9250) SwitchSPIEnabled(enabled bool) error {
	// TODO: Should be an argument to New().
	return m.transport.writeMaskedReg(reg.MPU9250_USER_CTRL, reg.MPU9250_I2C_IF_DIS_MASK, boolByte(enabled, reg.MPU9250_I2C_IF_DIS_MASK))
}

// ResetFIFO resets the FIFO.
//
// This bit resets the FIFO buffer when set to 1 while FIFO_EN equals 0. This
// bit automatically clears to 0 after the reset has been triggered.
func (m *MPU9250) ResetFIFO() error {
	// TODO: Only keep Reset() ?
	return m.transport.writeMaskedReg(reg.MPU9250_USER_CTRL, reg.MPU9250_FIFO_RST_MASK, reg.MPU9250_FIFO_RST_MASK)
}

// ResetI2CMaster resets the I2C Master.
//
// This bit resets the I2C Master when set to 1 while I2C_MST_EN equals 0.
// This bit automatically clears to 0 after the reset has been triggered.
func (m *MPU9250) ResetI2CMaster() error {
	// TODO: Only keep Reset() ?
	return m.transport.writeMaskedReg(reg.MPU9250_USER_CTRL, reg.MPU9250_I2C_MST_RST_MASK, reg.MPU9250_I2C_MST_RST_MASK)
}

// ResetSensors resets all sensor registers and signal paths.
//
// When set to 1, this bit resets the signal paths for all sensors (gyroscopes,
// accelerometers, and temperature sensor). This operation will also clear the
// sensor registers. This bit automatically clears to 0 after the reset has been
// triggered.
//
// When resetting only the signal path (and not the sensor registers), please
// use Register 104, SIGNAL_PATH_RESET.
func (m *MPU9250) ResetSensors() error {
	// TODO: Only keep Reset() ?
	return m.transport.writeMaskedReg(reg.MPU9250_USER_CTRL, reg.MPU9250_SIG_COND_RST_MASK, reg.MPU9250_SIG_COND_RST_MASK)
}

// Reset triggers a full device reset.
//
// A small delay of ~50ms may be desirable after triggering a reset.
func (m *MPU9250) Reset() error {
	// TODO: the driver should handle the delay transparently.
	return m.transport.writeMaskedReg(reg.MPU9250_PWR_MGMT_1, reg.MPU9250_H_RESET_MASK, reg.MPU9250_H_RESET_MASK)
}

// GetSleepEnabled gets the sleep mode status.
func (m *MPU9250) GetSleepEnabled() (bool, error) {
	return m.readMaskedRegBool(reg.MPU9250_PWR_MGMT_1, reg.MPU9250_SLEEP_MASK)
}

// SetSleepEnabled set sleep mode status.
//
// Setting the SLEEP bit in the register puts the device into very low power
// sleep mode. In this mode, only the serial interface and internal registers
// remain active, allowing for a very low standby current. Clearing this bit
// puts the device back into normal mode. To save power, the individual standby
// selections for each of the gyros should be used if any gyro axis is not used
// by the application.
func (m *MPU9250) SetSleepEnabled(enabled bool) error {
	// TODO: rename to Sleep() or something like that.
	return m.transport.writeMaskedReg(reg.MPU9250_PWR_MGMT_1, reg.MPU9250_SLEEP_MASK, boolByte(enabled, reg.MPU9250_SLEEP_MASK))
}

// GetWakeCycleEnabled gets the wake cycle enabled status.
func (m *MPU9250) GetWakeCycleEnabled() (bool, error) {
	return m.readMaskedRegBool(reg.MPU9250_PWR_MGMT_1, reg.MPU9250_CYCLE_MASK)
}

// SetWakeCycleEnabled sets the wake cycle enabled status.
//
// When this bit is set to 1 and SLEEP is disabled, the MPU-60X0 will cycle
// between sleep mode and waking up to take a single sample of data from active
// sensors at a rate determined by LP_WAKE_CTRL (register 108).
func (m *MPU9250) SetWakeCycleEnabled(enabled bool) error {
	return m.transport.writeMaskedReg(reg.MPU9250_PWR_MGMT_1, reg.MPU9250_CYCLE_MASK, boolByte(enabled, reg.MPU9250_CYCLE_MASK))
}

// GetClockSource gets the clock source setting.
func (m *MPU9250) GetClockSource() (byte, error) {
	return m.transport.readMaskedReg(reg.MPU9250_PWR_MGMT_1, reg.MPU9250_CLKSEL_MASK)
}

// GetDeviceID returns the device ID.
func (m *MPU9250) GetDeviceID() (byte, error) {
	// TODO: cache.
	return m.transport.readByte(reg.MPU9250_WHO_AM_I)
}

// FIFO_COUNT* registers

// GetFIFOCount gets the current FIFO buffer size.
//
// This value indicates the number of bytes stored in the FIFO buffer. This
// number is in turn the number of bytes that can be read from the FIFO buffer
// and it is directly proportional to the number of samples available given the
// set of sensor data bound to be stored in the FIFO (register 35 and 36).
func (m *MPU9250) GetFIFOCount() (uint16, error) {
	// TODO: make private.
	return m.transport.readUint16(reg.MPU9250_FIFO_COUNTH, reg.MPU9250_FIFO_COUNTL)
}

// FIFO_R_W register

// GetFIFOByte Get byte from FIFO buffer.
//
// This register is used to read and write data from the FIFO buffer. Data is
// written to the FIFO in order of register number (from lowest to highest). If
// all the FIFO enable flags (see below) are enabled and all External Sensor
// Data registers (Registers 73 to 96) are associated with a Slave device, the
// contents of registers 59 through 96 will be written in order at the Sample
// Rate.
//
// The contents of the sensor data registers (Registers 59 to 96) are written
// into the FIFO buffer when their corresponding FIFO enable flags are set to 1
// in FIFO_EN (Register 35). An additional flag for the sensor data registers
// associated with I2C Slave 3 can be found in I2C_MST_CTRL (Register 36).
//
// If the FIFO buffer has overflowed, the status bit FIFO_OFLOW_INT is
// automatically set to 1. This bit is located in INT_STATUS (Register 58).
// When the FIFO buffer has overflowed, the oldest data will be lost and new
// data will be written to the FIFO.
//
// If the FIFO buffer is empty, reading this register will return the last byte
// that was previously read from the FIFO until new data is available. The user
// should check FIFO_COUNT to ensure that the FIFO buffer is not read when
// empty.
func (m *MPU9250) GetFIFOByte() (byte, error) {
	// TODO: make private.
	return m.transport.readByte(reg.MPU9250_FIFO_R_W)
}

// SetFIFOByte writes byte to FIFO buffer.
func (m *MPU9250) SetFIFOByte(data byte) error {
	// TODO: make private.
	return m.transport.writeByte(reg.MPU9250_FIFO_R_W, data)
}

// GetXGyroOffset returns the offset of X gyroscope.
func (m *MPU9250) GetXGyroOffset() (uint16, error) {
	return m.getOffset(reg.MPU9250_XG_OFFSET_H, reg.MPU9250_XG_OFFSET_L)
}

// SetXGyroOffset sets the X gyroscope offset.
func (m *MPU9250) SetXGyroOffset(offset uint16) error {
	return m.setOffset(reg.MPU9250_XG_OFFSET_H, reg.MPU9250_XG_OFFSET_L, offset)
}

// GetYGyroOffset returns the offset of y gyroscope.
func (m *MPU9250) GetYGyroOffset() (uint16, error) {
	return m.getOffset(reg.MPU9250_YG_OFFSET_H, reg.MPU9250_YG_OFFSET_L)
}

// SetZGyroOffset sets the Z gyroscope offset.
func (m *MPU9250) SetZGyroOffset(offset uint16) error {
	return m.setOffset(reg.MPU9250_ZG_OFFSET_H, reg.MPU9250_ZG_OFFSET_L, offset)
}

// GetZGyroOffset returns the offset of Z gyroscope.
func (m *MPU9250) GetZGyroOffset() (uint16, error) {
	return m.getOffset(reg.MPU9250_ZG_OFFSET_H, reg.MPU9250_ZG_OFFSET_L)
}

// SetYGyroOffset sets the Y gyroscope offset.
func (m *MPU9250) SetYGyroOffset(offset uint16) error {
	return m.setOffset(reg.MPU9250_YG_OFFSET_H, reg.MPU9250_YG_OFFSET_L, offset)
}

// EnableAccelerometerAxis enables an accelerometer axis.
//
// axis is one of MPU9250_DISABLE_XA_MASK, MPU9250_DISABLE_YA_MASK,
// MPU9250_DISABLE_ZA_MASK.
func (m *MPU9250) EnableAccelerometerAxis(axis byte) error {
	return m.transport.writeMaskedReg(reg.MPU9250_PWR_MGMT_2, axis, 0)
}

// DisableAccelerometerAxis disables an accelerometer axis.
//
// axis is one of MPU9250_DISABLE_XA_MASK, MPU9250_DISABLE_YA_MASK,
// MPU9250_DISABLE_ZA_MASK.
func (m *MPU9250) DisableAccelerometerAxis(axis byte) error {
	// TODO: Remove.
	return m.transport.writeMaskedReg(reg.MPU9250_PWR_MGMT_2, axis, axis)
}

// EnableAccelerometer enables the accelerometer.
func (m *MPU9250) EnableAccelerometer() error {
	// TODO: Remove, always enable.
	return m.transport.writeMaskedReg(reg.MPU9250_PWR_MGMT_2, reg.MPU9250_DISABLE_XYZA_MASK, 0)
}

// GetAccelerometerTestData gets the self-test data values from the registers.
func (m *MPU9250) GetAccelerometerTestData() (*AccelerometerData, error) {
	// TODO: make private.
	x, err := m.transport.readByte(reg.MPU9250_SELF_TEST_X_ACCEL)
	if err != nil {
		return nil, err
	}
	y, err := m.transport.readByte(reg.MPU9250_SELF_TEST_Y_ACCEL)
	if err != nil {
		return nil, err
	}
	z, err := m.transport.readByte(reg.MPU9250_SELF_TEST_Z_ACCEL)
	if err != nil {
		return nil, err
	}
	return &AccelerometerData{X: int16(x), Y: int16(y), Z: int16(z)}, nil
}

// AccelerometerXIsEnabled gets the X accelerometer status.
func (m *MPU9250) AccelerometerXIsEnabled() (bool, error) {
	return negateBool(m.readMaskedRegBool(reg.MPU9250_PWR_MGMT_2, reg.MPU9250_DISABLE_XA_MASK))
}

// AccelerometerYIsEnabled gets the X accelerometer status.
func (m *MPU9250) AccelerometerYIsEnabled() (bool, error) {
	return negateBool(m.readMaskedRegBool(reg.MPU9250_PWR_MGMT_2, reg.MPU9250_DISABLE_YA_MASK))
}

// AccelerometerZIsEnabled gets the Z accelerometer status.
func (m *MPU9250) AccelerometerZIsEnabled() (bool, error) {
	return negateBool(m.readMaskedRegBool(reg.MPU9250_PWR_MGMT_2, reg.MPU9250_DISABLE_ZA_MASK))
}

// GetAccelerationX reads the X-axis accelerometer.
func (m *MPU9250) GetAccelerationX() (int16, error) {
	if enabled, err := m.AccelerometerXIsEnabled(); err == nil && enabled {
		return uintToInt(m.ReadWord(reg.MPU9250_ACCEL_XOUT_H, reg.MPU9250_ACCEL_XOUT_L))
	} else if !enabled {
		return 0, wrapf("X acceleration disabled")
	} else {
		return 0, err
	}
}

// GetAccelerationY reads the Y-axis accelerometer.
func (m *MPU9250) GetAccelerationY() (int16, error) {
	if enabled, err := m.AccelerometerYIsEnabled(); err == nil && enabled {
		return uintToInt(m.ReadWord(reg.MPU9250_ACCEL_YOUT_H, reg.MPU9250_ACCEL_YOUT_L))
	} else if !enabled {
		return 0, wrapf("Y acceleration disabled")
	} else {
		return 0, err
	}
}

// GetAccelerationZ reads the Z-axis accelerometer.
func (m *MPU9250) GetAccelerationZ() (int16, error) {
	if enabled, err := m.AccelerometerZIsEnabled(); err == nil && enabled {
		return uintToInt(m.ReadWord(reg.MPU9250_ACCEL_ZOUT_H, reg.MPU9250_ACCEL_ZOUT_L))
	} else if !enabled {
		return 0, wrapf("Z acceleration disabled")
	} else {
		return 0, err
	}
}

// SetAccelerationOffsetZ sets the acceleration trim offset for Z axis.
func (m *MPU9250) SetAccelerationOffsetZ(offset uint16) error {
	return m.setOffset(reg.MPU9250_ZA_OFFSET_H, reg.MPU9250_ZA_OFFSET_L, offset)
}

// SetAccelerationOffsetY sets the acceleration trim offset for Y axis.
func (m *MPU9250) SetAccelerationOffsetY(offset uint16) error {
	return m.setOffset(reg.MPU9250_YA_OFFSET_H, reg.MPU9250_YA_OFFSET_L, offset)
}

// SetAccelerationOffsetX sets the acceleration trim offset for X axis.
func (m *MPU9250) SetAccelerationOffsetX(offset uint16) error {
	return m.setOffset(reg.MPU9250_XA_OFFSET_H, reg.MPU9250_XA_OFFSET_L, offset)
}

// GetAcceleration reads the 3-axis accelerometer.
//
// These registers store the most recent accelerometer measurements.
// Accelerometer measurements are written to these registers at the Sample Rate
// as defined in Register 25.
//
// The accelerometer measurement registers, along with the temperature
// measurement registers, gyroscope measurement registers, and external sensor
// data registers, are composed of two sets of registers: an internal register
// set and a user-facing read register set.
//
// The data within the accelerometer sensors' internal register set is always
// updated at the Sample Rate. Meanwhile, the user-facing read register set
// duplicates the internal register set's data values whenever the serial
// interface is idle. This guarantees that a burst read of sensor registers will
// read measurements from the same sampling instant. Note that if burst reads
// are not used, the user is responsible for ensuring a set of single byte reads
// correspond to a single sampling instant by checking the Data Ready interrupt.
//
// Each 16-bit accelerometer measurement has a full scale defined in ACCEL_FS
// (Register 28). For each full scale setting, the accelerometers' sensitivity
// per LSB in ACCEL_xOUT is shown in the table below:
//
//	AFS_SEL | Full Scale Range | LSB Sensitivity
//	--------+------------------+----------------
//	0       | +/- 2g           | 8192 LSB/mg
//	1       | +/- 4g           | 4096 LSB/mg
//	2       | +/- 8g           | 2048 LSB/mg
//	3       | +/- 16g          | 1024 LSB/mg
func (m *MPU9250) GetAcceleration() (*AccelerometerData, error) {
	x, err := m.GetAccelerationX()
	if err != nil {
		return nil, err
	}
	y, err := m.GetAccelerationY()
	if err != nil {
		return nil, err
	}
	z, err := m.GetAccelerationZ()
	if err != nil {
		return nil, err
	}
	return &AccelerometerData{X: x, Y: y, Z: z}, nil
}

// Gyroscope functions.

// GetGyroTestData gets the test data from self-test registers, factory
// settings.
func (m *MPU9250) GetGyroTestData() (*GyroscopeData, error) {
	x, err := m.transport.readByte(reg.MPU9250_SELF_TEST_X_GYRO)
	if err != nil {
		return nil, err
	}
	y, err := m.transport.readByte(reg.MPU9250_SELF_TEST_Y_GYRO)
	if err != nil {
		return nil, err
	}
	z, err := m.transport.readByte(reg.MPU9250_SELF_TEST_Z_GYRO)
	if err != nil {
		return nil, err
	}
	return &GyroscopeData{X: int16(x), Y: int16(y), Z: int16(z)}, nil
}

// EnableGyro enables the gyroscope.
func (m *MPU9250) EnableGyro() error {
	return m.transport.writeMaskedReg(reg.MPU9250_PWR_MGMT_2, reg.MPU9250_DISABLE_XYZG_MASK, 0)
}

// DisableGyro disables the gyroscope.
func (m *MPU9250) DisableGyro() error {
	return m.transport.writeMaskedReg(reg.MPU9250_PWR_MGMT_2, reg.MPU9250_DISABLE_XYZG_MASK, reg.MPU9250_DISABLE_XYZG_MASK)
}

// GyroXIsEnabled gets the X axis enabled flag.
func (m *MPU9250) GyroXIsEnabled() (bool, error) {
	//WARNING: this should be verified here
	return negateBool(m.readMaskedRegBool(reg.MPU9250_PWR_MGMT_2, reg.MPU9250_DISABLE_XG_MASK))
}

// GyroYIsEnabled gets the Y axis enabled flag.
func (m *MPU9250) GyroYIsEnabled() (bool, error) {
	//WARNING: this should be verified here
	return negateBool(m.readMaskedRegBool(reg.MPU9250_PWR_MGMT_2, reg.MPU9250_DISABLE_YG_MASK))
}

// GyroZIsEnabled gets the Z axis enabled flag.
func (m *MPU9250) GyroZIsEnabled() (bool, error) {
	//WARNING: this should be verified here
	return negateBool(m.readMaskedRegBool(reg.MPU9250_PWR_MGMT_2, reg.MPU9250_DISABLE_ZG_MASK))
}

// GetRotationX gets X-axis gyroscope reading.
func (m *MPU9250) GetRotationX() (int16, error) {
	if enabled, err := m.GyroXIsEnabled(); enabled && err == nil {
		return asInt16(m.ReadWord(reg.MPU9250_GYRO_XOUT_H, reg.MPU9250_GYRO_XOUT_L))
	} else if !enabled {
		return 0, wrapf("X rotation disabled")
	} else {
		return 0, err
	}
}

// GetRotationY gets Y-axis gyroscope reading.
func (m *MPU9250) GetRotationY() (int16, error) {
	if enabled, err := m.GyroYIsEnabled(); enabled && err == nil {
		return asInt16(m.ReadWord(reg.MPU9250_GYRO_YOUT_H, reg.MPU9250_GYRO_YOUT_L))
	} else if !enabled {
		return 0, wrapf("Y rotation disabled")
	} else {
		return 0, err
	}
}

// GetRotationZ gets Z-axis gyroscope reading.
func (m *MPU9250) GetRotationZ() (int16, error) {
	if enabled, err := m.GyroZIsEnabled(); enabled && err == nil {
		return asInt16(m.ReadWord(reg.MPU9250_GYRO_ZOUT_H, reg.MPU9250_GYRO_ZOUT_L))
	} else if !enabled {
		return 0, wrapf("Z rotation disabled")
	} else {
		return 0, err
	}
}

// GetRotation reads the 3-axis gyroscope.
//
// These gyroscope measurement registers, along with the accelerometer
// measurement registers, temperature measurement registers, and external sensor
// data registers, are composed of two sets of registers: an internal register
// set and a user-facing read register set.
// The data within the gyroscope sensors' internal register set is always
// updated at the Sample Rate. Meanwhile, the user-facing read register set
// duplicates the internal register set's data values whenever the serial
// interface is idle. This guarantees that a burst read of sensor registers will
// read measurements from the same sampling instant. Note that if burst reads
// are not used, the user is responsible for ensuring a set of single byte reads
// correspond to a single sampling instant by checking the Data Ready interrupt.
//
// Each 16-bit gyroscope measurement has a full scale defined in FS_SEL
// (Register 27). For each full scale setting, the gyroscopes' sensitivity per
// LSB in GYRO_xOUT is shown in the table below:
//
//	FS_SEL | Full Scale Range   | LSB Sensitivity
//	-------+--------------------+----------------
//	0      | +/- 250 degrees/s  | 131 LSB/deg/s
//	1      | +/- 500 degrees/s  | 65.5 LSB/deg/s
//	2      | +/- 1000 degrees/s | 32.8 LSB/deg/s
//	3      | +/- 2000 degrees/s | 16.4 LSB/deg/s
func (m *MPU9250) GetRotation() (*RotationData, error) {
	x, err := m.GetRotationX()
	if err != nil {
		return nil, err
	}
	y, err := m.GetRotationY()
	if err != nil {
		return nil, err
	}
	z, err := m.GetRotationZ()
	if err != nil {
		return nil, err
	}
	return &RotationData{X: x, Y: y, Z: z}, nil
}

// GetMotion6 gets the motion data - accelerometer and rotation(gyroscope).
func (m *MPU9250) GetMotion6() (*AccelerometerData, *RotationData, error) {
	acc, err := m.GetAcceleration()
	if err != nil {
		return nil, nil, err
	}
	rot, err := m.GetRotation()
	if err != nil {
		return nil, nil, err
	}
	return acc, rot, nil
}

// Temperature functions.

// EnableTemperature enables internal temperature sensor.
func (m *MPU9250) EnableTemperature() error {
	// TODO: Remove, always enable.
	return m.transport.writeMaskedReg(reg.MPU9250_FIFO_EN, reg.MPU9250_TEMP_FIFO_EN_MASK, boolByte(true, reg.MPU9250_TEMP_FIFO_EN_MASK))
}

// DisableTemperature disables the internal temperature sensor.
func (m *MPU9250) DisableTemperature() error {
	// TODO: Remove.
	return m.transport.writeMaskedReg(reg.MPU9250_FIFO_EN, reg.MPU9250_TEMP_FIFO_EN_MASK, boolByte(false, reg.MPU9250_TEMP_FIFO_EN_MASK))
}

// TemperatureIsEnabled returns if the temperature sensor enabled.
func (m *MPU9250) TemperatureIsEnabled() (bool, error) {
	// TODO: Remove.
	return m.readMaskedRegBool(reg.MPU9250_FIFO_EN, reg.MPU9250_TEMP_FIFO_EN_MASK)
}

// GetTemperature gets the current temperature.
func (m *MPU9250) GetTemperature() (uint16, error) {
	// TODO: Use physic.Kelvin.
	//((Temp_out - room_temp_offset)/temp_sensitivity) + 21; //celcius
	if enabled, err := m.TemperatureIsEnabled(); enabled && err != nil {
		return 0, err
	}
	return m.ReadWord(reg.MPU9250_TEMP_OUT_H, reg.MPU9250_TEMP_OUT_L)
}

// WriteByteAddress writes to the byte address.
func (m *MPU9250) WriteByteAddress(address, value byte) error {
	// TODO: make private.
	return m.transport.writeByte(address, value)
}

// ReadWord reads unsigned int from the provided addresses.
//
//	hi high 8 bit address
//	lo low 8 bit address
func (m *MPU9250) ReadWord(hi, lo byte) (uint16, error) {
	// TODO: make private.
	hiByte, err := m.transport.readByte(hi)
	if err != nil {
		return 0, wrapf("can't read word %x(hi)=> %v", hi, lo, err)
	}
	loByte, err := m.transport.readByte(lo)
	if err != nil {
		return 0, wrapf("can't read word %x(lo)=> %v", hi, lo, err)
	}
	return uint16(hiByte)<<8 | uint16(loByte), nil
}

// ReadSignedWord reads signed word from the provided high/low registers
// addresses.
func (m *MPU9250) ReadSignedWord(hi, lo byte) (int16, error) {
	res, err := m.ReadWord(hi, lo)
	return int16(res), err
}

//

func (m *MPU9250) getOffset(regs ...byte) (uint16, error) {
	return m.transport.readUint16(regs...)
}

func (m *MPU9250) setOffset(regHi, regLo byte, offset uint16) error {
	msbOffset := byte(offset >> 8)
	lsbOffset := byte(offset & 0x00FF)
	if err := m.transport.writeByte(regHi, msbOffset); err != nil {
		return err
	}
	return m.transport.writeByte(regLo, lsbOffset)
}

func (m *MPU9250) readMaskedRegBool(address, mask byte) (bool, error) {
	response, err := m.transport.readMaskedReg(address, mask)
	if err != nil {
		return false, err
	}
	return response != 0, nil
}

func uintToInt(s uint16, err error) (int16, error) {
	return int16(s), err
}

func asInt16(src uint16, err error) (int16, error) {
	return int16(src), err
}

func boolByte(v bool, mask byte) byte {
	if v {
		return mask
	}
	return 0
}

func negateBool(src bool, err error) (bool, error) {
	if err != nil {
		return src, err
	}
	return !src, err
}

func (m *MPU9250) transferBatch(seq [][]byte, msg string) error {
	for i, cmds := range seq {
		if len(cmds) == 2 {
			if err := m.transport.writeByte(cmds[0], cmds[1]); err != nil {
				return wrapf(msg, i, cmds[0], cmds[1], err)
			}
		} else {
			time.Sleep(time.Duration(cmds[0]) * time.Millisecond)
		}
	}
	return nil
}

func wrapf(format string, a ...interface{}) error {
	return fmt.Errorf("mpu9250 "+format, a...)
}

var (
	initSequence = [][]byte{
		{reg.MPU9250_PWR_MGMT_1, 0x0}, // Clear sleep mode bit (6), enable all sensors
		{100},
		{reg.MPU9250_PWR_MGMT_2, 0x01}, // Auto select clock source to be PLL gyroscope reference if ready else
		{100},
		// Configure Gyro and Thermometer
		// Disable FSYNC and set thermometer and gyro bandwidth to 41 and 42 Hz, respectively;
		// minimum delay time for this setting is 5.9 ms, which means sensor fusion update rates cannot
		// be higher than 1 / 0.0059 = 170 Hz
		// DLPF_CFG = bits 2:0 = 011; this limits the sample rate to 1000 Hz for both
		// With the MPU9250, it is possible to get gyro sample rates of 32 kHz (!), 8 kHz, or 1 kHz
		{reg.MPU9250_CONFIG, 0x03},
		{reg.MPU9250_SMPLRT_DIV, 0x04}, // Set sample rate = gyroscope output rate/(1 + SMPLRT_DIV)
	}
	calibrateSequence = [][]byte{
		{reg.MPU9250_PWR_MGMT_1, 0x80}, // reset device
		{100},                          // sleep 100 ms
		{reg.MPU9250_PWR_MGMT_1, 1},    // get stable time source; Auto select clock source to be PLL gyroscope reference if ready else use the internal oscillator, bits 2:0 = 001
		{reg.MPU9250_PWR_MGMT_2, 0},
		{200},                         // wait 200 ms
		{reg.MPU9250_INT_ENABLE, 0},   // Disable all interrupts
		{reg.MPU9250_FIFO_EN, 0},      // Disable FIFO
		{reg.MPU9250_PWR_MGMT_1, 0},   // Turn on internal clock source
		{reg.MPU9250_I2C_MST_CTRL, 0}, // Disable I2C master
		{reg.MPU9250_USER_CTRL, 0},    // Disable FIFO and I2C master modes
		{reg.MPU9250_USER_CTRL, 0x0C}, // Reset FIFO and DMP
		{15},                          // wait 15 ms
		{reg.MPU9250_CONFIG, 0x01},    // Set low-pass filter to 188 Hz
		{reg.MPU9250_SMPLRT_DIV, 0},   // Set sample rate to 1 kHz
		{reg.MPU9250_GYRO_CONFIG, 0},  // Set gyro full-scale to 250 degrees per second, maximum sensitivity
		{reg.MPU9250_ACCEL_CONFIG, 0}, // Set accelerometer full-scale to 2 g, maximum sensitivity
		{reg.MPU9250_USER_CTRL, 0x40}, // Enable FIFO
		{reg.MPU9250_FIFO_EN, 0x78},   // Enable gyro and accelerometer sensors for FIFO  (max size 512 bytes in MPU-9150)
		{40},                          // wait 40 ms
		{reg.MPU9250_FIFO_EN, 0x00},   // Disable gyro and accelerometer sensors for FIFO
	}
)