/****************************************************************************
 *
 *   Copyright (c) 2012-2019 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

/**
 * @file qmc5883.cpp
 *
 * Driver for the QMC5883 magnetometer connected via I2C or SPI.
 */

#include <px4_config.h>
#include <px4_defines.h>

#include <drivers/device/i2c.h>

#include <sys/types.h>
#include <stdint.h>
#include <stdlib.h>
#include <stdbool.h>
#include <semaphore.h>
#include <string.h>
#include <fcntl.h>
#include <poll.h>
#include <errno.h>
#include <stdio.h>
#include <math.h>
#include <unistd.h>

#include <nuttx/arch.h>
#include <px4_work_queue/ScheduledWorkItem.hpp>
#include <nuttx/clock.h>

#include <board_config.h>

#include <perf/perf_counter.h>
#include <systemlib/err.h>

#include <drivers/drv_mag.h>
#include <drivers/drv_hrt.h>
#include <drivers/device/ringbuffer.h>
#include <drivers/drv_device.h>

#include <uORB/uORB.h>

#include <float.h>
#include <getopt.h>
#include <lib/conversion/rotation.h>

#include "qmc5883.h"

/*
 * QMC5883 internal constants and data structures.
 */

/* Max measurement rate is 200Hz */
#define QMC5883_CONVERSION_INTERVAL	(1000000 / 150)	/* microseconds */
#define QMC5883_MAX_COUNT 			32767

#define QMC5883_ADDR_DATA_OUT_X_LSB		0x00
#define QMC5883_ADDR_DATA_OUT_X_MSB		0x01
#define QMC5883_ADDR_DATA_OUT_Y_LSB		0x02
#define QMC5883_ADDR_DATA_OUT_Y_MSB		0x03
#define QMC5883_ADDR_DATA_OUT_Z_LSB		0x04
#define QMC5883_ADDR_DATA_OUT_Z_MSB		0x05
#define QMC5883_ADDR_STATUS			0x06
#define QMC5883_ADDR_TEMP_OUT_LSB 		0x07
#define QMC5883_ADDR_TEMP_OUT_MSB 		0x08
#define QMC5883_ADDR_CONTROL_1 			0x09
#define QMC5883_ADDR_CONTROL_2 			0x0A
#define QMC5883_ADDR_SET_RESET 			0x0B


#define QMC5883_STATUS_REG_DRDY 		(1 << 0)  /* Data Ready: "0": no new data, "1": new data is ready */
#define QMC5883_STATUS_REG_OVL 			(1 << 1)  /* Overflow Flag: "0": normal, "1": data overflow */
#define QMC5883_STATUS_REG_DOR			(1 << 2)  /* Data Skip: "0": normal, "1": data skipped for reading */

/* Control Register 1 */
#define QMC5883_MODE_REG_STANDBY 		(0 << 0)
#define QMC5883_MODE_REG_CONTINOUS_MODE         (1 << 0)
#define QMC5883_OUTPUT_DATA_RATE_10 		(0 << 2) /* Hz */
#define QMC5883_OUTPUT_DATA_RATE_50 		(1 << 2)
#define QMC5883_OUTPUT_DATA_RATE_100 		(2 << 2)
#define QMC5883_OUTPUT_DATA_RATE_200 		(3 << 2)
#define QMC5883_OUTPUT_RANGE_2G 		(0 << 4)  /* +/- 2 gauss */
#define QMC5883_OUTPUT_RANGE_8G 		(1 << 4)  /* +/- 8 gauss */
#define QMC5883_OVERSAMPLE_512 			(0 << 6)  /* controls digital filter bw - larger OSR -> smaller bw */
#define QMC5883_OVERSAMPLE_256 			(1 << 6)
#define QMC5883_OVERSAMPLE_128 			(2 << 6)
#define QMC5883_OVERSAMPLE_64 			(3 << 6)

