/**************************************************************************//**
 * @brief Implementation specific functions for HRM code
 * @version x.x.x
 ******************************************************************************
 * @section License
 * <b>(C) Copyright 2017 Silicon Labs, http://www.silabs.com</b>
 *******************************************************************************
 *
 * This file is licensed under the Silabs License Agreement that is included in
 * with the software release. Before using this software for any purpose, you
 * must agree to the terms of that agreement.
 *
 ******************************************************************************/
#include <sihrmUser.h>
#include <stdio.h>
#include "settings.h"
#include "udelay.h"
#include "em_device.h"
#include "em_gpio.h"
#include "i2cspm.h"
#include "em_i2c.h"
#include "string.h"
#include "sihrm_demo.h"
#include "pinconfig.h"
#include "em_letimer.h"
#include "../si117xlib/si117xhrm_static.h"

#include "accel_sys_out.h"
#include "../si117xdrv/si117x_functions.h"
#include "../si117xdrv/si117xdrv.h"
#if (UART_DEBUG == 1)
  #include "uart_debug.h"
#endif

/*  I2C port configuration */
static devicePortConfig_t _handle;

/*  interrupt sequence counter  */
static uint16_t irqSequence = 0;

/*  interrupt queue size in bytes  */
#define IRQ_QUEUE_DEPTH (8*45)          // 8 x 45 x sizeof(Si117xhrmIrqSample_t), 8 is the max oversampling of basic 25Hz supported (i.e. 200Hz)

/*****************************************************************************
* Assert((IRQ_QUEUE_SIZE % sizeof(Si117xhrmIrqSample_t)) == 0)
*
* IRQ_QUEUE_SIZE must be a multiple of sizeof(Si117xhrmIrqSample_t).  For
* example, if sizeof(Si117xhrmIrqSample_t) = 20 then valid values of
* IRQ_QUEUE_SIZE are 20, 40, 60 ... 600, 620, 640 ... etc.  The Queue size is
* application dependent but must be big enough to ensure that the queue does
* not overflow during the maximum latency.
*
* The following line is a compile-time assert ensuring that IRQ_QUEUE_SIZE is
* defined correctly.  If IRQ_QUEUE_SIZE is not correctly defined then data
* mis-alignment may occur in the functions sihrmUser_SampleQueue_Get() and
* sihrmUser_SampleQueue_Put() resulting in invalid data being sent to the
* algorithm.
*
* If the assertion fails, this typedef line will attempt to define an array of
* negative size, resulting in a compile error.  The error message is compiler
* defined but should indicate a "negative array size."
*
* Since this is a typedef it will not result in any memory allocation when
* successful.
******************************************************************************/
/*  interrupt queue data */
static SI117XDRV_DataQueueID_t sihrmQueueID = SI117XDRV_UNINITIALIZED_QUEUE_ID;

SI117XDRV_PPGSample_t sihrmIrqQueue[IRQ_QUEUE_DEPTH];

extern uint8_t use_accelerometer;   //defined in sihrm_demo.c
extern uint8_t si117x_rev_id;

/** Structure describing the i2c interface for the accelerometer */
extern AccelPortConfig_t accelI2C;
/** Structure describing the i2c interface for the HRM sensor */
extern devicePortConfig_t hrmI2C;
SI117XDRV_DeviceSelect_t si117xDevice = 0;

/*  Non-exported Function Prototypes  */
//static int32_t sihrmUser_i2c_smbus_write_byte_data(sihrmUserHandle_t sihrmUser_handle, uint8_t address, uint8_t data, uint8_t block);
//static int32_t sihrmUser_i2c_smbus_read_byte_data(sihrmUserHandle_t sihrmUser_handle, uint8_t address, uint8_t *data, uint8_t block);
//static int32_t sihrmUser_i2c_smbus_write_i2c_block_data(sihrmUserHandle_t sihrmUser_handle, uint8_t address, uint8_t length, uint8_t const* data, uint8_t block);
//static int32_t sihrmUser_i2c_smbus_read_i2c_block_data(sihrmUserHandle_t sihrmUser_handle, uint8_t address, uint8_t length, uint8_t* data, uint8_t block);
void EnableAccelerometer(void);

/**************************************************************************//**
 * @brief Enables/Disables USB debug output.
 *****************************************************************************/
