/******************************************************************************
SparkFun_Qwiic_KX13X.cpp
SparkFun Qwiic KX13X Library Source File
Elias Santistevan @ SparkFun Electronics
Original Creation Date: March, 2021
This file implements the QwiicKX13XCore, QwiicKX132, and QwiicKX134 class
Development environment specifics:
IDE: Arduino 1.8.12
This code is Lemonadeware; if you see me (or any other SparkFun employee) at the
local, and you've found our code helpful, please buy us a round!
Distributed as-is; no warranty is given.

 CPOL and CPHA are demonstrated on pg 25 of Specification Data sheet  
 CPOL = 0, CPHA = 0 SPI_MODE0
******************************************************************************/

#include "kx13x.h"

uint8_t getID(SPI_HandleTypeDef *hspi)
{
  uint8_t partID =0;
  KX13X_STATUS_t status = readRegister(hspi, KX13X_WHO_AM_I, &partID, 1);
  if( status != KX13X_SUCCESS ) 
    return status;
  else    
    return partID;
  
}

// This function sets various register with regards to these pre-determined
// settings. These settings are set according to "AN092 Getting Started" guide and can easily
// have additional presets added.
bool initialize(SPI_HandleTypeDef *hspi, uint8_t settings)
{

  KX13X_STATUS_t returnError = KX13X_GENERAL_ERROR;
  if( !accelControl(hspi, false) ){
    return false; 
  }
	
	
  if( settings == DEFAULT_SETTINGS )
    returnError = writeRegister(hspi, KX13X_CNTL1, 0x00, DEFAULT_SETTINGS, 0);
  if( settings == INT_SETTINGS ){
    setInterruptPin(hspi, true, 1, 0, false);
    routeHardwareInterrupt(hspi, HI_DATA_READY, 1);
    returnError = writeRegister(hspi, KX13X_CNTL1, 0x00, INT_SETTINGS, 0);
  }
  if( settings == SOFT_INT_SETTINGS ){
    returnError = writeRegister(hspi,KX13X_CNTL1, 0x00, INT_SETTINGS, 0);
  }
  if( settings == BUFFER_SETTINGS ){
    setInterruptPin(hspi, true, 1, 0, false);
    routeHardwareInterrupt(hspi, HI_BUFFER_FULL, 1);
    enableBuffer(hspi, true, true);
    setBufferOperation(hspi, BUFFER_MODE_FIFO, BUFFER_16BIT_SAMPLES);
    returnError = writeRegister(hspi, KX13X_CNTL1, 0x00, INT_SETTINGS, 0);
  }
	returnError = writeRegister(hspi, KX13X_ODCNTL, 0x3F, 0x01, 6);
  if( returnError == KX13X_SUCCESS )
    return true;
  else
    return false;
}

// Address: 0x1B, bit[7]: default value is: 0x00
// This function sets the accelerometer into stand-by mode or
// an active mode depending on the given argument.
bool accelControl(SPI_HandleTypeDef *hspi, bool standby){

  if( standby != true && standby != false )
    return false;
  
  KX13X_STATUS_t returnError;
  returnError = writeRegister(hspi, KX13X_CNTL1, 0x7F, standby, 7);
  if( returnError == KX13X_SUCCESS )
    return true;
  else
    return false;
  
}

// Address: 0x1B, bit[7]: default value is: 0x00
// This function reads whether the accelerometer is in stand by or an active
// mode. 
uint8_t readAccelState(SPI_HandleTypeDef *hspi){

  uint8_t tempRegVal;
  readRegister(hspi, KX13X_CNTL1, &tempRegVal, 1);
  return (tempRegVal & 0x80) >> 7;

}
// Address: 0x1B, bit[1:0]: default value is: 0x00 (2g)
// This function sets the acceleration range of the accelerometer outputs.
// Possible KX132 arguments: 0x00 (2g), 0x01 (4g), 0x02 (8g), 0x03 (16g)
// Possible KX134 arguments: 0x00 (8g), 0x01 (16g), 0x02 (32g), 0x03 (64g)
bool setRange(SPI_HandleTypeDef *hspi, uint8_t range){

  if( range > 3)
    return false;

  uint8_t accelState = readAccelState(hspi);
  accelControl(hspi,false);

  KX13X_STATUS_t returnError;
  returnError = writeRegister(hspi, KX13X_CNTL1, 0xE7, range, 3);
  if( returnError == KX13X_SUCCESS ){
    accelControl(hspi, accelState);
    return true;
  }
  else
    return false;
  
}


