/**************************************************************************//**
 * @brief Implementation specific functions for HRM code
 * @version 3.20.3
 ******************************************************************************
 * @section License
 * <b>(C) Copyright 2014 Silicon Labs, http://www.silabs.com</b>
 *******************************************************************************
 *
 * This file is licensed under the Silabs License Agreement. See the file
 * "Silabs_License_Agreement.txt" for details. Before using this software for
 * any purpose, you must agree to the terms of that agreement.
 *
 ******************************************************************************/
 
 
#include "accel_sys_out.h"

#include <stdio.h>
#include "em_gpio.h"
#include "i2cspm.h"
#include "em_i2c.h"
#include "string.h"
#include "sihrm_demo.h"
#include "../si117xlib/si117xhrm_static.h"
#include "../si117xdrv/si117x_functions.h"


#ifdef UART_DEBUG
    #include "uart_debug.h"
#endif

/*  I2C port configuration */
static AccelPortConfig_t _handle;
/*  interrupt queue size in bytes  */
#define	IRQ_QUEUE_SIZE	270
/*  interrupt queue data */
static u8 IrqQueue[IRQ_QUEUE_SIZE];
/*  interrupt queue current get index  */
static u16 irqQueueGetIndex = 0;
/*  interrupt queue current put index  */
static u16 irqQueuePutIndex = 0;
////
/*  Non-exported Function Prototypes  */
static s16 Accel_i2c_smbus_write_byte_data(HANDLE accel_handle, u8 address, u8 data, bool block);
static s16 Accel_i2c_smbus_read_byte_data(HANDLE accel_handle, u8 address, u8 *data, bool block);
static s16 Accel_i2c_smbus_write_i2c_block_data(HANDLE accel_handle, u8 address, u8 length, u8 const* values, bool block);
static s16 Accel_i2c_smbus_read_i2c_block_data(HANDLE accel_handle, u8 address, u8 length, u8* values, bool block);


/**************************************************************************//**
 * @brief Enables/Disables USB debug output.
 *****************************************************************************/
int AccelSetupDebug(HANDLE accel_handle, void *Accel_debug)
{
  return SI117xHRM_SUCCESS;
}

/**************************************************************************//**
 * @brief Prints USB debug output.
 *****************************************************************************/
int AccelOutputDebugMessage(HANDLE accel_handle, char *message)
{
  return SI117xHRM_SUCCESS;
}

/**************************************************************************//**
 * @brief Write to Accel register
 *****************************************************************************/
int16_t AccelWriteToRegister(HANDLE accel_handle, uint8_t address, uint8_t data, bool blocking)
{  // If "blocking" is true INT interrupt will be disabled during I2C operation
	  return Accel_i2c_smbus_write_byte_data(accel_handle, address, data, blocking);
}

/**************************************************************************//**
 * @brief Read from Accel register.
 *****************************************************************************/
int16_t AccelReadFromRegister(void *accel_handle, uint8_t address, uint8_t blocking)
{ // If "blocking" is true INT interrupt will be disabled during I2C operation
  u8 data;
  Accel_i2c_smbus_read_byte_data(accel_handle, address, &data, blocking);
  return data;
}

/**************************************************************************//**
 * @brief block write to Accel
 *****************************************************************************/
s16 AccelBlockWrite(HANDLE accel_handle,
                     u8 address, u8 length, u8 const *values)
{
  return Accel_i2c_smbus_write_i2c_block_data(accel_handle,
                                               address,
                                               length,
                                               values,
                                               true);
}

/**************************************************************************//**
 * @brief Block read from Accel.
 *****************************************************************************/
int16_t AccelBlockRead(void * accel_handle, uint8_t address, uint8_t length, uint8_t *values)
{
    return Accel_i2c_smbus_read_i2c_block_data(accel_handle,
                           address,    length,     values, true);
}

/**************************************************************************//**
 * @brief Disable GPIO interrupt for Accel interrupt.
 *****************************************************************************/
void DisableAccelInterrupt ()
{

	GPIO_IntDisable(1<<_handle.irqPin);
}

/**************************************************************************//**
 * @brief Enable GPIO interrupt for Accel.
 *****************************************************************************/
void EnableAccelInterrupt ()
{
	if (GPIO_PinInGet(_handle.irqPort, _handle.irqPin) == 0)
		GPIO_IntSet(1<<_handle.irqPin);
	GPIO_IntEnable(1<<_handle.irqPin);
}

/**************************************************************************//**
 * @brief Main interrupt processing routine for Accel.
 *****************************************************************************/