int32_t sihrmUser_SetupDebug(sihrmUserHandle_t sihrmUser_handle, int32_t enable, void *debug)
{
  return SI117xHRM_SUCCESS;
}

/**************************************************************************//**
 * @brief Prints USB debug output.
 *****************************************************************************/
int32_t sihrmUser_OutputDebugMessage(sihrmUserHandle_t sihrmUser_handle, const uint8_t *message)
{
  uint16_t i;
  uint8_t usb_data_buffer[((512)+3)&~3];

  usb_data_buffer[0] = 0x10;

  for(i=0; i<strlen((const char*)message); i++)
    usb_data_buffer[i+1] = message[i];

  usb_data_buffer[strlen((const char*)message)+1] = 0x10;

#if (UART_DEBUG == 1)
  UART_Send(usb_data_buffer, strlen((const char*)message)+2);
#endif
  return SI117xHRM_SUCCESS;
}

/**************************************************************************//**
 * @brief Outputs sample to USB debug interface
 *****************************************************************************/
int32_t sihrmUser_OutputRawSampleDebugMessage(sihrmUserHandle_t sihrmUser_handle, Si117xhrmIrqSample_t *sample, Si117xhrmSampleAuxDebug_t *aux)
{
  uint8_t usb_data_buffer[((512)+3)&~3];
  sprintf((char*) usb_data_buffer, "\n%d,%d,%d,%d,%d,%d,%d,%d,%d,", sample->timestamp, sample->sequence,
                        sample->ppg[0], sample->ppg[1], sample->ppg[2], sample->ppg[3],
                        sample->accelerometer_x, sample->accelerometer_y, sample->accelerometer_z);

#if (UART_DEBUG == 1)
  UART_Send(usb_data_buffer, strlen((const char*)usb_data_buffer));
#endif
  return SI117xHRM_SUCCESS;
}

/**************************************************************************//**
 * @brief Write to Si117x register
 *****************************************************************************/
//int32_t sihrmUser_WriteToRegister(sihrmUserHandle_t sihrmUser_handle, uint8_t address, uint8_t data)
//{
//  return sihrmUser_i2c_smbus_write_byte_data(sihrmUser_handle, address, data, true);
//}

/**************************************************************************//**
 * @brief Read from Si117x register.
 *****************************************************************************/
//int32_t sihrmUser_ReadFromRegister(sihrmUserHandle_t sihrmUser_handle, uint8_t address)
//{
//  u8 data;
//  sihrmUser_i2c_smbus_read_byte_data(sihrmUser_handle, address, &data, true);
//  return data;
//}

/**************************************************************************//**
 * @brief block write to si117x
 *****************************************************************************/
//int32_t sihrmUser_BlockWrite(sihrmUserHandle_t sihrmUser_handle, uint8_t address, uint8_t length, uint8_t const *values)
//{
//  return sihrmUser_i2c_smbus_write_i2c_block_data(sihrmUser_handle,
//                                               address,
//                                               length,
//                                               values,
//                                               true);
//}

/**************************************************************************//**
 * @brief Block read from Si117x.
 *****************************************************************************/
//int32_t sihrmUser_BlockRead(sihrmUserHandle_t sihrmUser_handle, uint8_t address, uint8_t length, uint8_t *values)
//{
//    return sihrmUser_i2c_smbus_read_i2c_block_data(sihrmUser_handle,
//                           address,    length,     values, true);
//}

/**************************************************************************//**
 * @brief Disable GPIO interrupt for Si117x interrupt.
 *****************************************************************************/
void DisableSi117xInterrupt ()
{

	GPIO_IntDisable(1<<_handle.irqPin);
}

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

/**************************************************************************//**
 * @brief 
 *****************************************************************************/
int32_t sihrmUser_PostSkinSensingCallback(sihrmUserHandle_t sihrmUser_handle)
{ int32_t error = SI117xHRM_SUCCESS;

  Si117xFlushFIFO(sihrmUser_handle);    // clear FIFO of any residual data

  // Initialize for Fs ratio locking
  // ResetResampler();
  if (use_accelerometer)
    EnableAccelerometer();
  SI117XDRV_InitializeSi117xFifoStateMachine(si117xDevice);
  return error;
}

SI117XDRV_DeviceSelect_t sihrmUser_getDeviceID (void)
{
  return si117xDevice;
}

/**************************************************************************//**
 * @brief Main interrupt processing routine for Si117x.
 *****************************************************************************/