//Address: 0x21, bits[3:0] - default value is 0x06 (50Hz)
//Sets the refresh rate of the accelerometer's data. 
// 0.781 * (2 * (n)) derived from pg. 26 of Techincal Reference Manual
bool setOutputDataRate(SPI_HandleTypeDef *hspi, uint8_t rate){

  if( rate > 15 )
    return false;

  uint8_t accelState = readAccelState(hspi); // Put it back where we found it.
  accelControl(hspi, false); // Can't adjust without putting to sleep

  KX13X_STATUS_t returnError;
  returnError = writeRegister(hspi, KX13X_ODCNTL, 0xF0, rate, 0);
  if( returnError == KX13X_SUCCESS ){
    accelControl(hspi, accelState);
    return true;
  }
  else
    return false;
}

// Address:0x21 , bit[3:0]: default value is: 0x06 (50Hz)
// Gets the accelerometer's output data rate. 
float readOutputDataRate(SPI_HandleTypeDef *hspi, uint32_t *delay){
  
  uint8_t tempRegVal;
  readRegister(hspi, KX13X_ODCNTL, &tempRegVal, 1);
  tempRegVal &= 0x0F;
  tempRegVal = (float)tempRegVal;
	*delay = DelayLUT[(int) tempRegVal];
  return (0.78 * (pow(2,tempRegVal)));

}


// Address: 0x22, bit[7:4] default value is 0000.
// This register controls the various interrupt settings, all of which can be
// set here. Note: trying to set just one will set the others to their default
// state.
bool setInterruptPin(SPI_HandleTypeDef *hspi, bool enable, uint8_t polarity, uint8_t pulseWidth, bool latchControl){
  
  if( enable != true && enable != false ) 
    return false;
  else if( polarity != 1 && polarity != 0 ) 
    return false;
  else if( pulseWidth != 1 && pulseWidth != 0 ) 
    return false;

  uint8_t accelState = readAccelState(hspi); // Put it back where we found it.
  accelControl(hspi, false); // Can't adjust without putting to sleep

  uint8_t combinedArguments = ((pulseWidth << 6) | (enable << 5) | (polarity << 4) | (latchControl << 3));
  KX13X_STATUS_t returnError;
  returnError = writeRegister(hspi, KX13X_INC1, 0x07, combinedArguments, 0);
  if( returnError == KX13X_SUCCESS ){
    accelControl(hspi, accelState); 
    return true;
  }
  else
    return false;
}


// Address: 0x25, bits[7:0]: default value is 0: disabled
// Enables anyone of the various interrupt settings to be routed to hardware
// interrupt pin one or pin two.
bool routeHardwareInterrupt(SPI_HandleTypeDef *hspi, uint8_t rdr, uint8_t pin){

  if(  rdr > 128 )
    return false;
  if( pin != 1 && pin != 2)
    return false;
  
  uint8_t accelState = readAccelState(hspi); // Put it back where we found it.
  accelControl(hspi,false); // Can't adjust without putting to sleep

  KX13X_STATUS_t returnError;

  if( pin == 1 ){
    returnError = writeRegister(hspi, KX13X_INC4, 0x00, rdr, 0);
    if( returnError == KX13X_SUCCESS ){
      accelControl(hspi, accelState); 
      return true;
    }
  else
    returnError = writeRegister(hspi, KX13X_INC6, 0x00, rdr, 0);
    if( returnError == KX13X_SUCCESS ){
      accelControl(hspi,accelState);
      return true;
    }
  }

  return false;

}

// Address: 0x1A , bit[7:0]: default value is: 0x00
// This function reads the interrupt latch release register, thus clearing any
// interrupts. 
bool clearInterrupt(SPI_HandleTypeDef *hspi){
  
  uint8_t tempRegVal;
  KX13X_STATUS_t returnError;
  returnError = readRegister(hspi, KX13X_INT_REL, &tempRegVal, 1);
  if( returnError == KX13X_SUCCESS )
    return true;
  else 
    return false;
}

// Address: 0x17 , bit[4]: default value is: 0
// This function triggers collection of data by the KX13X.
bool dataTrigger(SPI_HandleTypeDef *hspi){
  
  uint8_t tempRegVal;
  KX13X_STATUS_t returnError;
  returnError = readRegister(hspi, KX13X_INS2, &tempRegVal, 1);
  if( returnError == KX13X_SUCCESS ){
    if( tempRegVal & 0x10 )
      return true;
    else
      return false;
  }
  else 
    return false;

}