s16 AccelProcessIrq(HANDLE accel_handle, u16 timestamp)
{
  s16 error=0;
  return error;
}


/**************************************************************************//**
 * @brief Main interrupt processing routine for Accel.
 *****************************************************************************/
int16_t Accel_ReadAccelDataFromDevice(HANDLE accel_handle, int16_t *accel_x, int16_t *accel_y, int16_t *accel_z)
{
  u8 data_buffer[7];
  s16 error=0;
  Accel_i2c_smbus_read_i2c_block_data(accel_handle, 0x31, 7, data_buffer, false);		// Grab all data registers
  //Accel_i2c_smbus_write_byte_data(accel_handle, 0x21, data_buffer[0], false);      // Clear interrupts

  *accel_x = (((u16)(data_buffer[2]) << 8) & 0xff00) | data_buffer[1];            // PS1
  *accel_y = (((u16)(data_buffer[4]) << 8) & 0xff00) | data_buffer[3];            // PS2
  *accel_z = (((u16)(data_buffer[6]) << 8) & 0xff00) | data_buffer[5];            // PS3

  return error;
}

/**************************************************************************//**
 * @brief Query number of entries in the interrupt queue.
 *****************************************************************************/
s16 AccelIrqQueueNumentries(HANDLE accel_handle)
{
  (void) accel_handle;
  u16 runnerIndex = irqQueueGetIndex;
  s16 count=0;
  while (runnerIndex != irqQueuePutIndex)
  {
    runnerIndex++;
    count++;
    if(runnerIndex == IRQ_QUEUE_SIZE)
      runnerIndex = 0;
  }
  return (count/sizeof(Si117xhrmIrqSample_t));

}

/**************************************************************************//**
 * @brief Get sample from the interrupt queue.
 *****************************************************************************/
s16 AccelIrqQueue_Get(HANDLE accel_handle, Si117xhrmIrqSample_t *samples)
{
  (void) accel_handle;
  int16_t error = 0;
  uint16_t i;
  int8_t *data = (int8_t *)samples;
  DisableAccelInterrupt ();
  if (irqQueueGetIndex == irqQueuePutIndex)
    error = -1;
  else
  {
    for(i=0; i<sizeof(Si117xhrmIrqSample_t); i++)
    {
      data[i] = IrqQueue[irqQueueGetIndex];
      irqQueueGetIndex++;
      if(irqQueueGetIndex == IRQ_QUEUE_SIZE)
        irqQueueGetIndex = 0;

    }

  }
  EnableAccelInterrupt();


  return error;
}


/**************************************************************************//**
 * @brief Empty the interrupt queue.
 *****************************************************************************/
s16 AccelIrqQueue_Clear(HANDLE accel_handle)
{
  (void) accel_handle;
  irqQueueGetIndex = 0;
  irqQueuePutIndex = 0;
  return 0;
}

/**************************************************************************//**
 * @brief Initialize low level handle and clear irq queue.
 *****************************************************************************/
int16_t AccelInit(void *port, int options, void **accel_handle)
{
  s16 error = 0;
  u8 data;
  (void) options;
  *accel_handle = (HANDLE)&_handle;
  _handle.i2cAddress = ((AccelPortConfig_t*)port)->i2cAddress<<1;
  _handle.irqPort = ((AccelPortConfig_t*)port)->irqPort;
  _handle.irqPin = ((AccelPortConfig_t*)port)->irqPin;
  _handle.i2c = ((AccelPortConfig_t*)port)->i2c;

    data = AccelReadFromRegister((*accel_handle), REG_REV_ID, true);

  data = AccelReadFromRegister((*accel_handle), REG_PART_ID, true);

  if ((_handle.i2cAddress == (ACCEL_I2C_ADDR<<1)) && (data != ACCEL_ID))
    error = -1;

  AccelIrqQueue_Clear(*accel_handle);
  return error;
}

/**************************************************************************//**
 * @brief Close Accel.
 *****************************************************************************/
s16 AccelClose(HANDLE accel_handle)
{
  (void) accel_handle;
  _handle.i2cAddress = 0xff;
  return 0;
}

/**************************************************************************//**
 * @brief Reset not implemented.
 *****************************************************************************/
s16 AccelSysReset(HANDLE accel_handle)
{
  (void) accel_handle;
  return 0;
}


/**************************************************************************//**
 * @brief Write to Accel i2c.
 *****************************************************************************/
