#include <BLEDevice.h>
#include <BLEServer.h>
#include <BLEUtils.h>
#include <Wire.h>
#include "esp_camera.h"
#include "Arduino.h"
#include "soc/soc.h"           // Disable brownour problems
#include "soc/rtc_cntl_reg.h"  // Disable brownour problems
#include "driver/rtc_io.h"     // RTC I/O driver
#include <EEPROM.h>            // read and write from flash memory (might not be present, check chip specs)


// Uncomment this line for a debug build
// #define DEBUG


#ifdef DEBUG
    #define DEBUG_SETUP() do { Serial.begin(115200); Serial.setDebugOutput(true); } while (0)
    #define DEBUG_PRINT(str) Serial.print(str)
    #define DEBUG_PRINTLN(str) Serial.println(str)
#else
    #define DEBUG_SETUP()
    #define DEBUG_PRINT(str)
    #define DEBUG_PRINTLN(str)
#endif


// define the number of bytes you want to access
#define EEPROM_SIZE 1

// Pin definition for 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

int pictureNumber = 0;



void initializeCameraConfig() {
  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_sscb_sda = SIOD_GPIO_NUM;
  config.pin_sscb_scl = SIOC_GPIO_NUM;
  config.pin_pwdn = PWDN_GPIO_NUM;
  config.pin_reset = RESET_GPIO_NUM;
  config.xclk_freq_hz = 20000000;
  config.pixel_format = PIXFORMAT_JPEG; 
  config.frame_size = FRAMESIZE_SVGA;
  config.jpeg_quality = 12;
  config.fb_count = 1;
  
  // Init Camera
  esp_err_t err = esp_camera_init(&config);
  if (err != ESP_OK) {
    Serial.printf("Camera init failed with error 0x%x", err);
    return;
  }
  
}




  





#define bmeButtonPress
#define bmeImageCapture


//BLE server name
#define bleServerName "BLE_Serve_ESP32"


// Timer variables
unsigned long lastTime = 0;
unsigned long timerDelay = 30000;

bool deviceConnected = false;

bool debugActive = true;


// Create a UUID with the following tool:
// https://www.uuidgenerator.net/
#define SERVICE_UUID "ee0fd8f8-1f4d-4158-9970-15d913691c58"

// Button Press Characteristic and Descriptor
BLECharacteristic bmeDoorbellPressCharacteristics("395d76a3-ae11-42c5-b533-5733e0f85873", BLECharacteristic::PROPERTY_NOTIFY);
BLEDescriptor bmeDoorbellPressDescriptor(BLEUUID((uint16_t)0x2902));

// Image Characteristic and Descriptor
BLECharacteristic bmeImageCaptureCharacteristics("395d76a3-ae11-42c5-b533-5733e0f85873", BLECharacteristic::PROPERTY_NOTIFY);
BLEDescriptor bmeImageCaptureDescriptor(BLEUUID((uint16_t)0x2902));


// Stuff for later //

/* Enter deep sleep */
//esp_deep_sleep_start();



// Possibly relevant, but try without first
//Power up External 40 MHz XTAL

//    In some special applications, some modules have high requirements on clock accuracy and stability during sleep (such as BT). In this case, consider turning on External 40 MHz XTAL during sleep. The opening and closing code is as follows:

 //   ESP_ERROR_CHECK(esp_sleep_pd_config(ESP_PD_DOMAIN_XTAL, ESP_PD_OPTION_ON));
  //  ESP_ERROR_CHECK(esp_sleep_pd_config(ESP_PD_DOMAIN_XTAL, ESP_PD_OPTION_OFF));



// ^^^ Stuff for later //


class MyServerCallbacks: public BLEServerCallbacks {
    void onConnect(BLEServer* pServer) {
      deviceConnected = true;
      interface->onBoardLED->led[0] = interface->onBoardLED->LED_GREEN;
      interface->onBoardLED->statusLED(100, 1);
      BLEDevice::startAdvertising();
    };

    void onDisconnect(BLEServer* pServer) {
      deviceConnected = false;
      interface->onBoardLED->led[0] = interface->onBoardLED->LED_BLUE;
      interface->onBoardLED->statusLED(100, 1);
      interface->onBoardLED->led[0] = interface->onBoardLED->LED_BLUE;
      interface->onBoardLED->statusLED(100, 1);
      interface->onBoardLED->led[0] = interface->onBoardLED->LED_BLUE;
      interface->onBoardLED->statusLED(100, 1);
      pServer->getAdvertising()->start();
    }
};

