package com.systronix.devices;

import com.ajile.drivers.i2c.I2C;
import com.ajile.drivers.i2c.I2CException;
import com.ajile.jem.peripherals.Regs;

/**
 * Device Driver for the Devantech CMPS03 compass module.
 * <br />
 * Communicates with the compass module via the I2C bus.
 * <br />
 * The default I2C connections are Port A with Pin 1 for the clock and Pin 2 for the data.
 * <br />
 * You can pick your own ports and pins using the 4-arg constructor.
 * <br />
 * You can also pass in a pre-initalized I2C object using the I2C object constructor.
 * <br />
 * <br />
 * <b>Example (I2C is connected on PORTB, clock=PIN1, data=PIN2):</b>
 * <pre>
 * compass = new CMPS03(Regs.GPIOB_BASE,1,Regs.GPIOB_BASE,2);
 * System.out.println("Compass Bearing: " + compass.getBearing());
 * </pre>
 * 
 * @see <a href="http://www.robot-electronics.co.uk">Devantech</a>
 * @author Tim White (tim@cyface.com)
 * @version 1.0 (9/03/2004)
 */
public class CMPS03 {

	/*** VARIABLES ***********************************************************/
	//Constants
	private static final int COMPASS_I2C_ADDRESS = 0xC0;
	private static final int COMPASS_I2C_ADDRESS_READ = 0xC1;
	private static final int COMPASS_BEARING_SINGLE_BYTE_REGISTER = 0x01;
	private static final int COMPASS_BEARING_WORD_REGISTER = 0x02;
	private static final int COMPASS_CALIBRATE_REGISTER = 0x0E;
	private static final int COMPASS_CALIBRATE_START_BYTE = 0xFF;
	private static final int COMPASS_CALIBRATE_STOP_BYTE = 0x00;
	private static final int COMPASS_POSITIVE_ACK = 1;
	private static final int COMPASS_NEGATIVE_ACK = 0;
	private static final boolean COMPASS_CLOCK_OUT_ACTIVE_HIGH = true;
	private static final boolean COMPASS_DATA_OUT_ACTIVE_HIGH = true;

	//Fields
	private int clockPort = Regs.GPIOA_BASE;
	private int clockPin = 1;
	private int dataPort = Regs.GPIOA_BASE;
	private int dataPin = 2;

	//I2C Driver
	private I2C i2c = null;

	/*** CONSTUCTORS *********************************************************/

	/**
	 * Default constructor.
	 * I2C interface is set to PORTA, clock=PIN1, data=PIN2
	 */
	public CMPS03() {
		//No Arg Constructor
	}

	/**
	 * Constructor that allows setting of I2C Ports/Pins.
	 * @param clockPort Port the I2C clock is connected to
	 * @param clockPin Pin the I2C clock is connected to
	 * @param dataPort Port the I2C data is connected to
	 * @param dataPin Pin the I2C data is connected to
	 * @see com.ajile.jem.peripherals.Regs#GPIOA_BASE
	 */
	public CMPS03(int clockPort, int clockPin, int dataPort, int dataPin) {
		this.clockPort = clockPort;
		this.clockPin = clockPin;
		this.dataPort = dataPort;
		this.dataPin = dataPin;
	}

	/**
	 * Constructor that takes a pre-initialized I2C object.
	 * Useful if you are using I2C elsewhere in your app.<br />
	 * You need to make sure that the Ports/Pins of the I2C object have been set before calling this.
	 * @param i2c Already-initalized I2C object to use for communication with the compass
	 * @see com.ajile.drivers.i2c.I2C
	 */
	public CMPS03(I2C i2c) {
		this.i2c = i2c;
	}

	/*** PUBLIC METHODS ******************************************************/

	/** 
	 * Returns the compass bearing as 0-3599, representing 0-359.9 degrees.
	 * This reads compass registers 2 and 3.
	 * @return Compass bearing 0-3599
	 */
	public int getBearing() throws I2CException {
		initalize(); //Init the I2C bus driver

		int compassBearingHigh = 0;
		int compassBearingLow = 0;

		//Read the compass
		i2c.start(); //Send start byte
		i2c.sendByte(COMPASS_I2C_ADDRESS);
		delay(1);
		i2c.sendByte(COMPASS_BEARING_WORD_REGISTER); //Specify Register
		delay(1);
		i2c.start(); //Send start byte
		i2c.sendByte(COMPASS_I2C_ADDRESS_READ);
		compassBearingHigh = i2c.readByte(COMPASS_POSITIVE_ACK);
		compassBearingLow = i2c.readByte(COMPASS_NEGATIVE_ACK);
		i2c.stop();
		System.out.println("High Byte: " + compassBearingHigh);
		System.out.println("Low Byte: " + compassBearingLow);
		return ((compassBearingHigh * 256) + compassBearingLow);
	}

	/**
	 * Returns the compass bearing as 0-255.
	 * This reads compass register 1.
	 * @return Compass bearing 0-255
	 */
	public int getByteBearing() throws I2CException {
		initalize(); //Init the I2C bus driver

		int compassBearing = 0;

		//Read the compass
		i2c.start(); //Send start byte
		i2c.sendByte(COMPASS_I2C_ADDRESS);
		delay(1);
		i2c.sendByte(COMPASS_BEARING_SINGLE_BYTE_REGISTER); //Specify Register
		delay(1);
		i2c.start(); //Send start byte
		i2c.sendByte(COMPASS_I2C_ADDRESS_READ);
		compassBearing = i2c.readByte(COMPASS_NEGATIVE_ACK);
		i2c.stop();
		return compassBearing;
	}

