/* Edge Impulse Arduino examples

* Copyright (c) 2022 EdgeImpulse Inc.

*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/

// These sketches are tested with 2.0.4 ESP32 Arduino Core

// https://github.com/espressif/arduino-esp32/releases/tag/2.0.4

/* Includes ---------------------------------------------------------------- */
#include <AS_RoBo_smart_dustbin_inferencing.h>
#include "edge-impulse-sdk/dsp/image/image.hpp"

#include "esp_camera.h"

// Remove I2C OLED includes

// #include <Wire.h>
// #include <Adafruit_GFX.h>
// #include <Adafruit_SSD1306.h>

#include <LiquidCrystal.h> // Include LiquidCrystal for LCD 2004A

#include <ESP32Servo.h> // Include ESP32Servo library

Servo myServo; // Create a Servo object

// Define LCD pins (adjust if needed)

// LiquidCrystal(rs, enable, d4, d5, d6, d7)
LiquidCrystal lcd(33, 4, 13, 14, 15, 16);

// Select camera model - see camera_pins.h for details

// #define CAMERA_MODEL_ESP_EYE // Has PSRAM

#define CAMERA_MODEL_AI_THINKER // Has PSRAM

#if defined(CAMERA_MODEL_ESP_EYE)
#define PWDN_GPIO_NUM -1
#define RESET_GPIO_NUM -1
#define XCLK_GPIO_NUM 4
#define SIOD_GPIO_NUM 18
#define SIOC_GPIO_NUM 23

#define Y9_GPIO_NUM 36
#define Y8_GPIO_NUM 37
#define Y7_GPIO_NUM 38
#define Y6_GPIO_NUM 39
#define Y5_GPIO_NUM 35
#define Y4_GPIO_NUM 14
#define Y3_GPIO_NUM 13
#define Y2_GPIO_NUM 34
#define VSYNC_GPIO_NUM 5
#define HREF_GPIO_NUM 27
#define PCLK_GPIO_NUM 25

#elif defined(CAMERA_MODEL_AI_THINKER)
#define PWDN_GPIO_NUM 32
#define RESET_GPIO_NUM -1
#define XCLK_GPIO_NUM 0
#define SIOD_GPIO_NUM 26
#define SIOC_GPIO_NUM 27

#define Y9_GPIO_NUM 35
#define Y8_GPIO_NUM 34
#define Y7_GPIO_NUM 39
#define Y6_GPIO_NUM 36
#define Y5_GPIO_NUM 21
#define Y4_GPIO_NUM 19
#define Y3_GPIO_NUM 18
#define Y2_GPIO_NUM 5
#define VSYNC_GPIO_NUM 25
#define HREF_GPIO_NUM 23
#define PCLK_GPIO_NUM 22

#else
#error "Camera model not selected"
#endif

/* Constant defines -------------------------------------------------------- */

#define EI_CAMERA_RAW_FRAME_BUFFER_COLS 320
#define EI_CAMERA_RAW_FRAME_BUFFER_ROWS 240
#define EI_CAMERA_FRAME_BYTE_SIZE 3

#define SERVO_PIN 12 // Define servo control pin

/* Private variables ------------------------------------------------------- */

static bool debug_nn = false; // Set this to true to see e.g. features generated
from the raw signal
static bool is_initialised = false;
uint8_t *snapshot_buf; //points to the output of the capture

static camera_config_t camera_config = {

.pin_pwdn = PWDN_GPIO_NUM,
.pin_reset = RESET_GPIO_NUM,
.pin_xclk = XCLK_GPIO_NUM,
.pin_sscb_sda = SIOD_GPIO_NUM,
.pin_sscb_scl = SIOC_GPIO_NUM,

.pin_d7 = Y9_GPIO_NUM,
.pin_d6 = Y8_GPIO_NUM,
.pin_d5 = Y7_GPIO_NUM,
.pin_d4 = Y6_GPIO_NUM,
.pin_d3 = Y5_GPIO_NUM,
 .pin_d2 = Y4_GPIO_NUM,
.pin_d1 = Y3_GPIO_NUM,
.pin_d0 = Y2_GPIO_NUM,
.pin_vsync = VSYNC_GPIO_NUM,
.pin_href = HREF_GPIO_NUM,
.pin_pclk = PCLK_GPIO_NUM,

// XCLK 20MHz or 10MHz for OV2640 double FPS (Experimental)

.xclk_freq_hz = 20000000,
.ledc_timer = LEDC_TIMER_0,
.ledc_channel = LEDC_CHANNEL_0,

.pixel_format = PIXFORMAT_JPEG, // YUV422, GRAYSCALE, RGB565, JPEG

.frame_size = FRAMESIZE_QVGA, // QQVGA-UXGA Do not use sizes above QVGA when
not JPEG

.jpeg_quality = 12, // 0-63 lower number means higher quality

.fb_count = 1, // if more than one, i2s runs in continuous mode. Use only
with JPEG
.fb_location = CAMERA_FB_IN_PSRAM,
.grab_mode = CAMERA_GRAB_WHEN_EMPTY,
};