class pCharacteristicTX_Callbacks: public BLECharacteristicCallbacks {
    void onWrite(BLECharacteristic *pCharacteristic) {
      std::string rxValue = pCharacteristic->getValue();
    
      if (rxValue.length() > 0) {
        String msg="";
        mserial->printStrln("*********");
        mserial->printStr("Received Value: ");
        for (int i = 0; i < rxValue.length(); i++) {
           msg=msg+String(rxValue[i]);
           mserial->printStr(String(rxValue[i])); 
        } 
      
        mserial->printStrln(""); 
        mserial->printStrln("*********");

        //valueReceived=msg;
      } 
    }

    void onRead(BLECharacteristic *pCharacteristic) {
      mserial->printStrln("*********");
      mserial->printStr("onRead...");    
      pCharacteristic->setValue("OK");
    }

  }; 
                    
class pCharacteristicRX_Callbacks: public BLECharacteristicCallbacks {
    void onWrite(BLECharacteristic *pCharacteristic) {
      std::string rxValue = pCharacteristic->getValue();
      
      mserial->ble=false;

      if (rxValue.length() > 0) {
        String msg="";
        mserial->printStr("Received Value: ");
        for (int i = 0; i < rxValue.length(); i++) {
           msg=msg+String(rxValue[i]);
           mserial->printStr(String(rxValue[i])); 
        } 
      
        msg.trim();
        $BLE_CMD=msg;
        mserial->ble=true;
        newCMDarrived=true;
      }
    } 

    void onRead(BLECharacteristic *pCharacteristic) {
      mserial->printStrln("*********");
      mserial->printStr("onRead...");    
      pCharacteristic->setValue("OK");
    }

  }; 

//------------------------------------------------------------------


// Create the BLE Device 
  BLEDevice::init("Bluebell Doorbell");  // Give it a name 
  
  // Create the BLE Server 
  pServer = BLEDevice::createServer(); 
  pServer->setCallbacks(new MyServerCallbacks());

  // Create the BLE Service
  pService = pServer->createService(SERVICE_UUID);
  
  // Create a BLE Characteristic

  pCharacteristicTX = pService->createCharacteristic(
                      CHARACTERISTIC_UUID_TX,
                      BLECharacteristic::PROPERTY_NOTIFY
                    );


  pCharacteristicTX->addDescriptor(new BLE2902());

  pCharacteristicRX = pService->createCharacteristic(
                      CHARACTERISTIC_UUID_RX,
                      BLECharacteristic::PROPERTY_READ   |
                      BLECharacteristic::PROPERTY_WRITE  |
                      BLECharacteristic::PROPERTY_NOTIFY |
                      BLECharacteristic::PROPERTY_INDICATE
                    );

  pCharacteristicTX->setCallbacks(new pCharacteristicTX_Callbacks());
  pCharacteristicRX->setCallbacks(new pCharacteristicRX_Callbacks());

  interface->init_BLE(pCharacteristicTX);

  // Start the service
  pService->start();





// psuedocode


void lsleep_sleep() {
  btStop();
  delay(1000);
  WiFi.disconnect(true);
  WiFi.mode(WIFI_OFF);

  changeMcuFreq(interface, interface->MIN_MCU_FREQUENCY);
}


void lsleep_wake() {
  changeMcuFreq(interface, interface->WIFI_FREQUENCY);
  btStart();
}


/// end psuedocode

void setup() {
  WRITE_PERI_REG(RTC_CNTL_BROWN_OUT_REG, 0); // is disable brownout detector still required?
  DEBUG_SETUP();
  DEBUG_PRINTLN("Bluebell started, beginning setup!");
  initializeCameraConfig();
// Take Picture with Camera
fb = esp_camera_fb_get();  
if(!fb) {
  Serial.println("Camera capture failed");
  return;
}

// Send Picture over Serial
Serial.write(fb->buf, fb->len); // Send the image buffer over serial

esp_camera_fb_return(fb); 

// Turns off the ESP32-CAM white on-board LED (flash) connected to GPIO 4
pinMode(4, OUTPUT);
digitalWrite(4, LOW);
rtc_gpio_hold_en(GPIO_NUM_4);

delay(2000);
Serial.println("Going to sleep now");
delay(2000);
esp_deep_sleep_start();
  
}

void loop() {
  // put your main code here, to run repeatedly:
  delay(10); // this speeds up the simulation
}







// main psuedocode


// setup
//// #ifdef DEBUG
//////    Serial.begin(115200);
//////    Serial.println("Bluebell started, beginning setup!");
//// #endif
//// set GPIOs?
//// capture image
//// set bluetooth
////