	/**
	 * Allows calibration of the compass.
	 * This puts the compass into calibrate mode.<br />
	 * You then rotate the compass in a slow 360 degree circle within 20 seconds.<br />
	 * After 20 seconds, the compass taken out of calibrate mode.
	 * @see <a href="http://www.robot-electronics.co.uk/htm/cmps_cal.shtml">Devantech Calibration Instructions</a>
	 */
	public void calibrate() throws I2CException {
		calibrateStart();
		delay(20000);
		calibrateStop();
	}

	/**
	 * Puts the compass into calibrate mode.
	 * This sends a 255 to compass register 15. <br />
	 * You need to call calibrateStop to return to normal mode.
	 * @see <a href="http://www.robot-electronics.co.uk/htm/cmps_cal.shtml">Devantech Calibration Instructions</a>
	 */
	public void calibrateStart() throws I2CException {
		i2c.start(); //Send start byte
		i2c.sendByte(COMPASS_I2C_ADDRESS);
		delay(1);
		i2c.sendByte(COMPASS_CALIBRATE_REGISTER); //Specify Register
		delay(1);
		i2c.sendByte(COMPASS_CALIBRATE_START_BYTE);
		i2c.stop();
	}

	/**
	 * Takes the compass out of calibrate mode.
	 * This sends a 0 to compass register 15.
	 * @see <a href="http://www.robot-electronics.co.uk/htm/cmps_cal.shtml">Devantech Calibration Instructions</a>
	 */
	public void calibrateStop() throws I2CException {
		i2c.start(); //Send start byte
		i2c.sendByte(COMPASS_I2C_ADDRESS);
		delay(1);
		i2c.sendByte(COMPASS_CALIBRATE_REGISTER); //Specify Register
		delay(1);
		i2c.sendByte(COMPASS_CALIBRATE_STOP_BYTE);
		i2c.stop();
	}

	/**
	 * Basic test routine - println's the single & double byte bearings once per second.
	 * @param args If specified, clockPort, clockPin, dataPort, dataPin (all as ints)
	 */
	public static void main(String[] args) {

		System.out.println("JStamp Compass Reader Startup.");

		CMPS03 compass = null;

		if (args.length == 4) {
			compass =
				new CMPS03(
					Integer.parseInt(args[0]),
					Integer.parseInt(args[1]),
					Integer.parseInt(args[2]),
					Integer.parseInt(args[3]));
		} else {
			compass = new CMPS03();
		}

		while (true) {
			delay(1000);
			try {
				System.out.println(
					"Compass Bearing (Single Byte/Word): " + compass.getByteBearing() + "/" + compass.getBearing());
			} catch (I2CException e) {
				System.out.println("I2C Exception: " + e.getMessage());
			}
		}
	}

	/*** PRIVATE METHODS *****************************************************/

	/**
	 * Initalizes the I2C bus driver by creating an I2C object, and assigning it ports and pins.
	 * Called automatically before any read operation
	 */
	private void initalize() throws I2CException {
		if (this.i2c == null) {
			this.i2c = new I2C();
		}

		this.i2c.setClockInfo(this.clockPort, this.clockPin, COMPASS_CLOCK_OUT_ACTIVE_HIGH);
		this.i2c.setDataInfo(this.dataPort, this.dataPin, COMPASS_DATA_OUT_ACTIVE_HIGH);

		this.i2c.reset();
	}

	/**
	 * Causes the current thread to sleep.
	 * 
	 * @param msecs Number of milliseconds to sleep
	 */
	private static void delay(int msecs) {
		try {
			Thread.sleep(msecs);
		} catch (Exception e) {
			e.printStackTrace();
		}
	}

	/**
	 * Gets the currently assigned clock pin.
	 * @return int Pin that that I2C clock is assigned to
	 */
	public int getClockPin() {
		return clockPin;
	}

	/**
	 * Gets the currently assigned clock port.
	 * @return int Port that that I2C clock is assigned to
	 */
	public int getClockPort() {
		return clockPort;
	}

	/**
	 * Gets the currently assigned data pin
	 * @return int Pin that that I2C data is assigned to
	 */
	public int getDataPin() {
		return dataPin;
	}

	/**
	 * Gets the currently assigned data port.
	 * @return int Pin that that I2C data is assigned to
	 */
	public int getDataPort() {
		return dataPort;
	}

	/**
	 * Sets the I2C clock pin.
	 * @param clockPin Pin to set the I2C clock to
	 */
	public void setClockPin(int clockPin) {
		this.clockPin = clockPin;
	}

	/**
	 * Sets the I2C clock port.
	 * @param clockPort Pin to set the I2C clock port to
	 * @see com.ajile.jem.peripherals.Regs#GPIOA_BASE
	 */
	public void setClockPort(int clockPort) {
		this.clockPort = clockPort;
	}

	/**
	 * Sets the I2C data pin.
	 * @param dataPin Pin to set the I2C data pin to
	 */
	public void setDataPin(int dataPin) {
		this.dataPin = dataPin;
	}

	/**
	 * Sets the I2C data port.
	 * @param dataPort Pin to set the I2C data port to
	 * @see com.ajile.jem.peripherals.Regs#GPIOA_BASE
	 */
	public void setDataPort(int dataPort) {
		this.dataPort = dataPort;
	}

}