#include "esp_camera.h"
#include <WiFi.h>
#include <WebServer.h>
#include <ESP32Servo.h>

#define CAMERA_MODEL_AI_THINKER // Has PSRAM
#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

// WiFi credentials
const char* ssid = "FBI WiFi";
const char* password = "familylam180";

WebServer server(80);
Servo servo1;
int pos1 = 90;
int act = 0;

void setup() {
  // Initialize servo and attach it to pin 14
  servo1.attach(14, 500, 2500);
  servo1.write(90);
  
  // Initialize serial communication
  Serial.begin(9600);
  
  // Connect to WiFi
  WiFi.begin(ssid, password);
  while (WiFi.status() != WL_CONNECTED) {
    delay(1000);
    Serial.println("Connecting to WiFi...");
  }
  WiFi.setSleep(false);
  Serial.println("Connected to WiFi");

  // Print the IP address
  Serial.print("Web server started at http://");
  Serial.println(WiFi.localIP());

  // Configure the camera
  camera_config_t config;
  config.ledc_channel = LEDC_CHANNEL_0;
  config.ledc_timer = LEDC_TIMER_0;
  config.pin_d0 = Y2_GPIO_NUM;
  config.pin_d1 = Y3_GPIO_NUM;
  config.pin_d2 = Y4_GPIO_NUM;
  config.pin_d3 = Y5_GPIO_NUM;
  config.pin_d4 = Y6_GPIO_NUM;
  config.pin_d5 = Y7_GPIO_NUM;
  config.pin_d6 = Y8_GPIO_NUM;
  config.pin_d7 = Y9_GPIO_NUM;
  config.pin_xclk = XCLK_GPIO_NUM;
  config.pin_pclk = PCLK_GPIO_NUM;
  config.pin_vsync = VSYNC_GPIO_NUM;
  config.pin_href = HREF_GPIO_NUM;
  config.pin_sccb_sda = SIOD_GPIO_NUM;
  config.pin_sccb_scl = SIOC_GPIO_NUM;
  config.pin_pwdn = PWDN_GPIO_NUM;
  config.pin_reset = RESET_GPIO_NUM;
  config.xclk_freq_hz = 20000000;
  config.frame_size = FRAMESIZE_UXGA;
  config.pixel_format = PIXFORMAT_JPEG; // for streaming
  //config.pixel_format = PIXFORMAT_RGB565; // for face detection/recognition
  config.grab_mode = CAMERA_GRAB_WHEN_EMPTY;
  config.fb_location = CAMERA_FB_IN_PSRAM;
  config.jpeg_quality = 12;
  config.fb_count = 1;

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

  server.on("/", HTTP_GET, []() {
    server.send(200, "text/html", "<h1>ESP32-CAM Stream</h1>");  // Replace with actual HTML and JS code to display the camera stream
  });

  // Start the web server
  server.begin();

  // Create tasks pinned to each core
  xTaskCreatePinnedToCore(webServerTask, "WebServerTask", 10000, NULL, 1, NULL, 0);
  xTaskCreatePinnedToCore(servoTask, "ServoTask", 10000, NULL, 1, NULL, 1);
}

void loop() {
  // main loop is empty because tasks handle the functionality
}

void webServerTask(void * parameter) {
  while(1) {
    // Web server logic here
    server.handleClient();
    delay(20);
  }
}

void servoTask(void * parameter) {
  while(1) {
    if (Serial.available() > 0) {
      char command = Serial.read();
      switch(command) {
        case 'w':
          if (pos1 + 10 <= 180) {
            pos1 += 10;
            act = 1;
          } else {
            Serial.println("Servo 1 at maximum position");
          }
          break;
        case 's':
          if (pos1 - 10 >= 0) {
            pos1 -= 10;
            act = 1;
          } else {
            Serial.println("Servo 1 at minimum position");
          }
          break;
        default:
          break;
      }

      if(act == 1) {
        servo1.write(pos1);
        Serial.print("Servo 1 Position: ");
        Serial.println(pos1);
        delay(15);
      }
      act = 0;
    }
    delay(20);
  }
}