#include "esp_camera.h"
#include <WiFi.h>
#include <WiFiClientSecure.h>
#include <PubSubClient.h>
#include <ArduinoJson.h>
#include <AsyncJson.h>
#include <ESPAsyncWebSrv.h>

#define CAMERA_MODEL_AI_THINKER
#include "camera_pins.h"

/* CONSTANTS */
#define AP_SSID_PREFIX "ESP32CAM-"
#define AP_PASSWORD "BGARGJtd"
#define MQTT_CLIENT_PREFIX "esp32/"
#define MQTT_PING_TOPIC "esp32/ping"
#define MQTT_PONG_TOPIC "esp32/pong"

/* GLOBAL VARIABLES */
const String ESP32_ID = String(ESP.getEfuseMac() >> 24, HEX); // Unique ID
const String AP_SSID = AP_SSID_PREFIX + ESP32_ID; // Unique SSID
const String MQTT_CLIENT_NAME = MQTT_CLIENT_PREFIX + ESP32_ID; // Unique Client ID
const String MQTT_UPLOAD_TOPIC = MQTT_CLIENT_NAME + "/image"; 
const String MQTT_CONFIG_TOPIC = MQTT_CLIENT_NAME + "/config";
const String MQTT_OK_TOPIC = MQTT_CLIENT_NAME + "/ok";
String WIFI_SSID = "";  
String WIFI_PASSWORD = "";
String MQTT_BROKER_ADDRESS = "";
uint16_t MQTT_PORT = NULL;
String MQTT_USER = "";
String MQTT_PASS = "";
String TLS_CERTIFICATE = "";

WiFiClientSecure esp_secure_client;
WiFiClient esp_client;
PubSubClient client;  // MQTT Client
AsyncWebServer server(80);       // HTTP Server for the wifi and mqtt configuration
framesize_t current_framesize = FRAMESIZE_CIF;
bool current_auto_exposure = true; // Flag to check if the device is in auto exposure mode
bool current_auto_gain = true;    // Flag to check if the device is in auto gain mode
int current_contrast = 0;       // Contrast value
int current_brightness = 0;     // Brightness value
int current_saturation = 0;     // Saturation value
int current_gain = 0;          // Gain value
int current_exposure = 0;      // Exposure value
bool current_vertical_flip = false;  // Flag to check if the device is vertically flipped
bool current_horizontal_flip = false; // Flag to check if the device is horizontally flipped


bool led_on = false; // Flag to check if the LED is on
bool configured = false;    // Flag to check if the device is configured
bool streaming = true;      // Flag to check if the device is streaming
bool taking_photo = false;  // Flag to check if the device is taking a photo


