/* *****************************************************************************
 * File:      smartprobe.c
 * Date:      26 Dec 2024
 * Author:    Andrew Levido
 *
 * Copyright 2025 Imbrius Pty Ltd - All Rights Reserved
 *
 ******************************************************************************/

/* Includes ------------------------------------------------------------------*/
#include "smartprobe.h"
#include "beep.h"
#include "oled_X30654.h"
#include "acc_LIS2DW12.h"
#include "resources.h"
#include "adc.h"
#include "comparator.h"

/* Defines -------------------------------------------------------------------*/
extern void SystemClock_Config(void);

/* Typedefs & Enumerations ---------------------------------------------------*/
/* Operating Modes */
typedef enum {SP_MODE_VOLTAGE, SP_MODE_CONTINUITY} sp_mode_t;

/* Private function declarations ---------------------------------------------*/

/* Asynchronous callback functions */
void sp_tap_callback(acc_tap_t tap);
void sp_adc_callback(int32_t v_hi, int32_t vlo);

 /* Main loop helper Functions */
 void sp_init(sp_mode_t mode);    // Initialise the application & drivers
 bool sp_is_acc_asleep(void);     // Check if accelerometer is asleep
 void sp_update_display(void);    // Update the display
 void sp_set_orientation(void);   // Get orientation & flip display if needed
                                  
/* Functions to display stuff on the screen */
 void sp_display_value(int32_t value, bool hi_range);
 void sp_set_mode(sp_mode_t mode);

/* Functions to power down & up the peripherals when entering & leaving sleep */
 void sp_power_down(void);
 void sp_power_up(void);

/* Global Variables ----------------------------------------------------------*/

/* Application state */
static struct {
  volatile sp_mode_t mode;                     // Application mode
  volatile oled_orientation_t display_invert;  // Display orientation
  volatile bool cont_beep_on;                  // Continuity beeping flag
  volatile bool adc_update;                    // New ADC data flag
  volatile int32_t v_hi;                       // High voltage reading (mV)
  volatile int32_t v_lo;                       // Low voltage reading (mV)
} sp;

/* Configuration data for accelerometer system */
acc_init_cfg_t acc_config_struct = {
  .fullscale = ACC_FS_2G,             // +/- 2g full scale
  .pwrmode = ACC_PWR_HI,              // Normal power mode
  .lpmode = ACC_LP1,                  // Low power mode
  .odr = ACC_ODR_400HZ,               // 400Hz update rate (t = 2.5ms)
  .bandwidth = ACC_BW_2,              // Bypass LPF2, HPF ODR/2 
  .low_noise = 1,                     // Low noise
};

/* Configuration data for accelerometer orientation detection */
acc_orient_cfg_t acc_orient_cfg_struct = {
  .or_src = ACC_ORIENT_LPF,           // Orientation source lowpass filter
  .orient_ths = ACC_OR_THS_70,        // Orientation threshod 70 degrees
  .orient_limit_4D = true,            // Limit orientation to XY Axis
  .orient_cb = 0                      // Register callback function
};

/* Configuration data for accelerometer tap detection       
 * A tap is detected if the HPF acceleration exceeds the threshold and returns 
 * under it within the shock time. In our case ODR = 400Hz so shck time = 50ms */
acc_tap_cfg_t acc_tap_cfg_struct = {
  .tap_enable_x = true,               // Enable x-axis tap detection
  .tap_enable_y = false,              // Disable y-axis tap detection
  .tap_enable_z = false,              // Enable z-Axis tap detection
  .tap_latency_time = 0,              // 16/ODR + N 32/ODR 
  .tap_quiet_time = 0,                // 2/ODR + N 4/0DR 
  .tap_shock_time = 2,                // 4/ODR + N 8/ODR 
  .tap_prio = ACC_TAP_PRIO_XYZ,       // X-axis has priority
  .tap_ths_x = 26,                    // Tap threshold (1LSB is FS/32)
  .tap_ths_y = 31,                    // Tap threshold (1LSB is FS/32)
  .tap_ths_z = 31,                    // Tap threshold (1LSB is FS/32)
  .double_tap = false,                // single tap only
  .tap_cb = sp_tap_callback              // Register callback function
};

/* Configuration data for accelerometer inactivity/wake detection
 * Inactivity is detected if the acceleration remains below the threshold 
 * for the inactivity duration. Wake is detected if the acceleration exceeds 
 * the threshold for the wake duration. 
 * In HP mode ODR is 400Hz (t = 2.5ms) 2.5ms * 152 * 15 = 5.7s
 * In LP Mode ODR is 200Hz (t = 5.0ms) 5.0ms * 1 * 3 = 15ms */