int32_t sihrmUser_ProcessIrq(sihrmUserHandle_t sihrmUser_handle, uint16_t timestamp, Si117xhrmUserConfiguration_t *configuration)
{
  int32_t error = 0;
  uint8_t DC_bytesToRead = 16;
  uint8_t fifoData[256];   //2048 is the max size of the FIFO...this can be reduced to save RAM
  int32_t irqstat;

  // FIFO mode interrupt handling
  uint16_t numBytesInSi117xFifo;

  /* check for Si117x FIFO IRQ (cannot assume) */
  irqstat = Si117xGetIrqStatus(sihrmUser_handle, si117x_rev_id);

  if (irqstat & PPG1_IRQ)      //used for legacy mode and fifo mode during DC sensing
  {
	// Read the FIFO right here in to elastic buffer
	Si117xBlockRead(sihrmUser_handle, REG_FIFO_READ, DC_bytesToRead, fifoData);
	irqSequence++;
	SI117XDRV_ProcessPPGInterrupt(si117xDevice,fifoData,timestamp,irqSequence);
  }
  else if (irqstat & FIFO_IRQ)
  {
    numBytesInSi117xFifo = configuration->deviceConfiguration->fifo_int_level;
	irqSequence++;
	while (numBytesInSi117xFifo >0 )	// repeated block read may be needed since block is limited to 255
	{
	  if (numBytesInSi117xFifo > 254)
	  {
	    numBytesInSi117xFifo = numBytesInSi117xFifo - 254;
	    Si117xBlockRead(sihrmUser_handle, REG_FIFO_READ, 254, fifoData);
		SI117XDRV_ProcessFifoData(si117xDevice, fifoData, 254, timestamp, irqSequence, NULL);
	  }
	  else
	  {
		Si117xBlockRead(sihrmUser_handle, REG_FIFO_READ, numBytesInSi117xFifo, fifoData);
		SI117XDRV_ProcessFifoData(si117xDevice, fifoData, numBytesInSi117xFifo, timestamp, irqSequence, NULL);
		numBytesInSi117xFifo = 0;
	  }
	}
  }

  return error;
}

/**************************************************************************//**
 * @brief Query number of entries in the interrupt queue.
 *****************************************************************************/
int32_t sihrmUser_SampleQueueNumentries(sihrmUserHandle_t sihrmUser_handle)
{	
  (void) sihrmUser_handle;
  s16 count=0;

  DisableSi117xInterrupt ();
  count = SI117XDRV_NumSamplesInQueue(sihrmQueueID);
  EnableSi117xInterrupt();
  return count;
}

/**************************************************************************//**
 * @brief Get sample from the interrupt queue.
 *****************************************************************************/
int32_t sihrmUser_SampleQueue_Get(sihrmUserHandle_t sihrmUser_handle, uint8_t getAccelSamples, Si117xhrmIrqSample_t *samples)
{	
  (void) sihrmUser_handle;
  int16_t error = SI117xHRM_SUCCESS;
  SI117XDRV_PPGSample_t s;
  getAccelSamples = getAccelSamples & use_accelerometer;   // don't wait for G samples if accelerometer is disabled

  if ((sihrmUser_SampleQueueNumentries(sihrmUser_handle)>0) && (getAccelSamples == 0))
  { DisableSi117xInterrupt ();
    SI117XDRV_DequeuePPGSampleData(sihrmQueueID,&s);

    //copy to sihrmIrqSample struct
    samples->sequence = s.sequence;
    samples->timestamp = s.timestamp;
    samples->syncMessage = s.syncMessage;
    samples->ppg[0] = s.ppg1;
    samples->ppg[1] = s.ppg2;
    samples->ppg[2] = s.ppg3;
    samples->ppg[3] = s.ppg4;
    samples->accelerometer_x = 0;
    samples->accelerometer_y = 0;
    samples->accelerometer_z = 0;

    EnableSi117xInterrupt();

    if (getAccelSamples==1)  // FIFO mode and not DC sensing, get resampled G data (y and Z are zeros)
    { samples->accelerometer_x = 0;
      samples->accelerometer_y = 0;
      samples->accelerometer_z = 0;
    }
  }
  else
  { error = SI117xHRM_ERROR_SAMPLE_QUEUE_EMPTY;
    goto Error;
  }

Error:
  return error;
}

