/* $Id: cmps03.c,v 1.3 2007/08/02 00:50:18 bsd Exp $ */

#include <util/twi.h>

#include <inttypes.h>

#include "cmps03.h"
#include "i2c.h"

/*
 * read the data byte at the specified address from the specified device
 */
int8_t cmps03_get_byte(uint8_t device, uint8_t addr, uint8_t * value)
{
  uint8_t v;

  /* start condition */
  if (i2c_start(0x08, 1))
    return -1;

  /* address slave device, write */
  if (i2c_sla_rw(device, 0, TW_MT_SLA_ACK, 1))
    return -2;

  /* write register address */
  if (i2c_data_tx(addr, TW_MT_DATA_ACK, 1))
    return -3;

  /* repeated start condition */
  if (i2c_start(0x10, 1))
    return -4;

   /* address slave device, read */
  if (i2c_sla_rw(device, 1, TW_MR_SLA_ACK, 1))
    return -5;

  /* read data byte */
  if (i2c_data_rx(&v, I2C_NACK, TW_MR_DATA_NACK, 1))
    return -6;

  if (i2c_stop())
    return -7;

  *value = v;

  return 0;
}



/*
 * read the data byte at the specified address from the specified device
 */
int8_t cmps03_get_word(uint8_t device, uint8_t addr, uint16_t * value)
{
  uint8_t v1, v2;

  /* start condition */
  if (i2c_start(0x08, 1))
    return -1;

  /* address slave device, write */
  if (i2c_sla_rw(device, 0, TW_MT_SLA_ACK, 1))
    return -2;

  /* write register address */
  if (i2c_data_tx(addr, TW_MT_DATA_ACK, 1))
    return -3;

  /* repeated start condition */
  if (i2c_start(0x10, 1))
    return -4;

   /* address slave device, read */
  if (i2c_sla_rw(device, 1, TW_MR_SLA_ACK, 1))
    return -5;

  /* read data byte 1 */
  if (i2c_data_rx(&v1, I2C_ACK, TW_MR_DATA_ACK, 1))
    return -6;

  /* read data byte 2 */
  if (i2c_data_rx(&v2, I2C_NACK, TW_MR_DATA_NACK, 1))
    return -7;

  if (i2c_stop())
    return -8;

  *value = (v1 << 8) | v2 ;

  return 0;
}



int16_t bearing16(void)
{
  uint16_t d;
  int8_t rc;

  rc = cmps03_get_word(0x60, 2, &d);
  if (rc < 0) {
    return -1;
  }

  return d;
}

uint8_t bearing8(void)
{
  uint8_t d;

  cmps03_get_byte(0x60, 1, &d);

  return d;
}