acc_act_cfg_t acc_act_cfg_struct = {
  .act_src = ACC_ACT_HPF,             // Wakeup/activity source HPF
  .threshold = 6,                     // 0-63 in units of 1/64 of full scale
  .inact_dur = 15,                    // 0-15 in units of 152/ODR
  .wake_dur = 3,                      // 0-3 in units of 1/ODR
  .no_lp = false,                     // Go into LP when inactive
  .int_mode = ACC_ACT_INT_WAKEUP,     // Interrupt on wake
  .activity_cb = 0                    // Register callback function
};

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

void sp_main_loop(void)
{
  /* Initialise application state */
  sp.mode = SP_MODE_VOLTAGE;
  sp.adc_update = false;
  sp.cont_beep_on = false;
  sp.display_invert = OLED_NORMAL;
  sp.v_hi = 0;
  sp.v_lo = 0;

  /* Main Loop */
  while(1) {
    sp_init(sp.mode);                         // Initialise peripherals 

    while(sp_is_acc_asleep() == false) {      // Loop while awake
      if(sp.adc_update == true) {             // While awake update display 
        sp.adc_update = false;                // whenever the flag is set & 
        sp_update_display();                  // clear the flag 
      }
    }
    // The accelerometer is asleep so put micro to sleep until it wakes up
    SET_BIT(PWR->CR, PWR_CR_CWUF);            // Clear wakeup flag (2 cycles)
    CLEAR_BIT(PWR->CR, PWR_CR_PDDS);          // Enter stop mode on deepsleep
    SET_BIT(PWR->CR, PWR_CR_LPSDSR);          // Low power regulator mode
    SET_BIT(PWR->CR, PWR_CR_ULP);             // Disable VREFINT in stop mode
    SET_BIT(RCC->CFGR, RCC_CFGR_STOPWUCK);    // HS16 oscillator on waking
    SET_BIT(SCB->SCR, SCB_SCR_SLEEPDEEP_Msk); // Enable deep sleep mode

    __disable_irq();                          // Disable interrupts
    sp_power_down();                          // Power down all peripherals   
    __WFI();                                  // Enter sleep mode
    // Sleep until interrupt
    sp_power_up();
    __enable_irq();                           // Enable interrupts
  }
}


void sp_comp_1ms_tick(void)
{
   if(sp.mode == SP_MODE_CONTINUITY) {
    /* if in continuity mode, reads comparator & sounds beep if voltage low */
    if(comp_read() == COMP_VIN_LOW) {
      if(sp.cont_beep_on == false) {
        sp.cont_beep_on = true;
        beep(A6, BEEP_FOREVER); 
      }
    }
    else {
      /* Stop beep if voltage high */
      if(sp.cont_beep_on == true) {
        sp.cont_beep_on = false;
        beep_stop();
      }
    }
   }
}

void sp_fatal_error(uint8_t code)
{
  if(code < 4) {
    beep(A3, BEEP_LONG);
    oled_put_buffer(SP_ERR_X0_OFFS, SP_ERR_PAGE, SP_ERR_WIDTH, SP_ERR_HEIGHT, (uint8_t*)&error_msg[code][0][0]);
    oled_update();

    while(1);
  }
}

/* Private Functions ---------------------------------------------------------*/
void sp_tap_callback(acc_tap_t tap)
{
    switch(tap) {

    case ACC_TAP_XP:
    case ACC_TAP_XN:
      if(sp.mode == SP_MODE_VOLTAGE) {
        sp.mode = SP_MODE_CONTINUITY;
        beep(A5, BEEP_BLIP);
        sp_set_mode(SP_MODE_CONTINUITY);
      }
      else if (sp.mode == SP_MODE_CONTINUITY) {
        sp.mode = SP_MODE_VOLTAGE;
        beep(C5, BEEP_BLIP);
        sp_set_mode(SP_MODE_VOLTAGE);
      }
      break;
    default:
      break;
  }
}

void sp_adc_callback(int32_t v_hi, int32_t v_lo)
{
  sp.v_hi = v_hi;
  sp.v_lo = v_lo;
  sp.adc_update = true;
}

bool sp_is_acc_asleep(void)
{
  acc_act_t activity;

  if(acc_read_activity(&activity) != ACC_OK) {
    return(false);
  }
  if(activity == ACC_ACT_SLEEP) {
    return(true);
  }
  else {
    return(false);
  }
}