/* Function definitions ------------------------------------------------------- */

bool ei_camera_init(void);
void ei_camera_deinit(void);
bool ei_camera_capture(uint32_t img_width, uint32_t img_height, uint8_t *out_buf) ;

/**
* @brief Arduino setup function
*/
void setup()
{
// Initialize serial communication
Serial.begin(115200);

// Initialize LCD 2004A

lcd.begin(20, 4); // 20 columns and 4 rows
lcd.clear();
lcd.setCursor(0, 0);
lcd.print("Welcome to");
lcd.setCursor(0, 1);
lcd.print("AS RoBo");

// Initialize servo
myServo.attach(SERVO_PIN);
myServo.write(90); // Set initial servo position to 90 degrees

// Wait for serial connection if necessary

while (!Serial);
Serial.println("Edge Impulse Inferencing Demo");
if (ei_camera_init() == false) {
Serial.println("Failed to initialize Camera!");
}
else {
Serial.println("Camera initialized");
}

Serial.println("\nStarting continuous inference in 2 seconds...");

 delay(2000);

// Update LCD with starting message

lcd.clear();
lcd.setCursor(0, 0);
lcd.print("Starting continous");
lcd.setCursor(0, 1);
lcd.print("inference in");
lcd.setCursor(0, 2);
lcd.print("2 seconds...");
}

/**
* @brief Get data and run inferencing
*/
void loop()
{
// Clear LCD for each new loop
lcd.clear();

// Instead of wait_ms, wait on the signal

if (ei_sleep(5) != EI_IMPULSE_OK) {
return;
}

snapshot_buf = (uint8_t*)malloc(EI_CAMERA_RAW_FRAME_BUFFER_COLS *
EI_CAMERA_RAW_FRAME_BUFFER_ROWS * EI_CAMERA_FRAME_BYTE_SIZE);
if (snapshot_buf == nullptr) {
Serial.println("ERR: Failed to allocate snapshot buffer!");
return;
}

ei::signal_t signal;
signal.total_length = EI_CLASSIFIER_INPUT_WIDTH * EI_CLASSIFIER_INPUT_HEIGHT;
signal.get_data = &ei_camera_get_data;

if (ei_camera_capture((size_t)EI_CLASSIFIER_INPUT_WIDTH,
(size_t)EI_CLASSIFIER_INPUT_HEIGHT, snapshot_buf) == false) {
Serial.println("Failed to capture image");
free(snapshot_buf);
return;
}

// Run the classifier

ei_impulse_result_t result = { 0 };
EI_IMPULSE_ERROR err = run_classifier(&signal, &result, debug_nn);
if (err != EI_IMPULSE_OK) {
Serial.printf("ERR: Failed to run classifier (%d)\n", err);
free(snapshot_buf);
return;
}

// Print the predictions to serial

Serial.printf("Predictions (DSP: %d ms., Classification: %d ms., Anomaly: %d
ms.): \n",
result.timing.dsp, result.timing.classification,
result.timing.anomaly);

#if EI_CLASSIFIER_OBJECT_DETECTION == 1
 Serial.println("Object detection bounding boxes:");
for (uint32_t i = 0; i < result.bounding_boxes_count; i++) {
ei_impulse_result_bounding_box_t bb = result.bounding_boxes[i];
if (bb.value == 0) {
continue;
}
Serial.printf(" %s (%.5f) [ x: %u, y: %u, width: %u, height: %u ]\n",
bb.label,
bb.value,
bb.x,
bb.y,
bb.width,
bb.height);

// Display the label and confidence on LCD at row 2

lcd.setCursor(0, 2);
lcd.print(bb.label);
lcd.print("-");
lcd.print(int(bb.value * 100));
lcd.print("%");

// Servo actions based on detected label

if (strcmp(bb.label, "battery") == 0) {
ei_sleep(10);
myServo.write(180);
ei_sleep(2000);
myServo.write(90);
}
if (strcmp(bb.label, "paper") == 0) {
ei_sleep(10);
myServo.write(0);
ei_sleep(2000);
myServo.write(90);
}
}
#else
Serial.println("Predictions:");
for (uint16_t i = 0; i < EI_CLASSIFIER_LABEL_COUNT; i++) {
Serial.printf(" %s: %.5f\n", ei_classifier_inferencing_categories[i],
result.classification[i].value);
}
#endif