/* Control Register 2 */
#define QMC5883_INT_ENB 			(1 << 0)
#define QMC5883_ROL_PNT 			(1 << 6)
#define QMC5883_SOFT_RESET 			(1 << 7)

/* Set Register */
#define QMC5883_SET_DEFAULT 			(1 << 0)

#define QMC5883_TEMP_OFFSET 50 /* deg celsius */

enum QMC5883_BUS {
	QMC5883_BUS_ALL = 0,
	QMC5883_BUS_I2C_INTERNAL,
	QMC5883_BUS_I2C_EXTERNAL,
	QMC5883_BUS_SPI
};

class QMC5883 : public device::CDev, public px4::ScheduledWorkItem
{
public:
	QMC5883(device::Device *interface, const char *path, enum Rotation rotation);
	virtual ~QMC5883();

	virtual int		init();

	virtual ssize_t		read(struct file *filp, char *buffer, size_t buflen);
	virtual int		ioctl(struct file *filp, int cmd, unsigned long arg);

	/**
	 * Stop the automatic measurement state machine.
	 */
	void			stop();

	/**
	 * Diagnostics - print some basic information about the driver.
	 */
	void			print_info();

protected:
	Device			*_interface;

private:
	unsigned		_measure_interval{0};

	ringbuffer::RingBuffer	*_reports;
	struct mag_calibration_s	_scale;
	float 			_range_scale;
	float 			_range_ga;
	bool			_collect_phase;
	int			_class_instance;
	int			_orb_class_instance;

	orb_advert_t		_mag_topic;

	perf_counter_t		_sample_perf;
	perf_counter_t		_comms_errors;
	perf_counter_t		_range_errors;
	perf_counter_t		_conf_errors;

	/* status reporting */
	bool			_sensor_ok;		/**< sensor was found and reports ok */

	enum Rotation		_rotation;

	struct mag_report	_last_report {};         /**< used for info() */

	uint8_t			_range_bits;
	uint8_t			_conf_reg;
	uint8_t			_temperature_counter;
	uint8_t			_temperature_error_count;

	/**
	 * Initialise the automatic measurement state machine and start it.
	 *
	 * @note This function is called at open and error time.  It might make sense
	 *       to make it more aggressive about resetting the bus in case of errors.
	 */
	void			start();

	/**
	 * Reset the device
	 */
	int			reset();

	/**
	 * check the sensor configuration.
	 *
	 * checks that the config of the sensor is correctly set, to
	 * cope with communication errors causing the configuration to
	 * change
	 */
	void 			check_conf(void);

	/**
	 * Perform a poll cycle; collect from the previous measurement
	 * and start a new one.
	 *
	 * This is the heart of the measurement state machine.  This function
	 * alternately starts a measurement, or collects the data from the
	 * previous measurement.
	 *
	 * When the interval between measurements is greater than the minimum
	 * measurement interval, a gap is inserted between collection
	 * and measurement to provide the most recent measurement possible
	 * at the next interval.
	 */
	void			Run() override;

	/**
	 * Write a register.
	 *
	 * @param reg		The register to write.
	 * @param val		The value to write.
	 * @return		OK on write success.
	 */
	int			write_reg(uint8_t reg, uint8_t val);

	/**
	 * Read a register.
	 *
	 * @param reg		The register to read.
	 * @param val		The value read.
	 * @return		OK on read success.
	 */
	int			read_reg(uint8_t reg, uint8_t &val);

	/**
	 * Collect the result of the most recent measurement.
	 */
	int			collect();

	/* this class has pointer data members, do not allow copying it */
	QMC5883(const QMC5883 &);
	QMC5883 operator=(const QMC5883 &);
};

/*
 * Driver 'main' command.
 */
extern "C" __EXPORT int qmc5883_main(int argc, char *argv[]);