static s16 Accel_i2c_smbus_write_byte_data(HANDLE accel_handle, u8 address, u8 data, bool block)
{
  I2C_TransferSeq_TypeDef    seq;
  I2C_TransferReturn_TypeDef ret;
  uint8_t i2c_write_data[2];
  uint8_t i2c_read_data[1];
  (void) accel_handle;
  if (block)
    DisableAccelInterrupt ();
  seq.addr  = _handle.i2cAddress;
  seq.flags = I2C_FLAG_WRITE;
  /* Select register and data to write */
  i2c_write_data[0] = address;
  i2c_write_data[1] = data;
  seq.buf[0].data = i2c_write_data;
  seq.buf[0].len  = 2;
  seq.buf[1].data = i2c_read_data;
  seq.buf[1].len  = 0;

  ret = I2CSPM_Transfer(_handle.i2c, &seq);

  if (block)
    EnableAccelInterrupt();
  if (ret != i2cTransferDone)
  {
    return((int) ret);
  }
  return((int) 0);
}

/**************************************************************************//**
 * @brief Read from Accel i2c.
 *****************************************************************************/
static s16 Accel_i2c_smbus_read_byte_data(HANDLE accel_handle, u8 address, u8 *data, bool block)
{
  //  accel_handle is not used in the EFM32.   We use a global instead
  I2C_TransferSeq_TypeDef    seq;
  I2C_TransferReturn_TypeDef ret;
  uint8_t i2c_write_data[1];
  (void) accel_handle;
  if (block)
    DisableAccelInterrupt ();
  seq.addr  = _handle.i2cAddress;
  seq.flags = I2C_FLAG_WRITE_READ;
  /* Select register to start reading from */
  i2c_write_data[0] = address;
  seq.buf[0].data = i2c_write_data;
  seq.buf[0].len  = 1;
  /* Select length of data to be read */
  seq.buf[1].data = data;
  seq.buf[1].len  = 1;

  ret = I2CSPM_Transfer(_handle.i2c, &seq);

  if (block)
    EnableAccelInterrupt ();
  if (ret != i2cTransferDone)
  {
    *data = 0xff;
    return((int) ret);
  }
  return((int) 1);
}

/**************************************************************************//**
 * @brief Write block of data to Accel i2c.
 *****************************************************************************/
static s16 Accel_i2c_smbus_write_i2c_block_data(HANDLE accel_handle, u8 address, u8 length, u8 const* data, bool block)
{
  I2C_TransferSeq_TypeDef    seq;
  I2C_TransferReturn_TypeDef ret;
  uint8_t i2c_write_data[10];
  uint8_t i2c_read_data[1];
  int i;
  (void) accel_handle;
  if (block)
    DisableAccelInterrupt ();
  seq.addr  = _handle.i2cAddress;
  seq.flags = I2C_FLAG_WRITE;
  /* Select register to start writing to*/
  i2c_write_data[0] = address;
  for (i=0; i<length;i++)
  {
    i2c_write_data[i+1] = data[i];
  }
  seq.buf[0].data = i2c_write_data;
  seq.buf[0].len  = 1+length;
  seq.buf[1].data = i2c_read_data;
  seq.buf[1].len  = 0;

  ret = I2CSPM_Transfer(_handle.i2c, &seq);

  if (block)
    EnableAccelInterrupt ();
  if (ret != i2cTransferDone)
  {
    return((int) ret);
  }

  return((int) 0);
}

/**************************************************************************//**
 * @brief Read block of data from Accel i2c.
 *****************************************************************************/
static s16 Accel_i2c_smbus_read_i2c_block_data(HANDLE accel_handle, u8 address, u8 length, u8* data, bool block)
{
  I2C_TransferSeq_TypeDef    seq;
  I2C_TransferReturn_TypeDef ret;
  uint8_t i2c_write_data[1];
  (void) accel_handle;
  seq.addr  = _handle.i2cAddress;
  seq.flags = I2C_FLAG_WRITE_READ;
  if (block)
    DisableAccelInterrupt ();
  /* Select register to start reading from */
  i2c_write_data[0] = address;
  seq.buf[0].data = i2c_write_data;
  seq.buf[0].len  = 1;

  /* Select length of data to be read */
  seq.buf[1].data = data;
  seq.buf[1].len  = length;

  ret = I2CSPM_Transfer(_handle.i2c, &seq);

  if (block)
    EnableAccelInterrupt ();
  if (ret != i2cTransferDone)
  {
    return((int) ret);
  }

  return((int) 0);
}

int AccelFindEvb(char *port_description, char *last_port, int num_ports_found)
{
	(void) port_description;
	(void) last_port;
	(void) num_ports_found;
	return 0;
}