void sp_init(sp_mode_t mode)
{
  LL_SYSTICK_EnableIT();

  /* Power up boost converter */
  LL_GPIO_SetOutputPin(PWR_ON_GPIO_Port, PWR_ON_Pin);

  /* Power I2C Pullups */
  LL_GPIO_SetOutputPin(I2C_PU_GPIO_Port, I2C_PU_Pin);

  /* Wait for DC-DC converter to power up */
  LL_mDelay(100);

  /* Startup beep and wait so it won't be interrupted */
  beep(A6, BEEP_MEDIUM);
  LL_mDelay(BEEP_MEDIUM);

  /* Initialise accelerometer */
  if(acc_init(&acc_config_struct) != ACC_OK) {
    sp_fatal_error(SP_ERR_ACC);
  }

  /* Initialise orientation */
  if(acc_orient_cfg(&acc_orient_cfg_struct) != ACC_OK) {
    sp_fatal_error(SP_ERR_ACC);
  }
  
  /* Wait for it to be ready then read & set power-up orientation */
  LL_mDelay(10);
  sp_set_orientation();
  
  /* Initialise OLED with appropriate orientation */
  if(oled_init(sp.display_invert) != OLED_OK) {
    sp_fatal_error(SP_ERR_OLED);
  }
  /* Set initial mode & update display */
  sp_set_mode(mode);
  oled_update();

  /* Initialise & start tap detection */
  if(acc_tap_cfg(&acc_tap_cfg_struct) != ACC_OK) {
    sp_fatal_error(SP_ERR_ACC);
  }
  /* Enable tap interrupt / callback */
  if(acc_tap_start() != ACC_OK) {
    sp_fatal_error(SP_ERR_ACC);
  }

  /* Start ADC & register callback */
  adc_start(sp_adc_callback);

  /* Initialise comparator & set threshold */
  comp_set_threshold(COMP_THRESH_LOW);

  /* Initialise accelerometer sleep/wake function */
  if(acc_activity_cfg(&acc_act_cfg_struct) != ACC_OK) {
    sp_fatal_error(SP_ERR_ACC);
  }
  /* Start wake interrupt */
  if(acc_activity_start() != ACC_OK) {
    sp_fatal_error(SP_ERR_ACC);
  }
}

void sp_update_display(void)
{
  int32_t v_hi, v_lo;
  
  /* Scale voltages according to input circuit gain */
  v_hi = (sp.v_hi * SP_HIGH_GAIN) / 10000;
  v_lo = (sp.v_lo * SP_LOW_GAIN) / 10000;

  /* Detect overvoltage: Beep and flash warning */
  if((v_hi > SP_MAX_VOLTAGE) || (v_hi < -SP_MAX_VOLTAGE)) {
   // beep(C3, BEEP_LONG);
    oled_put_buffer(SP_OVERV_X0, SP_OVERV_PAGE, SP_OVERV_WIDTH, SP_OVERV_HEIGHT, (uint8_t*)&overvoltage_msg);
  }
  else {
    oled_clear(SP_OVERV_X0, SP_OVERV_PAGE, SP_OVERV_WIDTH, SP_OVERV_HEIGHT);
  }
  /* clear digit area */
  oled_clear(SP_SIGN_X0, SP_DIGIT_PAGE, SP_VOLTAGE_DISP_WIDTH, SP_DIGIT_HEIGHT);

  /* Detect open circuit in continuity mode & display "OPEN" */
  if((sp.mode == SP_MODE_CONTINUITY) && (v_lo > SP_OPEN_THRESHOLD)){
    oled_put_buffer(SP_OPEN_X0, SP_OPEN_PAGE, SP_OPEN_WIDTH, SP_OPEN_HEIGHT, (uint8_t*)&open_msg);
  }
   /* Otherwise display voltage */
  else {
    if((v_hi >= SP_HI_V_RANGE_THRESH) || (v_hi <= -SP_HI_V_RANGE_THRESH)) {
      sp_display_value(v_hi, true);
    }
    else {
      sp_display_value(v_lo, false);
    }
  } 
  /* Check orientation and update display if it has changed */
  sp_set_orientation();

  /* update display */
  if(oled_update() != OLED_OK) {
    sp_fatal_error(SP_ERR_OLED);
  }
}