QMC5883::QMC5883(device::Device *interface, const char *path, enum Rotation rotation) :
	CDev("QMC5883", path),
	ScheduledWorkItem(px4::device_bus_to_wq(interface->get_device_id())),
	_interface(interface),
	_reports(nullptr),
	_scale{},
	_range_scale(1.0f / 12000.0f),
	_range_ga(2.0f),
	_collect_phase(false),
	_class_instance(-1),
	_orb_class_instance(-1),
	_mag_topic(nullptr),
	_sample_perf(perf_alloc(PC_ELAPSED, "qmc5883_read")),
	_comms_errors(perf_alloc(PC_COUNT, "qmc5883_com_err")),
	_range_errors(perf_alloc(PC_COUNT, "qmc5883_rng_err")),
	_conf_errors(perf_alloc(PC_COUNT, "qmc5883_conf_err")),
	_sensor_ok(false),
	_rotation(rotation),
	_range_bits(0),
	_conf_reg(0),
	_temperature_counter(0),
	_temperature_error_count(0)
{
	// set the device type from the interface
	_device_id.devid_s.bus_type = _interface->get_device_bus_type();
	_device_id.devid_s.bus = _interface->get_device_bus();
	_device_id.devid_s.address = _interface->get_device_address();
	_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_QMC5883;

	// default scaling
	_scale.x_offset = 0;
	_scale.x_scale = 1.0f;
	_scale.y_offset = 0;
	_scale.y_scale = 1.0f;
	_scale.z_offset = 0;
	_scale.z_scale = 1.0f;
}

QMC5883::~QMC5883()
{
	/* make sure we are truly inactive */
	stop();

	if (_reports != nullptr) {
		delete _reports;
	}

	if (_class_instance != -1) {
		unregister_class_devname(MAG_BASE_DEVICE_PATH, _class_instance);
	}

	// free perf counters
	perf_free(_sample_perf);
	perf_free(_comms_errors);
	perf_free(_range_errors);
	perf_free(_conf_errors);
}

int
QMC5883::init()
{
	int ret = PX4_ERROR;

	ret = CDev::init();

	if (ret != OK) {
		DEVICE_DEBUG("CDev init failed");
		goto out;
	}

	/* allocate basic report buffers */
	_reports = new ringbuffer::RingBuffer(2, sizeof(mag_report));

	if (_reports == nullptr) {
		goto out;
	}

	/* reset the device configuration */
	reset();

	_class_instance = register_class_devname(MAG_BASE_DEVICE_PATH);

	ret = OK;
	/* sensor is ok */
	_sensor_ok = true;
out:
	return ret;
}

/**
   check that the configuration register has the right value. This is
   done periodically to cope with I2C bus noise causing the
   configuration of the compass to change.
 */
void QMC5883::check_conf(void)
{
	int ret;

	uint8_t conf_reg_in = 0;
	ret = read_reg(QMC5883_ADDR_CONTROL_1, conf_reg_in);

	if (OK != ret) {
		perf_count(_comms_errors);
		return;
	}

	if (conf_reg_in != _conf_reg) {
		perf_count(_conf_errors);
		ret = write_reg(QMC5883_ADDR_CONTROL_1, _conf_reg);

		if (OK != ret) {
			perf_count(_comms_errors);
		}
	}
}

ssize_t
QMC5883::read(struct file *filp, char *buffer, size_t buflen)
{
	unsigned count = buflen / sizeof(struct mag_report);
	struct mag_report *mag_buf = reinterpret_cast<struct mag_report *>(buffer);
	int ret = 0;

	/* buffer must be large enough */
	if (count < 1) {
		return -ENOSPC;
	}

	/* if automatic measurement is enabled */
	if (_measure_interval > 0) {
		/*
		 * While there is space in the caller's buffer, and reports, copy them.
		 * Note that we may be pre-empted by the workq thread while we are doing this;
		 * we are careful to avoid racing with them.
		 */
		while (count--) {
			if (_reports->get(mag_buf)) {
				ret += sizeof(struct mag_report);
				mag_buf++;
			}
		}

		/* if there was no data, warn the caller */
		return ret ? ret : -EAGAIN;
	}

	/* manual measurement - run one conversion */
	/* XXX really it'd be nice to lock against other readers here */
	do {
		_reports->flush();

		/* wait for it to complete */
		usleep(QMC5883_CONVERSION_INTERVAL);

		/* run the collection phase */
		if (OK != collect()) {
			ret = -EIO;
			break;
		}

		if (_reports->get(mag_buf)) {
			ret = sizeof(struct mag_report);
		}
	} while (0);

	return ret;
}

