/* *****************************************************************************
 * File:      acc_LIS2DW12.c
 * Date:      29 Jan 2024
 * Author:    Andrew Levido
 *
 * Copyright 2025
 *  Imbrius Pty Ltd - All Rights Reserved
 *
 ******************************************************************************/

/* Includes ------------------------------------------------------------------*/
#include "acc_LIS2DW12.h"
#include "acc_LIS2DW12_Regs.h"
#include "i2c_driver.h"

/* Defines -------------------------------------------------------------------*/
#define ACC_SPI_TIMEOUT       100
#define ACC_CHIP_ID           0x44

/* Typedefs & Enumerations ---------------------------------------------------*/

/* Global Variables ----------------------------------------------------------*/
static struct {
  bool init;
  acc_odr_t odr;
  acc_pwr_mode_t pmode;
  void (*read_cb)(bool valid);
  bool dtap;
  void (*tap_cb)(acc_tap_t tap);
  void (*orient_cb)(acc_orient_t or);
  void (*freefall_cb)(void);
  void (*activity_cb)(bool awake);
  int16_t x;
  int16_t y;
  int16_t z;
  int16_t t;
  bool reading;
  acc_act_int_t activity_int_mode;
} acc;

/* Private Functions ---------------------------------------------------------*/
int32_t acc_read_reg(uint8_t reg, uint8_t * data)
{
  i2c_result_t result;
  result = i2c_read(ACC_DEV_ADDRESS, reg, data, 1, false);
  if(result != I2C_OK) { return(ACC_ERR_I2C); }
  return(ACC_OK);
}

int32_t acc_write_reg(uint8_t reg, uint8_t data)
{
  i2c_result_t result;
  
  result = i2c_write(ACC_DEV_ADDRESS, reg, &data, 1);
  if(result != I2C_OK) { return(ACC_ERR_I2C); }
  return(ACC_OK);
}

/* Public Functions ----------------------------------------------------------*/

/* Initialisation - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
int32_t acc_init(acc_init_cfg_t * cfg)
{
  uint8_t buff;
  int32_t result;
  
  LL_mDelay(1);

  // Check Accelerometer comms
  result = acc_read_reg(LIS2DW12_WHO_AM_I, &buff);
  if(result != ACC_OK) { return(result); }
  if(buff != ACC_CHIP_ID) { return(ACC_ERR_ID); }

  // Soft Reset accelerometer to ensure registers at default values
  result = acc_soft_reset();
  if(result != ACC_OK) { return(result); }

  // Configure CTRL register 2
  lis2dw12_ctrl2_t ctrl2;
  ctrl2.byte = 0;                            // default
  ctrl2.bits.i2c_disable = false;            // don't disable 
  ctrl2.bits.cs_pu_disc = 1;                 // disable CS pullup
  ctrl2.bits.bdu = 1;                        // inhibit update until both bytes read
  result = acc_write_reg(LIS2DW12_CTRL2, ctrl2.byte);
  if(result != ACC_OK) { return(result); }

  // Configure CTRL register 3
  lis2dw12_ctrl3_t ctrl3;
  ctrl3.byte = 0;                           // default
  ctrl3.bits.slp_mode = 0x2;                // software trigger 1-shot conversion
  result = acc_write_reg(LIS2DW12_CTRL3, ctrl3.byte);
  if(result != ACC_OK) { return(result); }

  // Set up ODR
  if(cfg->pwrmode == ACC_PWR_HI) {
    if(cfg->odr == ACC_ODR_1HZ6) { acc.odr = ACC_ODR_12HZ5; }
    else { acc.odr = cfg->odr; }
  }
  else {
    if(cfg->odr > ACC_ODR_200HZ) { acc.odr = ACC_ODR_200HZ; } 
    else { acc.odr = cfg->odr; }
  }

  acc.pmode = cfg->pwrmode;

  // Configure CTRL register 1
  lis2dw12_ctrl1_t ctrl1;
  ctrl1.bits.pwr_mode = cfg->pwrmode;          // power mode
  ctrl1.bits.lp_mode = cfg->lpmode;            // low power mode
  ctrl1.bits.odr = acc.odr;                    // output data rate
  result = acc_write_reg(LIS2DW12_CTRL1, ctrl1.byte);
  if(result != ACC_OK) { return(result); }

  // Configure CTRL register 6
  lis2dw12_ctrl6_t ctrl6;
  ctrl6.byte = 0;                             // default
  ctrl6.bits.low_noise = cfg->low_noise;      // low noise
  ctrl6.bits.fs = cfg->fullscale;             // full scale acceleration
  ctrl6.bits.bw_filt = cfg->bandwidth;        // lp filter bandwidth
  result = acc_write_reg(LIS2DW12_CTRL6, ctrl6.byte);
  if(result != ACC_OK) { return(result); }

  // Configure CTRL register 7
  lis2dw12_ctrl7_t ctrl7;
  ctrl7.byte = 0;                             // default
  ctrl7.bits.drdy_pulsed = 1;                 // DRDY pulsed
  ctrl7.bits.int2_on_int1 = 1;                // All interrupts on INT1
  ctrl7.bits.interrupts_enable = 1;           // Allow function interrupts
  result = acc_write_reg(LIS2DW12_CTRL7, ctrl7.byte);
  if(result != ACC_OK) { return(result); }

  acc.init = true;
  return(ACC_OK);
}  

int32_t acc_soft_reset(void)
{
  int32_t result;

  // Reset control register values to default
  result = acc_write_reg(LIS2DW12_CTRL2, 0x40);
  if(result != ACC_OK) { return(result); }
  LL_mDelay(1);

  return(ACC_OK);
}

acc_odr_t acc_get_odr(void)
{
  return(acc.odr);
}

/* Reading - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -*/