void sp_display_value(int32_t value, bool hi_range)
{

  /* Display sign */
  if(value >= 0) {
    oled_put_buffer(SP_SIGN_X0, SP_SIGN_PAGE, SP_SIGN_WIDTH, SP_SIGN_HEIGHT, (uint8_t*)plus_glyph);
  }
  else {
    oled_put_buffer(SP_SIGN_X0, SP_SIGN_PAGE, SP_SIGN_WIDTH, SP_SIGN_HEIGHT, (uint8_t*)minus_glyph);
    value = -value;
  }

  /* Overload */
  if(value > 99990) { value = 99990; }

  if((hi_range == true) || (value > 9999)) {      /* Display ±xx.xx V */
    value = (value + 5)/10;
    oled_put_buffer(SP_DIGIT1_X0, SP_DIGIT_PAGE, SP_DIGIT_WIDTH, SP_DIGIT_HEIGHT, (uint8_t*)&digit_glyph[value%10]);
    value = value/10;
    oled_put_buffer(SP_DIGIT2_X0, SP_DIGIT_PAGE, SP_DIGIT_WIDTH, SP_DIGIT_HEIGHT, (uint8_t*)&digit_glyph[value%10]);
    oled_put_buffer(SP_DP_HI_X0, SP_DP_PAGE, SP_DP_WIDTH, SP_DP_HEIGHT, (uint8_t*)dp_glyph);
    value = value/10;
    oled_put_buffer(SP_DIGIT3_HI_X0, SP_DIGIT_PAGE, SP_DIGIT_WIDTH, SP_DIGIT_HEIGHT, (uint8_t*)&digit_glyph[value%10]);
    value = value/10;
    oled_put_buffer(SP_DIGIT4_X0, SP_DIGIT_PAGE, SP_DIGIT_WIDTH, SP_DIGIT_HEIGHT, (uint8_t*)&digit_glyph[value%10]);
  }
  else {                                        /* Display ±x.xxx V */
    oled_put_buffer(SP_DIGIT1_X0, SP_DIGIT_PAGE, SP_DIGIT_WIDTH, SP_DIGIT_HEIGHT, (uint8_t*)&digit_glyph[value%10]);
    value = value/10;
    oled_put_buffer(SP_DIGIT2_X0, SP_DIGIT_PAGE, SP_DIGIT_WIDTH, SP_DIGIT_HEIGHT, (uint8_t*)&digit_glyph[value%10]);
    oled_put_buffer(SP_DP_LO_X0, SP_DP_PAGE, SP_DP_WIDTH, SP_DP_HEIGHT, (uint8_t*)dp_glyph);
    value = value/10;
    oled_put_buffer(SP_DIGIT3_LO_X0, SP_DIGIT_PAGE, SP_DIGIT_WIDTH, SP_DIGIT_HEIGHT, (uint8_t*)&digit_glyph[value%10]);
    value = value/10;
    oled_put_buffer(SP_DIGIT4_X0, SP_DIGIT_PAGE, SP_DIGIT_WIDTH, SP_DIGIT_HEIGHT, (uint8_t*)&digit_glyph[value%10]);
  }
  oled_put_buffer(SP_UNITS_X0, SP_DIGIT_PAGE, SP_DIGIT_WIDTH, SP_DIGIT_HEIGHT, (uint8_t*)units_glyph);
}

void sp_set_mode(sp_mode_t mode)
{
  sp.mode = mode;
  switch(mode) {
  case SP_MODE_VOLTAGE:
    LL_GPIO_ResetOutputPin(CONT_OUT_GPIO_Port, CONT_OUT_Pin);
    LL_GPIO_SetOutputPin(SET_OFFS_GPIO_Port, SET_OFFS_Pin);
    oled_put_buffer(SP_MODE_X0, SP_MODE_PAGE, SP_MODE_WIDTH, SP_MODE_HEIGHT, (uint8_t*)&voltage_msg);
    break;
  case SP_MODE_CONTINUITY:
    LL_GPIO_SetOutputPin(CONT_OUT_GPIO_Port, CONT_OUT_Pin);
    LL_GPIO_ResetOutputPin(SET_OFFS_GPIO_Port, SET_OFFS_Pin);
    oled_put_buffer(SP_MODE_X0, SP_MODE_PAGE, SP_MODE_WIDTH, SP_MODE_HEIGHT, (uint8_t*)&continuity_msg);
    break;
  default:
  break;
  }
  /* Update LCD and wait for things to stabilise */
  oled_update();
  LL_mDelay(100);
}