int
QMC5883::ioctl(struct file *filp, int cmd, unsigned long arg)
{
	unsigned dummy = arg;

	switch (cmd) {
	case SENSORIOCSPOLLRATE: {
			switch (arg) {

			/* zero would be bad */
			case 0:
				return -EINVAL;

			/* set default polling rate */
			case SENSOR_POLLRATE_DEFAULT: {
					/* do we need to start internal polling? */
					bool want_start = (_measure_interval == 0);

					/* set interval for next measurement to minimum legal value */
					_measure_interval = QMC5883_CONVERSION_INTERVAL;

					/* if we need to start the poll state machine, do it */
					if (want_start) {
						start();
					}

					return OK;
				}

			/* adjust to a legal polling interval in Hz */
			default: {
					/* do we need to start internal polling? */
					bool want_start = (_measure_interval == 0);

					/* convert hz to tick interval via microseconds */
					unsigned interval = (1000000 / arg);

					/* check against maximum rate */
					if (interval < QMC5883_CONVERSION_INTERVAL) {
						return -EINVAL;
					}

					/* update interval for next measurement */
					_measure_interval = interval;

					/* if we need to start the poll state machine, do it */
					if (want_start) {
						start();
					}

					return OK;
				}
			}
		}

	case SENSORIOCRESET:
		return reset();

	case MAGIOCSRANGE:
		return OK;

	case MAGIOCSSCALE:
		/* set new scale factors */
		memcpy(&_scale, (struct mag_calibration_s *)arg, sizeof(_scale));
		return 0;

	case MAGIOCGSCALE:
		/* copy out scale factors */
		memcpy((struct mag_calibration_s *)arg, &_scale, sizeof(_scale));
		return 0;

	case MAGIOCGEXTERNAL:
		DEVICE_DEBUG("MAGIOCGEXTERNAL in main driver");
		return _interface->ioctl(cmd, dummy);

	default:
		/* give it to the superclass */
		return CDev::ioctl(filp, cmd, arg);
	}
}

void
QMC5883::start()
{
	/* reset the report ring and state machine */
	_collect_phase = false;
	_reports->flush();

	/* schedule a cycle to start things */
	ScheduleNow();
}

void
QMC5883::stop()
{
	if (_measure_interval > 0) {
		/* ensure no new items are queued while we cancel this one */
		_measure_interval = 0;
		ScheduleClear();
	}
}

int
QMC5883::reset()
{
	/* read 0x00 once */
	uint8_t data_bits_in = 0;
	read_reg(QMC5883_ADDR_DATA_OUT_X_LSB, data_bits_in);

	/* software reset */
	write_reg(QMC5883_ADDR_CONTROL_2, QMC5883_SOFT_RESET);

	/* set reset period to 0x01 */
	write_reg(QMC5883_ADDR_SET_RESET, QMC5883_SET_DEFAULT);

	//use fixed range of 2G
	_range_scale = 1.0f / 12000.0f;   // 12000 LSB/Gauss at +/- 2G range
	_range_ga = 2.00f;
	_range_bits = 0x00;

	/* set control register */
	_conf_reg = QMC5883_MODE_REG_CONTINOUS_MODE |
		    QMC5883_OUTPUT_DATA_RATE_200 |
		    QMC5883_OVERSAMPLE_512 |
		    QMC5883_OUTPUT_RANGE_2G;
	write_reg(QMC5883_ADDR_CONTROL_1, _conf_reg);

	return OK;
}