int32_t acc_read_cfg(acc_read_cfg_t * cfg)
{
  int32_t result;

  acc.read_cb = cfg->read_cb;

  // Modify CTRL register 6
  lis2dw12_ctrl6_t ctrl6;
  result = acc_read_reg(LIS2DW12_CTRL6, &ctrl6.byte);  
  if(result != ACC_OK) { return(result); }    
  ctrl6.bits.fds = cfg->src_hpf;     
  result = acc_write_reg(LIS2DW12_CTRL6, ctrl6.byte);
  return(result);
}

int32_t acc_read_start(void)
{
  int32_t result;
  
  if(acc.init == false) { return(ACC_ERR_INIT); }

  // configure data ready interrupt
  lis2dw12_ctrl4_t ctrl4;
  result = acc_read_reg(LIS2DW12_CTRL4, &ctrl4.byte);      
  ctrl4.bits.int1_drdy = 1;
  result |= acc_write_reg(LIS2DW12_CTRL4, ctrl4.byte);

  // start 1-shot conversion if that's the mode
  if(acc.pmode == ACC_PWR_1SHOT) {
    lis2dw12_ctrl3_t ctrl3;
    result = acc_read_reg(LIS2DW12_CTRL3, &ctrl3.byte);
    ctrl3.bits.slp_mode = 0x3;
    result |= acc_write_reg(LIS2DW12_CTRL3, ctrl3.byte);
  }
  acc.reading = true;
  return(result);
}

int32_t  acc_read_stop(void)
{
  int32_t result;
  
  if(acc.init == false) { return(ACC_ERR_INIT); }
  acc.reading = false;

  // Configure DRDY interrupt
  lis2dw12_ctrl4_t ctrl4;
  result = acc_read_reg(LIS2DW12_CTRL4, &ctrl4.byte); 
  if(result != ACC_OK) { return(result); }     
  ctrl4.bits.int1_drdy = 0;
  result = acc_write_reg(LIS2DW12_CTRL4, ctrl4.byte);

  return(result);
}
int16_t acc_read_x(void)
{
  return(acc.x);
}

