/* 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 <honourable_ages-project-1_inferencing.h>
#include "edge-impulse-sdk/dsp/image/image.hpp"
#include <edge-impulse-sdk/classifier/ei_run_classifier.h>

#include "esp_camera.h"
#include <Servo.h>

// Select camera model - find more camera models in camera_pins.h file here
// https://github.com/espressif/arduino-esp32/blob/master/libraries/ESP32/
examples/Camera/CameraWebServer/camera_pins.h

//#define CAMERA_MODEL_ESP_EYE // Has PSRAM

#define CAMERA_MODEL_AI_THINKER // Has PSRAM

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

// #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

// Ultrasonic Sensor Pins

#define TRIG_PIN 13
#define ECHO_PIN 14

// Buzzer Pin
#define BUZZER_PIN 15

// Servo Motor Pin

#define SERVO_PIN 12

// Bin Fullness Threshold (in cm)

#define BIN_FULL_LEVEL 2

Servo myServo;

static bool debug_nn = false;

static bool is_initialised = false;
uint8_t *snapshot_buf;

static camera_config_t camera_config = {

.pin_pwdn = 32, .pin_reset = -1, .pin_xclk = 0,
.pin_sscb_sda = 26, .pin_sscb_scl = 27,
.pin_d7 = 35, .pin_d6 = 34, .pin_d5 = 39, .pin_d4 = 36,
.pin_d3 = 21, .pin_d2 = 19, .pin_d1 = 18, .pin_d0 = 5,
.pin_vsync = 25, .pin_href = 23, .pin_pclk = 22,
.xclk_freq_hz = 20000000, .ledc_timer = LEDC_TIMER_0,
.ledc_channel = LEDC_CHANNEL_0, .pixel_format = PIXFORMAT_JPEG,
.frame_size = FRAMESIZE_QVGA, .jpeg_quality = 12, .fb_count = 1,
.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) ;

// Function Prototypes
bool ei_camera_init(void);
void check_bin_level();
void sound_buzzer();

void setup() {
Serial.begin(115200);

pinMode(TRIG_PIN, OUTPUT);
pinMode(ECHO_PIN, INPUT);
pinMode(BUZZER_PIN, OUTPUT);

myServo.attach(SERVO_PIN);
myServo.write(90);

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

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

delay(2000);
}

void loop() {
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(EI_CLASSIFIER_INPUT_WIDTH, EI_CLASSIFIER_INPUT_HEIGHT,
snapshot_buf) == false) {
Serial.println("Failed to capture image.");
free(snapshot_buf);
return;
}

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);
return;
}

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);
}

bool recognized = false;

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("Detected: %s (%.2f%%)\n", bb.label, bb.value * 100);

if (strcmp(bb.label, "battery") == 0) {
myServo.write(180);
delay(2000);
myServo.write(90);
recognized = true;
}
else if (strcmp(bb.label, "paper") == 0) {
myServo.write(0);
delay(2000);
myServo.write(90);
recognized = true;
}
}

if (!recognized) {
Serial.println("Unrecognized waste! Triggering buzzer...");
sound_buzzer();
}

check_bin_level();
free(snapshot_buf);
}

// Function to Check Bin Level

void check_bin_level() {
digitalWrite(TRIG_PIN, LOW);
delayMicroseconds(2);
digitalWrite(TRIG_PIN, HIGH);
delayMicroseconds(10);
 digitalWrite(TRIG_PIN, LOW);

long duration = pulseIn(ECHO_PIN, HIGH);

int distance = duration * 0.034 / 2;

Serial.printf("Bin level distance: %d cm\n", distance);

if (distance <= BIN_FULL_LEVEL) {

Serial.println("Bin is full! Triggering buzzer...");
sound_buzzer();
}
}

// Function to Sound Buzzer

void sound_buzzer() {
for (int i = 0; i < 3; i++) {
digitalWrite(BUZZER_PIN, HIGH);
delay(500);
digitalWrite(BUZZER_PIN, LOW);
delay(500);
}
}