void
QMC5883::Run()
{
	if (_measure_interval == 0) {
		return;
	}

	/* collection phase? */
	if (_collect_phase) {

		/* perform collection */
		if (OK != collect()) {
			DEVICE_DEBUG("collection error");
			/* restart the measurement state machine */
			start();
			return;
		}

		/* next phase is measurement */
		_collect_phase = false;

		/*
		 * Is there a collect->measure gap?
		 */
		if (_measure_interval > QMC5883_CONVERSION_INTERVAL) {
			/* schedule a fresh cycle call when we are ready to measure again */
			ScheduleDelayed(_measure_interval - QMC5883_CONVERSION_INTERVAL);

			return;
		}
	}

	/* next phase is collection */
	_collect_phase = true;

	if (_measure_interval > 0) {
		/* schedule a fresh cycle call when the measurement is done */
		ScheduleDelayed(QMC5883_CONVERSION_INTERVAL);
	}
}


int
QMC5883::collect()
{
#pragma pack(push, 1)
	struct { /* status register and data as read back from the device */
		uint8_t		x[2];
		uint8_t		y[2];
		uint8_t		z[2];
	}	qmc_report;
#pragma pack(pop)
	struct {
		int16_t		x, y, z;
	} report;

	int	ret;
	uint8_t check_counter;

	perf_begin(_sample_perf);
	struct mag_report new_report;
	bool sensor_is_onboard = false;

	float xraw_f;
	float yraw_f;
	float zraw_f;

	/* this should be fairly close to the end of the measurement, so the best approximation of the time */
	new_report.timestamp = hrt_absolute_time();
	new_report.error_count = perf_event_count(_comms_errors);
	new_report.scaling = _range_scale;
	new_report.device_id = _device_id.devid;

	/*
	 * @note  We could read the status register here, which could tell us that
	 *        we were too early and that the output registers are still being
	 *        written.  In the common case that would just slow us down, and
	 *        we're better off just never being early.
	 */

	/* get measurements from the device */
	ret = _interface->read(QMC5883_ADDR_DATA_OUT_X_LSB, (uint8_t *)&qmc_report, sizeof(qmc_report));

	if (ret != OK) {
		perf_count(_comms_errors);
		DEVICE_DEBUG("data/status read error");
		goto out;
	}

	/* map data we just received LSB, MSB */
	report.x = (((int16_t)qmc_report.x[1]) << 8) + qmc_report.x[0];
	report.y = (((int16_t)qmc_report.y[1]) << 8) + qmc_report.y[0];
	report.z = (((int16_t)qmc_report.z[1]) << 8) + qmc_report.z[0];

	/*
	 * If any of the values are -4096, there was an internal math error in the sensor.
	 * Generalise this to a simple range check that will also catch some bit errors.
	 */
	if ((abs(report.x) > QMC5883_MAX_COUNT) ||
	    (abs(report.y) > QMC5883_MAX_COUNT) ||
	    (abs(report.z) > QMC5883_MAX_COUNT)) {
		perf_count(_comms_errors);
		goto out;
	}

	/* get temperature measurements from the device */
	new_report.temperature = 0;

	if (_temperature_counter++ == 100) {
		uint8_t raw_temperature[2];

		_temperature_counter = 0;

		ret = _interface->read(QMC5883_ADDR_TEMP_OUT_LSB,
				       raw_temperature, sizeof(raw_temperature));

		if (ret == OK) {
			int16_t temp16 = (((int16_t)raw_temperature[1]) << 8) +
					 raw_temperature[0];
			new_report.temperature = QMC5883_TEMP_OFFSET + temp16 * 1.0f / 100.0f;
		}

	} else {
		new_report.temperature = _last_report.temperature;
	}


	/*
	 * RAW outputs
	 *
	 * to align the sensor axes with the board, x and y need to be flipped
	 * and y needs to be negated
	 */
	new_report.x_raw = -report.y;
	new_report.y_raw = report.x;
	/* z remains z */
	new_report.z_raw = report.z;

	/* scale values for output */

	// XXX revisit for SPI part, might require a bus type IOCTL
	unsigned dummy;
	sensor_is_onboard = !_interface->ioctl(MAGIOCGEXTERNAL, dummy);
	new_report.is_external = !sensor_is_onboard;

	if (sensor_is_onboard) {
		// convert onboard so it matches offboard for the
		// scaling below
		report.y = -report.y;
		report.x = -report.x;
	}

	/* the standard external mag by 3DR has x pointing to the
	 * right, y pointing backwards, and z down, therefore switch x
	 * and y and invert y */
	//TODO: sort out axes mapping
	xraw_f = -report.y;
	yraw_f = report.x;
	zraw_f = report.z;

	// apply user specified rotation
	rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);

	new_report.x = ((xraw_f * _range_scale) - _scale.x_offset) * _scale.x_scale;
	/* flip axes and negate value for y */
	new_report.y = ((yraw_f * _range_scale) - _scale.y_offset) * _scale.y_scale;
	/* z remains z */
	new_report.z = ((zraw_f * _range_scale) - _scale.z_offset) * _scale.z_scale;

	if (!(_pub_blocked)) {

		if (_mag_topic != nullptr) {
			/* publish it */
			orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report);

		} else {
			_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &new_report,
							 &_orb_class_instance, (sensor_is_onboard) ? ORB_PRIO_HIGH : ORB_PRIO_MAX);

			if (_mag_topic == nullptr) {
				DEVICE_DEBUG("ADVERT FAIL");
			}
		}
	}

	_last_report = new_report;

	/* post a report to the ring */
	_reports->force(&new_report);

	/* notify anyone waiting for data */
	poll_notify(POLLIN);

	/*
	  periodically check the configuration
	  registers. With a bad I2C cable it is possible for the
	  registers to become corrupt, leading to bad readings. It
	  doesn't happen often, but given the poor cables some
	  vehicles have it is worth checking for.
	 */
	check_counter = perf_event_count(_sample_perf) % 256;


	if (check_counter == 0) {
		check_conf();
	}

	ret = OK;