/**************************************************************************//**
 * @brief Empty the interrupt queue.
 *****************************************************************************/
int32_t sihrmUser_SampleQueue_Clear(sihrmUserHandle_t sihrmUser_handle)
{	
  (void) sihrmUser_handle;

  SI117XDRV_ClearQueue(sihrmQueueID);
  irqSequence = 0;
  return 0;
}

/**************************************************************************//**
 * @brief Initialize low level handle and clear irq queue.
 *****************************************************************************/
int32_t sihrmUser_Initialize(void *port, int16_t options, sihrmUserHandle_t *sihrmUser_handle, SI117XDRV_DeviceSelect_t *deviceID)
{	
  s16 error = 0;
  u8 data;
  (void) options;
  *sihrmUser_handle = (HANDLE)&_handle;

  if (deviceID != NULL)
  {
    *deviceID = si117xDevice;
  }
  _handle.i2cAddress = ((devicePortConfig_t*)port)->i2cAddress<<1;
  _handle.irqPort = ((devicePortConfig_t*)port)->irqPort;
  _handle.irqPin = ((devicePortConfig_t*)port)->irqPin;
  _handle.i2c = ((devicePortConfig_t*)port)->i2c;
  SI117XDRV_InitAPI(si117xDevice,(HANDLE)&_handle);

  data = Si117xReadFromRegister((*sihrmUser_handle), REG_PART_ID);
  si117x_rev_id = Si117xReadFromRegister((*sihrmUser_handle), REG_REV_ID);

  if ((_handle.i2cAddress == (0x60<<1)) && (data != 0x46) && (data != 0x47))
    error = -1;
  if ((_handle.i2cAddress == (0x5A<<1)) && (data != 0x43) && (data != 0x44))
    error = -1;
  if ((_handle.i2cAddress == (SI1175_I2C_ADDR<<1)) && (data != 0x71) && (data != 0x72) && (data != 0x73) && (data != 0x74) && (data != 0x75))
    error = -1;
  if (sihrmQueueID == SI117XDRV_UNINITIALIZED_QUEUE_ID)
  {
    SI117XDRV_AllocatePPGDataQueue(&sihrmQueueID, sihrmIrqQueue, IRQ_QUEUE_DEPTH*SI117XDRV_PPG_SAMPLE_SIZE_BYTES);
  }
  sihrmUser_SampleQueue_Clear(*sihrmUser_handle);

  return error;
}

/**************************************************************************//**
 * @brief Close Si117x.
 *****************************************************************************/
int32_t sihrmUser_Close(sihrmUserHandle_t sihrmUser_handle)
{	
  (void) sihrmUser_handle;
  _handle.i2cAddress = 0xff;
  return 0;
}

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

/**************************************************************************//**
 * @brief 10ms delay required by Si117x reset sequence.
 *****************************************************************************/
//void delay_10ms(void)
//{
//  int i;
//  	for (i=0;i<10;i++)
//  	{
//  		UDELAY_Delay(1000);
//  	}
//  return;
//}

/**************************************************************************//**
 * @brief Write to Si117x i2c.
 *****************************************************************************/
//static int32_t sihrmUser_i2c_smbus_write_byte_data(sihrmUserHandle_t sihrmUser_handle, uint8_t address, uint8_t data, uint8_t block)
//{
//  I2C_TransferSeq_TypeDef    seq;
//  I2C_TransferReturn_TypeDef ret;
//  uint8_t i2c_write_data[2];
//  uint8_t i2c_read_data[1];
//  (void) sihrmUser_handle;
//  if (block)
//    DisableSi117xInterrupt ();
//  seq.addr  = _handle.i2cAddress;
//  seq.flags = I2C_FLAG_WRITE;
//  /* Select register and data to write */
//  i2c_write_data[0] = address | 0x40;  // Set to disable auto increment;
//  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)
//    EnableSi117xInterrupt();
//  if (ret != i2cTransferDone)
//  {
//    return((int) ret);
//  }
//  return((int) 0);
//}

/**************************************************************************//**
 * @brief Read from Si117x i2c.
 *****************************************************************************/