// Address: 0x5E , bit[7:0]: default value is: unknown
// This function sets the number of samples (not bytes) that are held in the
// buffer. Each sample is one full word of X,Y,Z data and the minimum that this
// can be set to is two. The maximum is dependent on the resolution: 8 or 16bit,
// set in the BUF_CNTL2 (0x5F) register (see "setBufferOperation" below).  
bool setBufferThreshold(SPI_HandleTypeDef *hspi, uint8_t threshold){

  if( threshold < 2 || threshold > 171 )
    return false;

  
  uint8_t tempRegVal;
  uint8_t resolution;
  KX13X_STATUS_t returnError;
  returnError = readRegister(hspi, KX13X_BUF_CNTL2, &tempRegVal, 1);
  resolution = (tempRegVal & 0x40) >> 6; 
  if( returnError != KX13X_SUCCESS )
    return false;

  if( threshold > 86 && resolution == 1 ) // 1 = 16bit resolution, max samples: 86
    threshold = 86; 
  
  returnError = writeRegister(hspi, KX13X_BUF_CNTL1, 0x00, threshold, 0);
  if( returnError == KX13X_SUCCESS )
    return true;
  else 
    return false;

}

// Address: 0x5F, bits[6] and bits[1:0]: default value is: 0x00
// This functions sets the resolution and operation mode of the buffer. This does not include
// the threshold - see "setBufferThreshold". This is a "On-the-fly" register, accel does not need
// to be powered down to adjust settings.
bool setBufferOperation(SPI_HandleTypeDef *hspi, uint8_t operationMode, uint8_t resolution){

  if( resolution > 1 )
    return false;
  if( operationMode > 2 )
    return false; 


  uint8_t combinedSettings = (resolution << 6) | operationMode;
  KX13X_STATUS_t returnError;
  returnError = writeRegister(hspi, KX13X_BUF_CNTL2, 0xBC, combinedSettings, 0);
  if( returnError == KX13X_SUCCESS )
    return true;
  else
    return false;
}

// Address: 0x5F, bit[7] and bit[5]: default values is: 0x00
// This functions enables the buffer and also whether the buffer triggers an
// interrupt when full. This is a "On-the-fly" register, accel does not need
// to be powered down to adjust settings.
bool enableBuffer(SPI_HandleTypeDef *hspi, bool enable, bool enableInterrupt){

  if( ( enable != true && enable != false)  && (enableInterrupt != true && enableInterrupt != false) )
    return false;

  uint8_t combinedSettings = (enable << 7) | (enableInterrupt << 5); 
  KX13X_STATUS_t returnError;
  returnError = writeRegister(hspi, KX13X_BUF_CNTL2, 0x5F, combinedSettings, 0);
  if( returnError == KX13X_SUCCESS )
    return true;
  else
    return false;
}

// Address: 0x1C, bit[6]: default value is: 0x00 
//Tests functionality of the integrated circuit by setting the command test
//control bit, then checks the results in the COTR register (0x12): 0xAA is a
//successful read, 0x55 is the default state. 
bool runCommandTest(SPI_HandleTypeDef *hspi)
{
  
  uint8_t tempRegVal;
  KX13X_STATUS_t returnError;

  returnError = writeRegister(hspi, KX13X_CNTL2, 0xBF, 1, 6);
  if( returnError != KX13X_SUCCESS )
    return false;

  returnError = readRegister(hspi, KX13X_COTR, &tempRegVal, 1);
  if( returnError == KX13X_SUCCESS && tempRegVal == COTR_POS_STATE)
    return true;
  else 
    return false;
}

// Address:0x08 - 0x0D or 0x63 , bit[7:0]
// Reads acceleration data from either the buffer or the output registers
// depending on if the user specified buffer usage.
bool getRawAccelData(SPI_HandleTypeDef *hspi, rawOutputData *rawAccelData){
  uint8_t tempRegVal;
  KX13X_STATUS_t returnError;
  uint8_t tempRegData[TOTAL_ACCEL_DATA_16BIT] = {0}; 

  returnError = readRegister(hspi, KX13X_INC4, &tempRegVal, 1);
  if( returnError != KX13X_SUCCESS )
    return false;

  if( tempRegVal & 0x40 ){ // If Buffer interrupt is enabled, then we'll read accelerometer data from buffer register.
    returnError = readRegister(hspi, KX13X_BUF_READ, tempRegData, TOTAL_ACCEL_DATA_16BIT);
  }
  else
    returnError = readRegister(hspi, KX13X_XOUT_L, tempRegData, TOTAL_ACCEL_DATA_16BIT);



  if( returnError == KX13X_SUCCESS ) {
    rawAccelData->xData = tempRegData[XLSB]; 
    rawAccelData->xData |= ((uint16_t)(tempRegData[XMSB]) << 8); 
    rawAccelData->yData = tempRegData[YLSB]; 
    rawAccelData->yData |= ((uint16_t)(tempRegData[YMSB]) << 8); 
    rawAccelData->zData = tempRegData[ZLSB]; 
    rawAccelData->zData |= ((uint16_t)(tempRegData[ZMSB]) << 8); 
    return true;
  }

  return false;
}