out:
	perf_end(_sample_perf);
	return ret;
}

int
QMC5883::write_reg(uint8_t reg, uint8_t val)
{
	uint8_t buf = val;
	return _interface->write(reg, &buf, 1);
}

int
QMC5883::read_reg(uint8_t reg, uint8_t &val)
{
	uint8_t buf = val;
	int ret = _interface->read(reg, &buf, 1);
	val = buf;
	return ret;
}

void
QMC5883::print_info()
{
	perf_print_counter(_sample_perf);
	perf_print_counter(_comms_errors);
	printf("poll interval:  %u us\n", _measure_interval);
	print_message(_last_report);
	_reports->print_info("report queue");
}

/**
 * Local functions in support of the shell command.
 */
namespace qmc5883
{

/*
  list of supported bus configurations
 */
struct qmc5883_bus_option {
	enum QMC5883_BUS busid;
	const char *devpath;
	QMC5883_constructor interface_constructor;
	uint8_t busnum;
	QMC5883	*dev;
} bus_options[] = {
	{ QMC5883_BUS_I2C_EXTERNAL, "/dev/qmc5883_ext", &QMC5883_I2C_interface, PX4_I2C_BUS_EXPANSION, NULL },
#ifdef PX4_I2C_BUS_EXPANSION1
	{ QMC5883_BUS_I2C_EXTERNAL, "/dev/qmc5883_ext1", &QMC5883_I2C_interface, PX4_I2C_BUS_EXPANSION1, NULL },
#endif
#ifdef PX4_I2C_BUS_EXPANSION2
	{ QMC5883_BUS_I2C_EXTERNAL, "/dev/qmc5883_ext2", &QMC5883_I2C_interface, PX4_I2C_BUS_EXPANSION2, NULL },
#endif
#ifdef PX4_I2C_BUS_ONBOARD
	{ QMC5883_BUS_I2C_INTERNAL, "/dev/qmc5883_int", &QMC5883_I2C_interface, PX4_I2C_BUS_ONBOARD, NULL },
#endif
#ifdef PX4_SPIDEV_HMC
	{ QMC5883_BUS_SPI, "/dev/qmc5883_spi", &QMC5883_SPI_interface, PX4_SPI_BUS_SENSORS, NULL },
#endif
};
#define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0]))

void	start(enum QMC5883_BUS busid, enum Rotation rotation);
int		stop();
bool	start_bus(struct qmc5883_bus_option &bus, enum Rotation rotation);
struct qmc5883_bus_option &find_bus(enum QMC5883_BUS busid);
void	test(enum QMC5883_BUS busid);
void	reset(enum QMC5883_BUS busid);
int	info(enum QMC5883_BUS busid);
int	temp_enable(QMC5883_BUS busid, bool enable);
void	usage();

/**
 * start driver for a specific bus option
 */
bool
start_bus(struct qmc5883_bus_option &bus, enum Rotation rotation)
{
	if (bus.dev != nullptr) {
		errx(1, "bus option already started");
	}

	device::Device *interface = bus.interface_constructor(bus.busnum);

	if (interface->init() != OK) {
		delete interface;
		warnx("no device on bus %u (type: %u)", (unsigned)bus.busnum, (unsigned)bus.busid);
		return false;
	}

	bus.dev = new QMC5883(interface, bus.devpath, rotation);

	if (bus.dev != nullptr && OK != bus.dev->init()) {
		delete bus.dev;
		bus.dev = NULL;
		return false;
	}

	int fd = open(bus.devpath, O_RDONLY);

	if (fd < 0) {
		return false;
	}

	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
		close(fd);
		errx(1, "Failed to setup poll rate");
	}

