/***************************************************************************
 *   Copyright (C) 30.06.2004 by                                           *
 *   Albrecht Schmidt (albrecht@ifi.lmu.de)                                *
 *   Matthias Kranz (matthias.kranz@ifi.lmu.de)                            *
 *   Paul Holleis (paul.holleis@ifi.lmu.de)                                *
 *                                                                         *
 *   header file for cmps03 compass sensor                                 *
 *   Last modified: 21.10.2004                                             *
 *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 *   This program is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
 *   GNU General Public License for more details.                          *
 *                                                                         *
 *   You should have received a copy of the GNU General Public License     *
 *   along with this program; if not, write to the                         *
 *   Free Software Foundation, Inc.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 **************************************************************************/
/******************************************************************************
 * defines
 *****************************************************************************/
#ifndef __I2C
	#include <i2c.h>
#endif

#ifndef __CMPS03
	#include <i2c.h>
#endif
#ifndef CMPS03_PWM_PIN
  /// SmartITs standard pin used for reading the cmps03 pulse width modulation (pwm) values.
  #define CMPS03_PWM_PIN PIN_A0
#endif 

#ifndef CMPS03_CALIBRATION_PIN
	/// SmartITs standard pin used for calibrating the cmps03.
  #define CMPS03_CALIBRATION_PIN PIN_A1
#endif

/// overflow at value computation
#define CMPS03_OVERFLOW 20000

/******************************************************************************
 * function signatures 
 *****************************************************************************/
// PWM
long get_angle_pwm();

// I2C
long get_angle();
BYTE get_sw_version();
BYTE get_angle_short();
float get_angle_float();

//internal
void init_i2c_compass();
BYTE read_reg1(BYTE register_number);
long read_reg2(BYTE register_number);

/******************************************************************************
 * Code
 *****************************************************************************/
/**
 * Function to read bearing.
 * \return bearing
 */ 
long read_bearing()
{
	long angle;
	angle = get_angle(); // a value between 0...3599 -> 0 ... 359.9
	angle = angle / 10; // divide and lose the 1/10
	//bearing = angle;
	return angle;
}

/**
 * Measures the direction using pulse width modulation (pwm).
 * \return a counter, a large value means a large angle between north and 
 * current heading, 0 means north.
 */
long get_angle_pwm() {
	long counter;
	counter=0;
  	// if it's high, wait for a low 
  	while(input(CMPS03_PWM_PIN) && (counter<CMPS03_OVERFLOW)) {++counter;}
  	// account for fall time
  	delay_us(1);              
  	counter=0;
  	// wait for signal go high
  	while(!input(CMPS03_PWM_PIN) && (counter<CMPS03_OVERFLOW)) {++counter;}
  	counter=0;
  	// wait for signal to go low 
  	while(input(CMPS03_PWM_PIN) && (counter<CMPS03_OVERFLOW)) {counter++;}
  	return counter;
}

/**
 * Initializes the i2c compass module cmps03.
 */
void init_i2c_compass() 
{
	output_float(I2C_SCL);
  output_float(I2C_SDA);
 // output_high(CMPS03_CALIBRATION_PIN);
}

/**
 * Used to do calibration, once for north, east, west and south. The sequence
 * is arbitrary. 
 */
void write_calibration_register()
{
	/// standard i2c bus start sequence.
	i2c_start();
	/// make a write request to address 0xC1 to signal a write to the following
	/// register.
	/// see i2c protocoll specification (cmps03 handbook).
  	i2c_write(0xC1);
  	/// write the register of interest, this time register 15.
  	i2c_write(0x0F);
  	/// write the calibration value (255)
  	i2c_write(0xFF);
	/// standard i2c bus stop sequence.
  	i2c_stop();
  	delay_ms(11);
}

/**
 * Method used for calibration. Align compass to {north|south|east|west}
 * and call the pin_calibration method. Do this for each direction.
 */
void pin_calibration()
{
	/// make sure the pin state is well defined.
	output_high(CMPS03_CALIBRATION_PIN);
	delay_ms(10);
	/// to calbrate, drive pin low and release it immediately.
	output_low(CMPS03_CALIBRATION_PIN);
	delay_ms(10);
	output_high(CMPS03_CALIBRATION_PIN);
}

/**
 * Reads one byte from register regno.
 * \param register_number register to be read.
 * \return  byte in register register_number
 */
unsigned int read_reg1(BYTE register_number)
{        
	unsigned int angle;
  BYTE datal;
  /// standard i2c bus start sequence.
  i2c_start();
 	/// make a read request to address 0xC0 to signal a read from the following
	/// register.
	/// see i2c protocoll specification (cmps03 handbook).
	i2c_write(0xC0);
	/// write the register of interest (which is to be read).
	i2c_write(register_number); // register 2 and 3
	/// standard i2c bus stop sequence.
	i2c_stop();
	delay_ms(10);
	/// standard i2c bus start sequence.
	i2c_start();
	/// make a write request to address 0xC1 to signal a write to the following
	/// register.
	/// see i2c protocoll specification (cmps03 handbook).
	i2c_write(0xC1);
	/// read the register of interest (which was written before).
  	/// a read with param 1 means "ack", a read with param 0 means no ack.
	datal=i2c_read(0); //no ack
  	/// standard i2c bus stop sequence.
  	i2c_stop();
   	angle = (unsigned int) datal;
	return(angle);
}

/**
 * Reads two byte from register regno.
 * \param register_number register to be read.
 * \return long (2 bytes), starting in register register_number
 */
long read_reg2(BYTE register_number)
{        
	long angle;
	BYTE datah,datal;

  	/// standard i2c bus start sequence.
	i2c_start();
 	/// make a read request to address 0xC0 to signal a read from the following
	/// register.
	/// see i2c protocoll specification (cmps03 handbook).
	i2c_write(0xC0);
	/// write the register of interest (which is to be read).
	i2c_write(register_number); // register 2 and 3
	/// standard i2c bus stop sequence.
	i2c_stop();
	delay_ms(10);
  	/// standard i2c bus start sequence.
	i2c_start();
	/// make a write request to address 0xC1 to signal a write to the following
	/// register.
	/// see i2c protocoll specification (cmps03 handbook).
	i2c_write(0xC1);
	/// read the register of interest (which was written before).
	/// a read with param 1 means "ack", a read with param 0 means no ack.
	datah=i2c_read(1); // ack
  	datal=i2c_read(0); // no ack
  	/// standard i2c bus stop sequence.
  	i2c_stop();
 	angle = make16(datah, datal);
	return(angle);
}

/**
 * Reads software version of copass module cmps03.
 * \return software version of compass module cmps03.
 */
BYTE get_sw_version() {
	BYTE ret;
	init_i2c_compass();
	ret = read_reg1(0);
	return (ret);
}

/**
 * Returns compass angle (compass module cmps03 register 1) as BYTE.
 * \return compass module cmps03 register 1 as BYTE.
 */
BYTE get_angle_short() {
	BYTE ret;
	init_i2c_compass();
	ret = read_reg1(1);
	return (ret);
}

/**
 * Returns compass angle (compass module cmps03 register 1) as long.
 * \return compass module cmps03 register 2 and 3 as one long.
 */
long get_angle() {
	long ret;
	init_i2c_compass();
	ret = read_reg2(2);
	return (ret);
}

/**
 * Returns compass angle (compass module cmps03 register 1) as float.
 * This method internally uses get_angle.
 * \return compass module cmps03 register 2 and 3 as one float.
 */
float get_angle_float() {
	float ret;
	long temp_angle;
	temp_angle=get_angle();
  	ret = ((float)(temp_angle))*0.1;
	return (ret);
}