int16_t acc_read_y(void)
{
  return(acc.y);
}

int16_t acc_read_z(void)
{
  return(acc.z);
}

int16_t acc_read_temp(void)
{
  return(acc.t);
}

/* orientation - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -*/

int32_t acc_orient_cfg(acc_orient_cfg_t * cfg)
{
  int32_t result;

  if(acc.init == false) { return(ACC_ERR_INIT); }

  acc.orient_cb = cfg->orient_cb;

  // Update TAP_THS_X
  lis2dw12_tap_ths_x_t tapthsx;
  result = acc_read_reg(LIS2DW12_TAP_THS_X, &tapthsx.byte);  
  if(result != ACC_OK) { return(result); }    
  tapthsx.bits._4d_en = cfg->orient_limit_4D ? 1 : 0;
  tapthsx.bits._6d_ths = cfg->orient_ths;
  result = acc_write_reg(LIS2DW12_TAP_THS_X, tapthsx.byte);
  return(result);
}

int32_t acc_orient_start(void)
{
  int32_t result;

  if(acc.init == false) { return(ACC_ERR_INIT); }

  // Configure 6D interrupt
  lis2dw12_ctrl4_t ctrl4;
  result = acc_read_reg(LIS2DW12_CTRL4, &ctrl4.byte);      
  ctrl4.bits.int1_6d = 1;
  result |= acc_write_reg(LIS2DW12_CTRL4, ctrl4.byte);
  return(result);

}

int32_t  acc_orient_stop(void)
{
  int32_t result;

  if(acc.init == false) { return(ACC_ERR_INIT); }
  
    // Configure 6D interrupt
  lis2dw12_ctrl4_t ctrl4;
  result = acc_read_reg(LIS2DW12_CTRL4, &ctrl4.byte);      
  ctrl4.bits.int1_6d = 0;
  result |= acc_write_reg(LIS2DW12_CTRL4, ctrl4.byte);
  return(result);
}

int32_t acc_read_orient(acc_orient_t * orientation)
{
  int32_t result;
  uint8_t data;
  
  if(acc.init == false) { return(ACC_ERR_INIT); }
  result = acc_read_reg(LIS2DW12_SIXD_SRC, &data);  
  if(result == ACC_OK) {
   *orientation = data & 0x3f;
  }
  else {
    *orientation = ACC_OR_BAD;
  }
  return(result);
}