	close(fd);

	return true;
}

/**
 * Start the driver.
 *
 * This function call only returns once the driver
 * is either successfully up and running or failed to start.
 */
void
start(enum QMC5883_BUS busid, enum Rotation rotation)
{
	bool started = false;

	for (unsigned i = 0; i < NUM_BUS_OPTIONS; i++) {
		if (busid == QMC5883_BUS_ALL && bus_options[i].dev != NULL) {
			// this device is already started
			continue;
		}

		if (busid != QMC5883_BUS_ALL && bus_options[i].busid != busid) {
			// not the one that is asked for
			continue;
		}

		started |= start_bus(bus_options[i], rotation);
	}

	if (!started) {
		exit(1);
	}
}

int
stop()
{
	bool stopped = false;

	for (unsigned i = 0; i < NUM_BUS_OPTIONS; i++) {
		if (bus_options[i].dev != nullptr) {
			bus_options[i].dev->stop();
			delete bus_options[i].dev;
			bus_options[i].dev = nullptr;
			stopped = true;
		}
	}

	return !stopped;
}

/**
 * find a bus structure for a busid
 */
struct qmc5883_bus_option &find_bus(enum QMC5883_BUS busid)
{
	for (unsigned i = 0; i < NUM_BUS_OPTIONS; i++) {
		if ((busid == QMC5883_BUS_ALL ||
		     busid == bus_options[i].busid) && bus_options[i].dev != NULL) {
			return bus_options[i];
		}
	}

	errx(1, "bus %u not started", (unsigned)busid);
}


/**
 * Perform some basic functional tests on the driver;
 * make sure we can collect data from the sensor in polled
 * and automatic modes.
 */