void sp_set_orientation(void)
{
  acc_orient_t orientation;

  if(acc_read_orient(&orientation) != ACC_OK) {
    sp_fatal_error(SP_ERR_ACC);
  }
  if(orientation == ACC_OR_XU) { 
    if(sp.display_invert == OLED_NORMAL) {
      oled_flip(OLED_FLIP);
      sp.display_invert = OLED_FLIP;
    }
  }
  else if(orientation == ACC_OR_XD) { 
    if(sp.display_invert == OLED_FLIP) {
      oled_flip(OLED_NORMAL);
      sp.display_invert = OLED_NORMAL;
    }
  }
}
void sp_power_down(void)
{
  /* Turn OLED off then set Reset pin to analog */
  oled_off();
  LL_GPIO_SetPinMode(DSP_RST_GPIO_Port, DSP_RST_Pin, LL_GPIO_MODE_ANALOG);

  /* Stop ADC & disable interrupts */
  adc_stop();
  
  /* Disable tap interrupts (but leave acivity active) */
  acc_tap_stop(); 

  CLEAR_BIT(SYSCFG->CFGR3, SYSCFG_CFGR3_EN_VREFINT);
  CLEAR_BIT(SYSCFG->CFGR3, SYSCFG_CFGR3_ENBUFLP_VREFINT_COMP);
  CLEAR_BIT(SYSCFG->CFGR3, SYSCFG_CFGR3_ENBUF_VREFINT_ADC);

  LL_ADC_DisableInternalRegulator(ADC1); 

  /* Turn beeper off an set PWM pin to analog */
  beep_stop();
  LL_GPIO_SetPinMode(BEEPER_GPIO_Port, BEEPER_Pin, LL_GPIO_MODE_ANALOG);

  /* Turn off I2C pull ups and set pins to analog */
  LL_I2C_Disable(I2C1); 
  LL_GPIO_SetPinMode(I2C_PU_GPIO_Port, I2C_PU_Pin, LL_GPIO_MODE_ANALOG);
  LL_GPIO_SetPinMode(SDA_GPIO_Port, SDA_Pin, LL_GPIO_MODE_ANALOG);
  LL_GPIO_SetPinMode(SCL_GPIO_Port, SCL_Pin, LL_GPIO_MODE_ANALOG);
 
  /* Set continuity and offset pins to analog */
  LL_GPIO_SetPinMode(CONT_OUT_GPIO_Port, CONT_OUT_Pin, LL_GPIO_MODE_ANALOG);
  LL_GPIO_SetPinMode(SET_OFFS_GPIO_Port, SET_OFFS_Pin, LL_GPIO_MODE_ANALOG);

  /* Turn off DC-DC converter, then set pin analog */
  LL_GPIO_ResetOutputPin(PWR_ON_GPIO_Port, PWR_ON_Pin);
  LL_GPIO_SetPinMode(PWR_ON_GPIO_Port, PWR_ON_Pin, LL_GPIO_MODE_ANALOG);

  /* Turn off interrupts that may cause exit from sleep */
  LL_SYSTICK_DisableIT();
}

void sp_power_up(void)
{
  SystemClock_Config();

  /* Turn on DC-DC Pin - It should be off */
  LL_GPIO_SetPinMode(PWR_ON_GPIO_Port, PWR_ON_Pin, LL_GPIO_MODE_OUTPUT);

  /* return continuity and offset pins to outputs - they should remember state*/
  LL_GPIO_SetPinMode(CONT_OUT_GPIO_Port, CONT_OUT_Pin, LL_GPIO_MODE_OUTPUT);
  LL_GPIO_SetPinMode(SET_OFFS_GPIO_Port, SET_OFFS_Pin, LL_GPIO_MODE_OUTPUT);

  /* Turn on I2C pullup and enable pins */
  LL_GPIO_SetPinMode(I2C_PU_GPIO_Port, I2C_PU_Pin, LL_GPIO_MODE_OUTPUT);
  LL_GPIO_SetPinMode(SDA_GPIO_Port, SDA_Pin, LL_GPIO_MODE_ALTERNATE);
  LL_GPIO_SetPinMode(SCL_GPIO_Port, SCL_Pin, LL_GPIO_MODE_ALTERNATE);
  LL_I2C_Enable(I2C1); 
  
  SET_BIT(SYSCFG->CFGR3, SYSCFG_CFGR3_EN_VREFINT);
  SET_BIT(SYSCFG->CFGR3, SYSCFG_CFGR3_ENBUFLP_VREFINT_COMP);
  SET_BIT(SYSCFG->CFGR3, SYSCFG_CFGR3_ENBUF_VREFINT_ADC);

  LL_ADC_EnableInternalRegulator(ADC1);
  /* Tun on Beeeper pin */
  LL_GPIO_SetPinMode(BEEPER_GPIO_Port, BEEPER_Pin, LL_GPIO_MODE_ALTERNATE);
  
  /* Turn OLED Reset pin on*/
  LL_GPIO_SetPinMode(DSP_RST_GPIO_Port, DSP_RST_Pin, LL_GPIO_MODE_OUTPUT);
}
/* End smartprobe */