/* tap detection - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -*/
int32_t acc_tap_cfg(acc_tap_cfg_t * cfg)
{
  int32_t result;

  if(acc.init == false) { return(ACC_ERR_INIT); }

  acc.tap_cb = cfg->tap_cb;
  acc.dtap = cfg->double_tap;

  // Update TAP_THS_X
  lis2dw12_tap_ths_x_t tapthsx;
  result = acc_read_reg(LIS2DW12_TAP_THS_X, &tapthsx.byte);  
  if(result != ACC_OK) { return(result); }    
  tapthsx.bits.tap_thsx = cfg->tap_ths_x > 0x1f ? 0x1f : cfg->tap_ths_x;
  result = acc_write_reg(LIS2DW12_TAP_THS_X, tapthsx.byte);
  if(result != ACC_OK) { return(result); }   

  // Update TAP_THS_Y
  lis2dw12_tap_ths_y_t tapthsy;
  result = acc_read_reg(LIS2DW12_TAP_THS_Y, &tapthsy.byte);  
  if(result != ACC_OK) { return(result); }    
  tapthsy.bits.tap_thsy = cfg->tap_ths_y > 0x1f ? 0x1f : cfg->tap_ths_y;
  tapthsy.bits.tap_prior = cfg->tap_prio;
  result = acc_write_reg(LIS2DW12_TAP_THS_Y, tapthsy.byte);
  if(result != ACC_OK) { return(result); } 

  // Update TAP_THS_Z
  lis2dw12_tap_ths_z_t tapthsz;
  result = acc_read_reg(LIS2DW12_TAP_THS_Z, &tapthsz.byte);  
  if(result != ACC_OK) { return(result); }    
  tapthsz.bits.tap_thsz = cfg->tap_ths_z > 0x1f ? 0x1f : cfg->tap_ths_z;
  tapthsz.bits.tap_x_en = cfg->tap_enable_x == true ? 1 : 0;
  tapthsz.bits.tap_y_en = cfg->tap_enable_y == true ? 1 : 0; 
  tapthsz.bits.tap_z_en = cfg->tap_enable_z == true ? 1 : 0;
  result = acc_write_reg(LIS2DW12_TAP_THS_Z, tapthsz.byte);
  if(result != ACC_OK) { return(result); } 

  if(acc.dtap == true) {
      // Allow double tap
      lis2dw12_wake_up_ths_t wuths;
      result = acc_read_reg(LIS2DW12_WAKE_UP_THS, &wuths.byte);      
      wuths.bits.single_double_tap = 1;
      result |= acc_write_reg(LIS2DW12_WAKE_UP_THS, wuths.byte);
      if(result != ACC_OK) { return(result); } 
  }

  lis2dw12_int_dur_t intdur;
  result = acc_read_reg(LIS2DW12_INT_DUR, &intdur.byte);  
  if(result != ACC_OK) { return(result); }    
  intdur.bits.shock = cfg->tap_shock_time > 3 ? 3 : cfg->tap_shock_time;
  intdur.bits.quiet = cfg->tap_quiet_time > 3 ? 3 : cfg->tap_quiet_time;
  intdur.bits.latency = cfg->tap_latency_time > 15 ? 15 : cfg->tap_latency_time;
  result = acc_write_reg(LIS2DW12_INT_DUR, intdur.byte);
  return(result);
}

int32_t acc_tap_start(void)
{
  int32_t result;

  if(acc.init == false) { return(ACC_ERR_INIT); }
  
  // Configure tap interrupts
  lis2dw12_ctrl4_t ctrl4;
  result = acc_read_reg(LIS2DW12_CTRL4, &ctrl4.byte);  
  if(acc.dtap == true) {
    ctrl4.bits.int1_tap = 1;
  }   
  else {
    ctrl4.bits.int1_single_tap = 1;
  }
  result |= acc_write_reg(LIS2DW12_CTRL4, ctrl4.byte);
  return(result);
}


int32_t  acc_tap_stop(void)
{
  int32_t result;

  if(acc.init == false) { return(ACC_ERR_INIT); }
  
  // Configure tap interrupt
  lis2dw12_ctrl4_t ctrl4;
  result = acc_read_reg(LIS2DW12_CTRL4, &ctrl4.byte);      
  ctrl4.bits.int1_single_tap = 0;
  ctrl4.bits.int1_tap = 0;
  result |= acc_write_reg(LIS2DW12_CTRL4, ctrl4.byte);
  return(result);
}

/* freefal detection - - - - - - - - - - - - - - - - - - - - - - - - - - - - -*/
int32_t acc_freefall_cfg(acc_ff_cfg_t * cfg)
{
  int32_t result;

  if(acc.init == false) { return(ACC_ERR_INIT); }

  acc.freefall_cb = cfg->freefall_cb;

  // Update Freefall
  lis2dw12_free_fall_t ffreg;
  result = acc_read_reg(LIS2DW12_FREE_FALL, &ffreg.byte);  
  if(result != ACC_OK) { return(result); }    
  ffreg.bits.ff_ths = cfg->threshold;
  ffreg.bits.ff_dur = cfg->duration > 7 ? 7 : cfg->duration;
  result = acc_write_reg(LIS2DW12_FREE_FALL, ffreg.byte);
  return(result); 
}