void
test(enum QMC5883_BUS busid)
{
	struct qmc5883_bus_option &bus = find_bus(busid);
	struct mag_report report;
	ssize_t sz;
	int ret;
	const char *path = bus.devpath;

	int fd = open(path, O_RDONLY);

	if (fd < 0) {
		err(1, "%s open failed (try 'qmc5883 start')", path);
	}

	/* do a simple demand read */
	sz = read(fd, &report, sizeof(report));

	if (sz != sizeof(report)) {
		err(1, "immediate read failed");
	}

	print_message(report);

	/* check if mag is onboard or external */
	if ((ret = ioctl(fd, MAGIOCGEXTERNAL, 0)) < 0) {
		errx(1, "failed to get if mag is onboard or external");
	}

	warnx("device active: %s", ret ? "external" : "onboard");

	/* read the sensor 5x and report each value */
	for (unsigned i = 0; i < 5; i++) {
		struct pollfd fds;

		/* wait for data to be ready */
		fds.fd = fd;
		fds.events = POLLIN;
		ret = poll(&fds, 1, 2000);

		if (ret != 1) {
			errx(1, "timed out waiting for sensor data");
		}

		/* now go get it */
		sz = read(fd, &report, sizeof(report));

		if (sz != sizeof(report)) {
			err(1, "periodic read failed");
		}

		print_message(report);
	}

	PX4_INFO("PASS");
	exit(0);
}

/**
 * Reset the driver.
 */
void
reset(enum QMC5883_BUS busid)
{
	struct qmc5883_bus_option &bus = find_bus(busid);
	const char *path = bus.devpath;

	int fd = open(path, O_RDONLY);

	if (fd < 0) {
		err(1, "failed ");
	}

	if (ioctl(fd, SENSORIOCRESET, 0) < 0) {
		err(1, "driver reset failed");
	}

	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
		err(1, "driver poll restart failed");
	}

	exit(0);
}


/**
 * Print a little info about the driver.
 */
int
info(enum QMC5883_BUS busid)
{
	struct qmc5883_bus_option &bus = find_bus(busid);

	warnx("running on bus: %u (%s)\n", (unsigned)bus.busid, bus.devpath);
	bus.dev->print_info();
	exit(0);
}

void
usage()
{
	warnx("missing command: try 'start', 'info', 'test', 'reset', 'info'");
	warnx("options:");
	warnx("    -R rotation");
	warnx("    -X only external bus");
#if (PX4_I2C_BUS_ONBOARD || PX4_SPIDEV_HMC)
	warnx("    -I only internal bus");
#endif
}

} // namespace

int
qmc5883_main(int argc, char *argv[])
{
	int ch;
	enum QMC5883_BUS busid = QMC5883_BUS_ALL;
	enum Rotation rotation = ROTATION_NONE;

	if (argc < 2) {
		qmc5883::usage();
		exit(0);
	}

	while ((ch = getopt(argc, argv, "XISR:CT")) != EOF) {
		switch (ch) {
		case 'R':
			rotation = (enum Rotation)atoi(optarg);
			break;
#if (PX4_I2C_BUS_ONBOARD || PX4_SPIDEV_HMC)

		case 'I':
			busid = QMC5883_BUS_I2C_INTERNAL;
			break;
#endif

		case 'X':
			busid = QMC5883_BUS_I2C_EXTERNAL;
			break;

		case 'S':
			busid = QMC5883_BUS_SPI;
			break;

		default:
			qmc5883::usage();
			exit(0);
		}
	}

	const char *verb = argv[optind];

	/*
	 * Start/load the driver.
	 */
	if (!strcmp(verb, "start")) {
		qmc5883::start(busid, rotation);

		exit(0);
	}

	/*
	 * Stop the driver
	 */
	if (!strcmp(verb, "stop")) {
		return qmc5883::stop();
	}

	/*
	 * Test the driver/device.
	 */
	if (!strcmp(verb, "test")) {
		qmc5883::test(busid);
	}

	/*
	 * Reset the driver.
	 */
	if (!strcmp(verb, "reset")) {
		qmc5883::reset(busid);
	}


	/*
	 * Print driver information.
	 */
	if (!strcmp(verb, "info") || !strcmp(verb, "status")) {
		qmc5883::info(busid);
	}

	errx(1, "unrecognized command, try 'start', 'test', 'reset', or 'info'");
}
