/* =============
  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 <SparkFun_VL53L5CX_Library.h>
#include "NanoEdgeAI.h"
#include "knowledge.h"
#include <Keyboard.h>

/* Macros definitions */
#define SERIAL_BAUD_RATE          115200

/* Frame resolution: 4x4 or 8x8 */
#define SENSOR_FRAME_RESOLUTION  64

/* Number of successive frames to be collected */
#define SENSOR_FRAMES    1

/* Sensor data rate */
#define SENSOR_DATA_RATE  15

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

/* Depending on your target, you need to adapt
   the i2c connection. For example, you have to use
   "Wire" with Arduino Uno R4 Wifi but, "Wire1" with
   the Arduino Giga R1 board. Check your hardware !
*/
#define i2c_comm Wire

/* In this example, we use I2C connection */
SparkFun_VL53L5CX myImager;
VL53L5CX_ResultsData measurementData;

/* Global variables definitions */
static uint8_t neai_code = 0;
static uint16_t neai_ptr = 0;
static float neai_buffer[SENSOR_FRAME_RESOLUTION * SENSOR_FRAMES] = {0.0};

/* NEAI library variables */
uint16_t id_class = 0; // Point to id class (see argument of neai_classification fct)
float input_user_buffer[DATA_INPUT_USER * AXIS_NUMBER]; // Buffer of input values
float output_class_buffer[CLASS_NUMBER]; // Buffer of class probabilities
const char *id2class[CLASS_NUMBER + 1] = { // Buffer for mapping class id to class name
  "unknown",
  "dm_jumpcenter",
  "dm_jumpleft",
  "dm_jumpright",
  "dm_left",
  "dm_right",
  "dm_stand",
};
/* 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);

  /* Init I2C connection between board & sensor */
  i2c_comm.begin();
  i2c_comm.setClock(400000);
  if (myImager.begin() == false) {
    Serial.print("Sensor not found - check your wiring. Freezing.\n");
    while (1);
  }

  /* Init VL53L5CX sensor settings: resolution & data rate */
  myImager.setResolution(SENSOR_FRAME_RESOLUTION);
  myImager.setRangingFrequency(SENSOR_DATA_RATE);
  myImager.startRanging();

  Keyboard.begin();

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

  delay(10000);
}

/* Main function: Code run indefinitely */
void loop() {
  while (neai_ptr < SENSOR_FRAMES) {
    /* Check if new data if available */
    if (myImager.isDataReady() == true) {
      /* If new data is available we read it ! */
      myImager.getRangingData(&measurementData);
      /* Get data in the neai buffer */
      for (uint16_t i = 0; i < SENSOR_FRAME_RESOLUTION; i++) {
        neai_buffer[i + (SENSOR_FRAME_RESOLUTION * neai_ptr)] = (float) measurementData.distance_mm[i];
      }
      neai_ptr++;
    }
  }


  /* Depending on NEAI_MODE value, run NanoEdge AI functions
     or print accelerometer data to the serial (datalogging)
  */
  if (NEAI_MODE) {
    neai_classification(neai_buffer, output_class_buffer, &id_class);
    Serial.print((String)"Class: " + id2class[id_class] + ".\n");

    switch (id_class) {
      case 6:
        //stand
        Keyboard.releaseAll();
        break;
      case 4:
        //move left
        Keyboard.press('q');
        break;
      case 5:
        //move right
        Keyboard.press('d');
        break;
      case 1:
        //jump
        Keyboard.press(0x20);
        break;
      case 2:
        //jump left
        Keyboard.press(0x20);
        Keyboard.press('q');
        break;
      case 3:
        //jump right
        Keyboard.press(0x20);
        Keyboard.press('d');
        break;
    }
    delay(500);
    Keyboard.releaseAll();
  }
  else {
    /* Print the whole buffer to the serial */
    for (uint16_t i = 0; i < (uint16_t) (SENSOR_FRAME_RESOLUTION * SENSOR_FRAMES); i++) {
      Serial.print((String) neai_buffer[i] + " ");
    }
    Serial.print("\n");
  }

  /* Clean neai buffer */
  memset(neai_buffer, 0.0, (uint16_t) (SENSOR_FRAME_RESOLUTION * SENSOR_FRAMES) * sizeof(float));
  /* Reset pointer */
  neai_ptr = 0;
}