int32_t acc_freefall_start(void)
{
  int32_t result;

  if(acc.init == false) { return(ACC_ERR_INIT); }


  // Configure 6D interrupt
  lis2dw12_ctrl4_t ctrl4;
  result = acc_read_reg(LIS2DW12_CTRL4, &ctrl4.byte);      
  ctrl4.bits.int1_ff = 1;
  result |= acc_write_reg(LIS2DW12_CTRL4, ctrl4.byte);
  return(result);
}

int32_t  acc_freefall_stop(void)
{
  int32_t result;

  if(acc.init == false) { return(ACC_ERR_INIT); }

  // Configure 6D interrupt
  lis2dw12_ctrl4_t ctrl4;
  result = acc_read_reg(LIS2DW12_CTRL4, &ctrl4.byte);      
  ctrl4.bits.int1_ff = 0;
  result |= acc_write_reg(LIS2DW12_CTRL4, ctrl4.byte);
  return(result);
}

/* sleep/activity  - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -*/

int32_t acc_activity_cfg(acc_act_cfg_t * cfg)
{
  int32_t result;

  if(acc.init == false) { return(ACC_ERR_INIT); }

  acc.activity_cb = cfg->activity_cb;
  acc.activity_int_mode = cfg->int_mode;

  // Update Wake up threshold 
  lis2dw12_wake_up_ths_t wureg;
  result = acc_read_reg(LIS2DW12_WAKE_UP_THS, &wureg.byte);  
  if(result != ACC_OK) { return(result); } 
  wureg.bits.wk_ths = cfg->threshold > 63 ? 63 : cfg->threshold ;
  wureg.bits.sleep_on = 1;   
  result = acc_write_reg(LIS2DW12_WAKE_UP_THS, wureg.byte);
  if(result != ACC_OK) { return(result); } 

  lis2dw12_wake_up_dur_t durreg;
  result = acc_read_reg(LIS2DW12_WAKE_UP_DUR, &durreg.byte);  
  if(result != ACC_OK) { return(result); } 
  durreg.bits.sleep_dur = cfg->inact_dur > 15 ? 15 : cfg->inact_dur;
  durreg.bits.wake_dur = cfg->wake_dur > 3 ? 3 : cfg->wake_dur;
  durreg.bits.stationary = cfg->no_lp ? 1 : 0;
  result = acc_write_reg(LIS2DW12_WAKE_UP_DUR, wureg.byte);
  return(result);  
}

int32_t acc_activity_start(void)
{
  int32_t result;

  if(acc.init == false) { return(ACC_ERR_INIT); }

  if( acc.activity_int_mode == ACC_ACT_INT_CHANGE) {
    lis2dw12_ctrl5_t ctrl5;
    result = acc_read_reg(LIS2DW12_CTRL5, &ctrl5.byte); 
    ctrl5.bits.int2_sleep_chg = 1; 
    result |= acc_write_reg(LIS2DW12_CTRL5, ctrl5.byte);
  }
  else {
    lis2dw12_ctrl4_t ctrl4;
    result = acc_read_reg(LIS2DW12_CTRL4, &ctrl4.byte);    
    ctrl4.bits.int1_wu = 1;
    result |= acc_write_reg(LIS2DW12_CTRL4, ctrl4.byte);
  }
  return(result);
}

int32_t  acc_activity_stop(void)
{
  int32_t result;

  if(acc.init == false) { return(ACC_ERR_INIT); }

  if( acc.activity_int_mode == ACC_ACT_INT_CHANGE) {
    lis2dw12_ctrl5_t ctrl5;
    result = acc_read_reg(LIS2DW12_CTRL5, &ctrl5.byte); 
    ctrl5.bits.int2_sleep_chg = 0; 
    result |= acc_write_reg(LIS2DW12_CTRL5, ctrl5.byte);
  }
  else {
    lis2dw12_ctrl4_t ctrl4;
    result = acc_read_reg(LIS2DW12_CTRL4, &ctrl4.byte);    
    ctrl4.bits.int1_wu = 0;
    result |= acc_write_reg(LIS2DW12_CTRL4, ctrl4.byte);
  }

  return(result);
}