esp_err_t camera_init() {
  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 = current_framesize;
  config.pixel_format = PIXFORMAT_JPEG;
  config.grab_mode = CAMERA_GRAB_WHEN_EMPTY;
  config.fb_location = CAMERA_FB_IN_PSRAM;
  config.jpeg_quality = 12;
  config.fb_count = 1;
  config.grab_mode = CAMERA_GRAB_LATEST;

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


void init_access_point() {  // Start Access Point
  WiFi.softAP(AP_SSID, AP_PASSWORD);
  Serial.println("Access Point started:");
  Serial.println(AP_SSID);
  Serial.println(AP_PASSWORD);
  Serial.println(WiFi.softAPIP());
  Serial.println();
}


void init_wifi() {  // Connect to WiFi network
  WiFi.mode(WIFI_STA);
  Serial.print("Connecting to WiFi");
  WiFi.begin(WIFI_SSID, WIFI_PASSWORD);
  WiFi.setSleep(false);

  while (WiFi.status() != WL_CONNECTED) {
    delay(1000);
    Serial.print(".");
  }
  Serial.print("\nWiFi connected. IP address: ");
  Serial.println(WiFi.localIP());
}


void callback(char *topic, byte *message, unsigned int length) {  // MQTT Callback
  if (strcmp(topic, MQTT_CONFIG_TOPIC.c_str()) == 0) { // If the topic is the configuration topic  
    Serial.println("Configuration received");                 
    configure_camera(message, length);
  } else if (strcmp(topic, MQTT_PING_TOPIC) == 0) { // If the topic is the ping topic
    // Send the camera ID to PONG topic
    Serial.println("PING received");
    send_pong();
  } else {
    Serial.print("ERROR: Unknown topic ");
    Serial.println(topic);
  }
}

void send_pong() {  // Send the camera status to the PONG topic
  String payload = "";
  const String frame_size = get_current_frame_size();
  payload += "{\"camera_id\":\"" + ESP32_ID + "\",";
  payload += "\"led_on\":" + String(led_on) + ",";
  payload += "\"frame_size\":\"" + frame_size + "\",";
  payload += "\"brightness\":" + String(current_brightness) + ",";
  payload += "\"contrast\":" + String(current_contrast) + ",";
  payload += "\"saturation\":" + String(current_saturation) + ",";
  payload += "\"auto_exposure\":" + String(current_auto_exposure) + ",";
  payload += "\"exposure_value\":" + String(current_exposure) + ",";
  payload += "\"auto_gain\":" + String(current_auto_gain) + ",";
  payload += "\"gain_value\":" + String(current_gain) + ",";
  payload += "\"vertical_flip\":" + String(current_vertical_flip) + ",";
  payload += "\"horizontal_flip\":" + String(current_horizontal_flip) + "}"; 
  
  client.publish(MQTT_PONG_TOPIC, payload.c_str());
  Serial.println("PONG sent");
}

void configure_camera(byte *message, unsigned int length) {  // Configure camera with the received JSON
  StaticJsonDocument<200> doc;
  if (deserializeJson(doc, (char *)message) != DeserializationError::Ok) {
    Serial.println("ERROR: Failed to parse JSON: ");
    return;
  }

  if (doc["frame_size"].isNull() || doc["brightness"].isNull() || doc["contrast"].isNull() || doc["saturation"].isNull() || doc["auto_exposure"].isNull() || doc["auto_gain"].isNull() || doc["exposure_value"].isNull() || doc["gain_value"].isNull() || doc["vertical_flip"].isNull() || doc["horizontal_flip"].isNull() || doc["led_on"].isNull()) {
    Serial.println("ERROR: Invalid JSON");
    return;
  }

  const char *frame_size = doc["frame_size"];
  const float brightness = doc["brightness"];
  const float contrast = doc["contrast"];
  const float saturation = doc["saturation"];
  const bool auto_exposure = doc["auto_exposure"];
  const bool auto_gain = doc["auto_gain"];
  const uint exposure = doc["exposure_value"];
  const uint gain = doc["gain_value"];
  const bool vertical_flip = doc["vertical_flip"];
  const bool horizontal_flip = doc["horizontal_flip"];
  const bool led_on = doc["led_on"];

  if (led_on) {
    turn_on_led();
  } else {
    turn_off_led();
  }
  
  if (set_current_frame_size(frame_size)) {
    Serial.println("ERROR: Invalid framesize specified");
    return;
  }

  streaming = false;       // Stop streaming
  while (taking_photo) {}  // Wait to last photo to be sent

  if (esp_camera_deinit() != ESP_OK) {
    Serial.println("ERROR: Failed to deinitialize camera");
    return;
  }

  if (camera_init() != ESP_OK) {
    Serial.println("ERROR: Failed to reinitialize camera");
    return;
  }

  if (set_brightness(brightness)) {
    Serial.println("ERROR: Failed to set brightness");
    return;
  }

  if (set_contrast(contrast)) {
    Serial.println("ERROR: Failed to set contrast");
    return;
  }

  if (set_saturation(saturation)) {
    Serial.println("ERROR: Failed to set saturation");
    return;
  }

  if (set_auto_exposure(auto_exposure)) {
    Serial.println("ERROR: Failed to set autoexposure");
    return;
  }

  if (set_exposure(exposure)) {
    Serial.println("ERROR: Failed to set autoexposure");
    return;
  }

  if (set_auto_gain(auto_gain)) {
    Serial.println("ERROR: Failed to set autogain");
    return;
  }

  if (set_gain(gain)) {
    Serial.println("ERROR: Failed to set gain");
    return;
  }

  if (set_vertical_flip(vertical_flip)) {
    Serial.println("ERROR: Failed to set vertical flip");
    return;
  }

  if (set_horizontal_flip(horizontal_flip)) {
    Serial.println("ERROR: Failed to set horizontal flip");
    return;
  }

  client.publish(MQTT_OK_TOPIC.c_str(), "{\"status\":\"ok\"}"); // Send the status to the OK topic
  Serial.println("Camera configured");
  streaming = true;  // Start streaming
}

const int set_brightness(const int brightness) {
  sensor_t *s = esp_camera_sensor_get();
  if (brightness > 2 || brightness < -2) {
    return 1;
  }
  s->set_brightness(s, brightness);  // -2 to 2
  current_brightness = brightness; 
  Serial.print("Brightness: ");
  Serial.println(brightness);
  return 0;
}

const int set_contrast(const int contrast) {
  sensor_t *s = esp_camera_sensor_get();
  if (contrast > 2 || contrast < -2) {
    return 1;
  }
  s->set_contrast(s, contrast);  // -2 to 2
  current_contrast = contrast;
  Serial.print("Contrast: ");
  Serial.println(contrast);
  return 0;
}

const int set_saturation(const int saturation) {
  sensor_t *s = esp_camera_sensor_get();
  if (saturation > 2 || saturation < -2) {
    return 1;
  }
  s->set_saturation(s, saturation);  // -2 to 2
  current_saturation = saturation;
  Serial.print("Saturation: ");
  Serial.println(saturation);
  return 0;
}

const int set_current_frame_size(const char *framesize) {
  /*
    Available framesizes:
    FRAMESIZE_QVGA = "320x240"
    FRAMESIZE_CIF = "400x296"
    FRAMESIZE_VGA = "640x480"
    FRAMESIZE_SVGA = "800x600"
    FRAMESIZE_XGA = "1024x768"
    FRAMESIZE_SXGA = "1280x1024"
  */

  if (strcmp(framesize, "320x240") == 0) {
    current_framesize = FRAMESIZE_QVGA;

  } else if (strcmp(framesize, "400x296") == 0) {
    current_framesize = FRAMESIZE_CIF;

  } else if (strcmp(framesize, "640x480") == 0) {
    current_framesize = FRAMESIZE_VGA;

  } else if (strcmp(framesize, "800x600") == 0) {
    current_framesize = FRAMESIZE_SVGA;

  } else if (strcmp(framesize, "1024x768") == 0) {
    current_framesize = FRAMESIZE_XGA;

  } else if (strcmp(framesize, "1280x1024") == 0) {
    current_framesize = FRAMESIZE_SXGA;

  } else {
    return 1;
  }
  return 0;
}

const String get_current_frame_size() {
  if (current_framesize == FRAMESIZE_QVGA) {
    return "320x240";
  } else if (current_framesize == FRAMESIZE_CIF) {
    return "400x296";
  } else if (current_framesize == FRAMESIZE_VGA) {
    return "640x480";
  } else if (current_framesize == FRAMESIZE_SVGA) {
    return "800x600";
  } else if (current_framesize == FRAMESIZE_XGA) {
    return "1024x768";
  } else if (current_framesize == FRAMESIZE_SXGA) {
    return "1280x1024";
  } else {
    return "Unknown";
  }
}

const int set_auto_exposure(const bool auto_exposure) {
  sensor_t *s = esp_camera_sensor_get();
  s->set_exposure_ctrl(s, !auto_exposure);  // 1 or 0
  current_auto_exposure = auto_exposure;
  Serial.print("Auto exposure: ");
  Serial.println(auto_exposure);
  return 0;
}


const uint set_exposure(const uint exposure) {
  sensor_t *s = esp_camera_sensor_get();
  if (exposure < 0 || exposure > 1200) {
    return 1;
  }
  s->set_exposure_ctrl(s, exposure);  // 0 to 1200
  current_exposure = exposure;
  Serial.print("Exposure: ");
  Serial.println(exposure);
  return 0;
}

const int set_auto_gain(const bool auto_gain) {
  sensor_t *s = esp_camera_sensor_get();
  s->set_gain_ctrl(s, !auto_gain);  // 1 or 0
  current_auto_gain = auto_gain;
  Serial.print("Auto gain: ");
  Serial.println(auto_gain);
  return 0;
}


const uint set_gain(const uint gain) {
  sensor_t *s = esp_camera_sensor_get();
  if (gain < 0 || gain > 30) {
    return 1;
  }
  s->set_exposure_ctrl(s, gain);  // 0 to 1200
  current_gain = gain;
  Serial.print("Gain: ");
  Serial.println(gain);
  return 0;
}

const uint set_vertical_flip(const bool vertical_flip) {
  sensor_t *s = esp_camera_sensor_get();
  s->set_vflip(s, vertical_flip);  // 1 or 0
  current_vertical_flip = vertical_flip;
  Serial.print("Vertical flip: ");
  Serial.println(vertical_flip);
  return 0;
}

const uint set_horizontal_flip(const bool horizontal_flip) {
  sensor_t *s = esp_camera_sensor_get();
  s->set_hmirror(s, horizontal_flip);  // 1 or 0
  current_horizontal_flip = horizontal_flip;
  Serial.print("Horizontal flip: ");
  Serial.println(horizontal_flip);
  return 0;
}

void init_mqtt() {
  client.setServer(MQTT_BROKER_ADDRESS.c_str(), MQTT_PORT);
  client.setBufferSize(60000); // Set buffer size to 60KB
  client.setCallback(callback);
}

void connect_mqtt() {
  Serial.print("Connecting with "); Serial.print(MQTT_BROKER_ADDRESS);
  Serial.print(" in port "); Serial.println(MQTT_PORT);

  if (MQTT_USER == "") { // If user and password are not provided
    Serial.println("Connecting without user and password");
    client.connect(MQTT_CLIENT_NAME.c_str());
  } else {
    Serial.print("Connecting with user "); Serial.print(MQTT_USER);
    Serial.print(" and password "); Serial.println(MQTT_PASS);
    client.connect(MQTT_CLIENT_NAME.c_str(), MQTT_USER.c_str(), MQTT_PASS.c_str());
  }

  if (client.connected()) {
    Serial.println("Connected to MQTT broker.");
    client.subscribe(MQTT_CONFIG_TOPIC.c_str());
    client.subscribe(MQTT_PING_TOPIC);
  } else {
    Serial.println("Failed to connect to MQTT broker.");
  }
}

void init_http_server() {
  AsyncCallbackJsonWebHandler *handler = new AsyncCallbackJsonWebHandler("/configure", [](AsyncWebServerRequest *request, JsonVariant &json) {
    JsonObject jsonObj = json.as<JsonObject>();

    // Check if all required fields are present
    if (jsonObj["wifi_ssid"].isNull() || jsonObj["wifi_password"].isNull() || jsonObj["mqtt_broker_address"].isNull() || jsonObj["mqtt_port"].isNull()){
      request->send(400, "application/json", "{\"status\": \"error\", \"message\": \"Missing required fields\"}");
      return;
    }
    Serial.println("Configuration received");
    TLS_CERTIFICATE = jsonObj["tls_certificate"].as<String>();

    if (TLS_CERTIFICATE != "") { // If the certificate is provided, use secure connection
      client.setClient(esp_secure_client);
      esp_secure_client.setCACert(TLS_CERTIFICATE.c_str());
      Serial.println("Secure connection enabled");
    } else {
      client.setClient(esp_client);
      Serial.println("Secure connection disabled");
    }

    WIFI_PASSWORD = jsonObj["wifi_password"].as<String>();
    MQTT_BROKER_ADDRESS = jsonObj["mqtt_broker_address"].as<String>();
    MQTT_PORT = (uint16_t)jsonObj["mqtt_port"].as<const uint16_t>();
    MQTT_USER = jsonObj["mqtt_user"].as<String>();
    MQTT_PASS = jsonObj["mqtt_pass"].as<String>();
    WIFI_SSID = jsonObj["wifi_ssid"].as<String>(); // This must be the last one to be set because it will trigger the connection to the wifi
    
    // Serial.print("WiFi SSID"); Serial.println(WIFI_SSID);
    // Serial.print("WiFi Password"); Serial.println(WIFI_PASSWORD);
    // Serial.print("MQTT Broker Address"); Serial.println(MQTT_BROKER_ADDRESS);
    // Serial.print("MQTT Port"); Serial.println(MQTT_PORT);
    // Serial.print("MQTT User"); Serial.println(MQTT_USER);
    // Serial.print("MQTT Pass"); Serial.println(MQTT_PASS);
    
    request->send(200, "application/json", "{\"status\": \"ok\"}");
  });
  server.addHandler(handler);
  server.begin();
}

void init_led() {
  pinMode(LED_GPIO_NUM, OUTPUT);
  digitalWrite(LED_GPIO_NUM, LOW); // Turn off the LED
}

void turn_on_led() {
  digitalWrite(LED_GPIO_NUM, HIGH); // Turn on the LED
  led_on = true;
}

void turn_off_led() {
  digitalWrite(LED_GPIO_NUM, LOW); // Turn off the LED
  led_on = false;
}


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

  camera_init();
  init_access_point();
  init_http_server();
  init_led();
}

void loop() {
  if (WiFi.status() == WL_CONNECTED) {  // Connected to WiFi
    client.loop();
    if (client.connected()) { // Connected to Broker
      if (streaming) {
        taking_photo = true;
        camera_fb_t *fb = esp_camera_fb_get();
        client.publish(MQTT_UPLOAD_TOPIC.c_str(), fb->buf, fb->len, false);
        esp_camera_fb_return(fb);
        taking_photo = false;
      }
    } else {
      Serial.println("Cannot connect with MQTT Brocker. Trying in 5 seconds...");
      sleep(5);
      connect_mqtt();
    }
  } else if (WIFI_SSID != "" && !configured) {  // WiFi and Broker credentials provided and not configured
    sleep(3); // Wait for send the response to the client using the http server
    init_wifi();
    init_mqtt();
    connect_mqtt();
    configured = true;
  }
}
