/* =============
Copyright (c) 2024, STMicroelectronics

All rights reserved.

Redistribution and use in source and binary forms, with or without modification, are permitted provided that
the following conditions are met:

* Redistributions of source code must retain the above copyright notice, this list of conditions and the
  following disclaimer.

* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the
  following disclaimer in the documentation and/or other materials provided with the distribution.

* Neither the name of the copyright holders nor the names of its contributors may be used to endorse or promote
  products derived from this software without specific prior written permission.

*THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER / OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE
USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.*
*/

/* If you want to use NEAI functions please, include NEAI library
 * in your Arduino libraries then, uncomment NEAI parts in the following code
 */

/* Libraries part */
#include "Wire.h"
#include <LSM6DSOSensor.h>
// #include <NanoEdgeAI.h>

/* Macros definitions */
#define SERIAL_BAUD_RATE  115200

/* Define the data type you want to collect */
#define ACCELEROMETER // Could be either ACCELEROMETER or GYROSCOPE

/* Sensor data rates.
 * You can choose from the following values for both accel & gyro:
 * 12.5f, 26.0f, 52.0f, 104.0f, 208.0f, 417.0f, 833.0f & 1667.0f.
  */
#define SENSOR_DATA_RATE	1667.0f

/* Sensor ranges.
 * You can choose from:
 * 2, 4, 8 & 16 for accelerometer.
 * 125, 250, 500, 1000 & 2000 for gyroscope.
 */
#define SENSOR_RANGE	8

/* NanoEdgeAI defines part
 * NEAI_MODE = 1: NanoEdgeAI functions = AI Mode.
 * NEAI_MODE = 0: Datalogging mode.
 */
#define NEAI_MODE 0
#define SENSOR_SAMPLES	512
#define AXIS  3

/* Sensor object declaration using I2C */
LSM6DSOSensor AccGyr(&Wire);

/* Global variables definitions */
static uint8_t drdy = 0;
static uint16_t neai_ptr = 0;
static int32_t sensor_values[3];
static float neai_buffer[SENSOR_SAMPLES * AXIS] = {0.0};

/* NEAI library variables */
// static uint8_t neai_code = 0, similarity = 0;
// static uint16_t neai_cnt = 0;

/* Initialization function: In this function,
 *  code runs only once at boot / reset.
 */
void setup() {
  /* Init serial at baud rate 115200 */
  Serial.begin(SERIAL_BAUD_RATE);

  /* I2C workaround: Sometimes, on some boards,
   * I2C get stuck after software reboot, reset so,
   * to avoid this, we toggle I2C clock pin at boot.
   */
  pinMode(SCL, OUTPUT);
  for (uint8_t i = 0; i < 20; i++) {
    digitalWrite(SCL, !digitalRead(SCL));
    delay(1);
  }
  delay(100);

  Wire.begin();
  AccGyr.begin();
#ifdef ACCELEROMETER
    AccGyr.Enable_X();
    AccGyr.Disable_G();
    AccGyr.Set_X_ODR(SENSOR_DATA_RATE);
    AccGyr.Set_X_FS(SENSOR_RANGE);
#else
    AccGyr.Enable_G();
    AccGyr.Disable_X();
    AccGyr.Set_G_ODR(SENSOR_DATA_RATE);
    AccGyr.Set_G_FS(SENSOR_RANGE);
#endif

  /* Initialize NanoEdgeAI AI */
  // neai_code = neai_anomalydetection_init();
  // if(neai_code != NEAI_OK) {
  //   Serial.print("Not supported board.\n");
  // }
}

/* Main function: Code run indefinitely */
void loop() {
  /* Get data in the neai buffer */
  while(neai_ptr < SENSOR_SAMPLES) {
    /* Check if new data if available */
#ifdef ACCELEROMETER
      AccGyr.Get_X_DRDY_Status(&drdy);
#else
      AccGyr.Get_G_DRDY_Status(&drdy);
#endif
    if(drdy) {
      /* If new data is available we read it ! */
#ifdef ACCELEROMETER
        AccGyr.Get_X_Axes(sensor_values);
#else
        AccGyr.Get_G_Axes(sensor_values);
#endif
      /* Fill neai buffer with new accel data */
      neai_buffer[AXIS * neai_ptr] = (float) sensor_values[0];
      neai_buffer[(AXIS * neai_ptr) + 1] = (float) sensor_values[1];
      neai_buffer[(AXIS * neai_ptr) + 2] = (float) sensor_values[2];
      /* Increment neai pointer */
      neai_ptr++;
    }
  }
  /* Reset pointer */
  neai_ptr = 0;
  /* Depending on NEAI_MODE value, run NanoEdge AI functions
   * or print accelerometer data to the serial (datalogging)
   */
  // if(NEAI_MODE) {
  //   if(neai_cnt < MINIMUM_ITERATION_CALLS_FOR_EFFICIENT_LEARNING) {
  //     neai_anomalydetection_learn(neai_buffer);
  //     Serial.print((String)"Learn: " + neai_cnt + "/" + MINIMUM_ITERATION_CALLS_FOR_EFFICIENT_LEARNING + ".\n");
  //     neai_cnt++;
  //   }
  //   else {
  //     neai_anomalydetection_detect(neai_buffer, &similarity);
  //     Serial.print((String)"Detect: " + similarity + "/100.\n");
  //   }
  // }
  // else {
    /* Print the whole buffer to the serial */
    for(uint16_t i = 0; i < AXIS * SENSOR_SAMPLES; i++) {
      Serial.print((String)neai_buffer[i] + " ");
    }
    Serial.print("\n");
  // }

  /* Clean neai buffer */
  memset(neai_buffer, 0.0, AXIS * SENSOR_SAMPLES * sizeof(float));
}