int32_t acc_read_activity(acc_act_t * activity)
{
  int32_t result;
  lis2dw12_wake_up_src_t wu_src;

  if(acc.init == false) { return(ACC_ERR_INIT); }
  result = acc_read_reg(LIS2DW12_WAKE_UP_SRC, &wu_src.byte); 
  if(result == ACC_OK) {
    if(wu_src.bits.wu_ia == 1) {
      *activity = ACC_ACT_WAKE;
    }
    else if(wu_src.bits.sleep_state_ia == 1) {
      *activity = ACC_ACT_SLEEP;
    }
   
    else {
      *activity = ACC_ACT_INACTIVE;
    }
  }
  else {
    *activity = ACC_ACT_INACTIVE;
  }
  return(result);
}

/* Interrupt Service Routines ------------------------------------------------*/

void acc_ISR(void) 
{
  uint32_t result;
  union { 
    uint8_t byte[2];
    uint16_t word;
  } data;
 
  // read the status register
  lis2dw12_status_dup_t stat;
  lis2dw12_all_int_src_t src;
  acc_read_reg(LIS2DW12_STATUS_DUP, &stat.byte);   

  if((stat.bits.drdy) && (acc.reading == true)) {          // read data 
    result  = acc_read_reg(LIS2DW12_OUT_X_L, &data.byte[0]);  
    result |= acc_read_reg(LIS2DW12_OUT_X_H, &data.byte[1]); 
    acc.x = data.word;
    result |= acc_read_reg(LIS2DW12_OUT_Y_L, &data.byte[0]);  
    result |= acc_read_reg(LIS2DW12_OUT_Y_H, &data.byte[1]); 
    acc.y = data.word; 
    result |= acc_read_reg(LIS2DW12_OUT_Z_L, &data.byte[0]);  
    result |= acc_read_reg(LIS2DW12_OUT_Z_H, &data.byte[1]); 
    acc.z = data.word;
    result |= acc_read_reg(LIS2DW12_OUT_T_L, &data.byte[0]);  
    result |= acc_read_reg(LIS2DW12_OUT_T_H, &data.byte[1]); 
    acc.t = data.word;  
    if(acc.read_cb) { acc.read_cb(result == ACC_OK ? true : false); }
  }
  result = acc_read_reg(LIS2DW12_ALL_INT_SRC, &src.byte);

  if(src.bits._6d_ia) {        // orientation 
    result = acc_read_reg(LIS2DW12_SIXD_SRC, &data.byte[0]);  
    if(result != ACC_OK) {
      data.byte[0] = ACC_OR_BAD;
    }
    if(acc.orient_cb) { acc.orient_cb(data.byte[0] & 0x3f); }
  }
  if(src.bits.single_tap || src.bits.double_tap) {    // tap
    result = acc_read_reg(LIS2DW12_TAP_SRC, &data.byte[0]);  
    if(result != ACC_OK) {
      data.byte[0] = ACC_TAP_BAD;
    }
    if(acc.tap_cb) { acc.tap_cb(data.byte[0] & 0x0f); }
  }

   if(src.bits.ff_ia) {    // free-fall
    if(acc.freefall_cb) { acc.freefall_cb(); }
  } 
  if(src.bits.sleep_change_ia) {    // activity

    if(acc.activity_cb) { acc.activity_cb(stat.bits.sleep_state_ia == 1 ? false : true ); }
  } 

  if(src.bits.wu_ia) { // wake up 
    if(acc.activity_cb) { acc.activity_cb(true); }
  }
}



/* End acc_LIS2DW12 */