#if EI_CLASSIFIER_HAS_ANOMALY
Serial.printf("Anomaly prediction: %.3f\n", result.anomaly);
#endif

#if EI_CLASSIFIER_HAS_VISUAL_ANOMALY
Serial.println("Visual anomalies:");
for (uint32_t i = 0; i < result.visual_ad_count; i++) {
ei_impulse_result_bounding_box_t bb = result.visual_ad_grid_cells[i];
if (bb.value == 0) {
continue;
}
Serial.printf(" %s (%.5f) [ x: %u, y: %u, width: %u, height: %u ]\n",
bb.label,
bb.value,
bb.x,
bb.y,
 bb.width,
bb.height);
}
#endif

free(snapshot_buf);
}

/**
* @brief Setup image sensor & start streaming
*
* @retval false if initialisation failed
*/
bool ei_camera_init(void) {

if (is_initialised) return true;

#if defined(CAMERA_MODEL_ESP_EYE)
pinMode(13, INPUT_PULLUP);
pinMode(14, INPUT_PULLUP);
#endif

// Initialize the camera

esp_err_t err = esp_camera_init(&camera_config);
if (err != ESP_OK) {
Serial.printf("Camera init failed with error 0x%x\n", err);
return false;
}

sensor_t * s = esp_camera_sensor_get();
// For specific sensors, adjust settings as needed
if (s->id.PID == OV3660_PID) {
s->set_vflip(s, 1); // flip it back
s->set_brightness(s, 1);
s->set_saturation(s, 0);
}

#if defined(CAMERA_MODEL_M5STACK_WIDE)
s->set_vflip(s, 1);
s->set_hmirror(s, 1);
#elif defined(CAMERA_MODEL_ESP_EYE)
s->set_vflip(s, 1);
s->set_hmirror(s, 1);
s->set_awb_gain(s, 1);
#endif

is_initialised = true;
return true;
}

/**
* @brief Stop streaming of sensor data
*/
void ei_camera_deinit(void) {
esp_err_t err = esp_camera_deinit();
if (err != ESP_OK) {
Serial.println("Camera deinit failed");
}
is_initialised = false;
}

/**
* @brief Capture, rescale and crop image
*/
bool ei_camera_capture(uint32_t img_width, uint32_t img_height, uint8_t *out_buf) {
bool do_resize = false;

if (!is_initialised) {
Serial.println("ERR: Camera is not initialized");
return false;
}

camera_fb_t *fb = esp_camera_fb_get();

if (!fb) {
Serial.println("Camera capture failed");
return false;
}

bool converted = fmt2rgb888(fb->buf, fb->len, PIXFORMAT_JPEG, snapshot_buf);

esp_camera_fb_return(fb);

if (!converted) {
Serial.println("Conversion failed");
return false;
}

if ((img_width != EI_CAMERA_RAW_FRAME_BUFFER_COLS) ||
(img_height != EI_CAMERA_RAW_FRAME_BUFFER_ROWS)) {
do_resize = true;
}

if (do_resize) {
ei::image::processing::crop_and_interpolate_rgb888(
out_buf,
EI_CAMERA_RAW_FRAME_BUFFER_COLS,
EI_CAMERA_RAW_FRAME_BUFFER_ROWS,
out_buf,
img_width,
img_height);
}

return true;
}

static int ei_camera_get_data(size_t offset, size_t length, float *out_ptr)

{
// Convert offset into a pixel index (3 bytes per pixel)
size_t pixel_ix = offset * 3;
size_t pixels_left = length;
size_t out_ptr_ix = 0;

while (pixels_left != 0) {
// Swap BGR to RGB due to known ESP32 camera issue
out_ptr[out_ptr_ix] = (snapshot_buf[pixel_ix + 2] << 16) +
(snapshot_buf[pixel_ix + 1] << 8) +
snapshot_buf[pixel_ix];

out_ptr_ix++;
 pixel_ix += 3;
pixels_left--;
}
// Optionally clear LCD (if desired) after data retrieval:
lcd.clear();
return 0;
}

#if !defined(EI_CLASSIFIER_SENSOR) || EI_CLASSIFIER_SENSOR !=

EI_CLASSIFIER_SENSOR_CAMERA
#error "Invalid model for current sensor"
#endif