// Reads a single register using the selected bus. 
KX13X_STATUS_t readRegister(SPI_HandleTypeDef *hspi, uint8_t reg, uint8_t *bufp,
                             uint16_t len)
{				

    /* Read command */
		reg |= SPI_READ; 
    HAL_GPIO_WritePin(GPIOC, GPIO_PIN_9,GPIO_PIN_RESET);
		HAL_SPI_Transmit(hspi, &reg, 1, 1000);
    HAL_SPI_Receive(hspi, bufp, len, 1000);
		//HAL_SPI_TransmitReceive(handle,&reg,bufp,1,1000);
    HAL_GPIO_WritePin(GPIOC, GPIO_PIN_9, GPIO_PIN_SET);
 
	return KX13X_SUCCESS;
}



// Writes the given value to the given register, using the provided mask and
// bit position. 
KX13X_STATUS_t writeRegister(SPI_HandleTypeDef *hspi, uint8_t reg, uint8_t mask, uint8_t data, uint8_t bitPos)
{
		uint8_t tempRegVal; 
		KX13X_STATUS_t returnError;

		returnError = readRegister(hspi, reg, &tempRegVal, 1);
		if( returnError != KX13X_SUCCESS )
			return KX13X_I2C_ERROR;
		tempRegVal &= mask;
		tempRegVal |= (data << bitPos);  
		
    HAL_GPIO_WritePin(GPIOC, GPIO_PIN_9, GPIO_PIN_RESET);
    HAL_SPI_Transmit(hspi, &reg, 1, 1000);
    HAL_SPI_Transmit(hspi, &tempRegVal, 1, 1000);
    HAL_GPIO_WritePin(GPIOC, GPIO_PIN_9, GPIO_PIN_SET);
    return KX13X_SUCCESS;


}


// Grabs raw accel data and passes it to the following function to be
// converted.
outputData getAccelData(SPI_HandleTypeDef *hspi){
  
	outputData userAccel;
	rawOutputData rawAccelData;
	
  if( getRawAccelData(hspi,&rawAccelData) &&
    convAccelData(hspi, &userAccel, &rawAccelData) )
      return userAccel;

  userAccel.xData = 0xFF;
  userAccel.yData = 0xFF;
  userAccel.zData = 0xFF;
  return userAccel;
}

// Converts acceleration data according to the set range value. 
bool convAccelData(SPI_HandleTypeDef *hspi, outputData *userAccel, rawOutputData *rawAccelData){

	uint8_t dev_ID = getID(hspi);
  uint8_t regVal;
  uint8_t range; 
  KX13X_STATUS_t returnError;
  returnError = readRegister(hspi, KX13X_CNTL1, &regVal, 1);
  if( returnError != KX13X_SUCCESS )
    return false; 

  range = (regVal & 0x18) >> 3;
  
 if(dev_ID == KX134_WHO_AM_I)
	{
		switch( range ) {
			case KX134_RANGE8G:
				userAccel->xData = (float)rawAccelData->xData * KX134convRange8G;
				userAccel->yData = (float)rawAccelData->yData * KX134convRange8G;
				userAccel->zData = (float)rawAccelData->zData * KX134convRange8G;
				break;                                               
			case KX134_RANGE16G:                                   
				userAccel->xData = (float)rawAccelData->xData * KX134convRange16G;
				userAccel->yData = (float)rawAccelData->yData * KX134convRange16G;
				userAccel->zData = (float)rawAccelData->zData * KX134convRange16G;
				break;                                               
			case KX134_RANGE32G:                                   
				userAccel->xData = (float)rawAccelData->xData * KX134convRange32G;
				userAccel->yData = (float)rawAccelData->yData * KX134convRange32G;
				userAccel->zData = (float)rawAccelData->zData * KX134convRange32G;
				break;                                               
			case KX134_RANGE64G:                                   
				userAccel->xData = (float)rawAccelData->xData * KX134convRange64G;
				userAccel->yData = (float)rawAccelData->yData * KX134convRange64G;
				userAccel->zData = (float)rawAccelData->zData * KX134convRange64G;
				break;

		}		
	
	}

  return true;
}