/* Copyright 2012, Sebastian Reichel <sre@ring0.de>
 *
 * Permission to use, copy, modify, and/or distribute this software for any
 * purpose with or without fee is hereby granted, provided that the above
 * copyright notice and this permission notice appear in all copies.
 *
 * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
 * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
 * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
 * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
 * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
 * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
 * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
 */

public class ITG3200 : I2CDevice, Gyroscope {
	public double[] rate { get; set; }
	private int[] zero = new int[3];

	/* ITG3200 14.375 LSBs per °/sec in radians */
	private static const double scale = 14.375;

	public ITG3200(uint8 dev, uint8 addr = 0x68) throws I2CError {
			setup(dev, addr);
			devsetup();
	}

	private void devsetup() throws I2CError {
		if((get_byte(0x00) & 0x7e) != 0x68)
			log("ITG3200", LogLevelFlags.LEVEL_ERROR, @"ITG3200: invalid device id");
		else
			log("ITG3200", LogLevelFlags.LEVEL_DEBUG, @"ITG3200: correct device id");

		/* setup FS_SEL for "proper operation" */
		set_byte(0x16, 0x18);
		calibrate();
	}

	public void init(KeyFile cfg) throws KeyFileError, I2CError {
		rate = new double[3];

		var adapter = cfg.get_uint64("ITG3200", "i2c-adapter");
		var address = cfg.get_uint64("ITG3200", "i2c-address");
		setup((uint8) adapter, (uint8) address);
		devsetup();
	}

	public uint8 get_address() throws I2CError {
		return (uint8) get_byte(0x00);
	}

	public void calibrate() throws I2CError {
		uint8 samples = 10;
		zero[AXIS.X] = 0;
		zero[AXIS.Y] = 0;
		zero[AXIS.Z] = 0;

		for(int i=0; i<samples; i++) {
			zero[AXIS.X] += ((int16) get_big_word(0x1d));
			zero[AXIS.Y] += ((int16) get_big_word(0x1f));
			zero[AXIS.Z] += ((int16) get_big_word(0x21));
		}

		zero[AXIS.X] /= samples;
		zero[AXIS.Y] /= samples;
		zero[AXIS.Z] /= samples;
	}

	/* TODO: add method for register 0x16 (frequency + range) */
	/* TODO: add method for register 0x17 (interrupt config) */
	/* TODO: add method for register 0x3E (power management) */

	public uint8 get_sample_rate_divider() throws I2CError {
		/* sample rate = internal frequency / (divider + 1) */
		return (uint8) get_byte(0x15);
	}

	public void set_sample_rate_divider(uint8 val) throws I2CError {
		set_byte(0x15, val);
	}

	public int16 get_temperature() throws I2CError {
		int16 raw = (int16) get_big_word(0x1b);
		return 350 + ((raw + 13200) / 28);
	}

	public void get_data(out double x, out double y, out double z) throws I2CError {
		x = ((int16) get_big_word(0x1d)-zero[AXIS.X]) / scale;
		y = ((int16) get_big_word(0x1f)-zero[AXIS.Y]) / scale;
		z = ((int16) get_big_word(0x21)-zero[AXIS.Z]) / scale;

		rate[AXIS.X] = radians(x);
		rate[AXIS.Y] = radians(y);
		rate[AXIS.Z] = radians(z);
	}
}

public Type register_plugin(Module module) {
       // types are registered automatically
       return typeof(ITG3200);
}