//static int32_t sihrmUser_i2c_smbus_read_byte_data(sihrmUserHandle_t sihrmUser_handle, uint8_t address, uint8_t *data, uint8_t block)
//{
//  //  si117x_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) sihrmUser_handle;
//  if (block)
//    DisableSi117xInterrupt ();
//  seq.addr  = _handle.i2cAddress;
//  seq.flags = I2C_FLAG_WRITE_READ;
//  /* Select register to start reading from */
//  i2c_write_data[0] = address | 0x40;  // Set to disable auto increment;
//  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)
//    EnableSi117xInterrupt ();
//  if (ret != i2cTransferDone)
//  {
//    *data = 0xff;
//    return((int) ret);
//  }
//  return((int) 1);
//}

/**************************************************************************//**
 * @brief Write block of data to Si117x i2c.
 *****************************************************************************/
//static int32_t sihrmUser_i2c_smbus_write_i2c_block_data(sihrmUserHandle_t sihrmUser_handle, uint8_t address, uint8_t length, uint8_t const* data, uint8_t block)
//{
//  I2C_TransferSeq_TypeDef    seq;
//  I2C_TransferReturn_TypeDef ret;
//  uint8_t i2c_write_data[10];
//  uint8_t i2c_read_data[1];
//  int i;
//  (void) sihrmUser_handle;
//  if (block)
//    DisableSi117xInterrupt ();
//  seq.addr  = _handle.i2cAddress;
//  seq.flags = I2C_FLAG_WRITE;
//  /* Select register to start writing to*/
//  i2c_write_data[0] = address | 0x40;  // Set to disable auto increment;
//  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)
//    EnableSi117xInterrupt ();
//  if (ret != i2cTransferDone)
//  {
//    return((int) ret);
//  }
//
//  return((int) 0);
//}

/**************************************************************************//**
 * @brief Read block of data from Si117x i2c.
 *****************************************************************************/
//static int32_t sihrmUser_i2c_smbus_read_i2c_block_data(sihrmUserHandle_t sihrmUser_handle, uint8_t address, uint8_t length, uint8_t* data, uint8_t block)
//{
//  I2C_TransferSeq_TypeDef    seq;
//  I2C_TransferReturn_TypeDef ret;
//  uint8_t i2c_write_data[1];
//  (void) sihrmUser_handle;
//  seq.addr  = _handle.i2cAddress;
//  seq.flags = I2C_FLAG_WRITE_READ;
//  if (block)
//    DisableSi117xInterrupt ();
//  /* Select register to start reading from */
//  i2c_write_data[0] = address | 0x40;  // Set to disable auto increment
//  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)
//    EnableSi117xInterrupt ();
//  if (ret != i2cTransferDone)
//  {
//    return((int) ret);
//  }
//
//  return((int) 0);
//}


/**************************************************************************//**
 * @brief
 *****************************************************************************/
void EnableAccelerometer(void)
{  // For BMA255 only
#if LEGACY==1
  AccelWriteToRegister(&accelI2C, 0x17, 0x00, true);		// INT disabled.
  AccelWriteToRegister(&accelI2C, 0x1a, 0x00, true);		// INT2 pin disabled
  AccelReadFromRegister(&accelI2C, 0x0a, true);             // clear INGT flag
#else

  AccelWriteToRegister(&accelI2C, 0x30, ACCEL_FIFO_LEVEL, true);		// 24, FIFO watermark =< 32
  AccelWriteToRegister(&accelI2C, 0x17, 0x50, true);		// FIFO watermark INT enabled.
  AccelWriteToRegister(&accelI2C, 0x1a, 0x42, false);		// INT2 and INT1 pins enabled for FIFO watermark

  AccelWriteToRegister(&accelI2C, 0x21, 0x87, false);		// INT is latched
#endif

  AccelWriteToRegister(&accelI2C, 0x3e, 0x40, true);		// clear Acc FIFO
  AccelWriteToRegister(&accelI2C, 0x11, 0x00, true);		// Normal power mode
  AccelWriteToRegister(&accelI2C, 0x12, 0x00, true);		// Normal power mode
}

void DisableAccelerometer(void)
{   // For BMA255 only
  AccelWriteToRegister(&accelI2C, 0x12, 0x40, true);		// low power mode
  AccelWriteToRegister(&accelI2C, 0x11, 0x80, true);		// Suspend power mode
  AccelWriteToRegister(&accelI2C, 0x17, 0x00, true);		// Disable INT enabled.
  AccelWriteToRegister(&accelI2C, 0x21, 0x87, true);  	    // clear INT so the
}
