#include "esp_camera.h"
#include <esp_sleep.h>
#include <driver/uart.h>
#include <WiFi.h>

//
// WARNING!!! PSRAM IC required for UXGA resolution and high JPEG quality
//            Ensure ESP32 Wrover Module or other board with PSRAM is selected
//            Partial images will be transmitted if image exceeds buffer size
//

// Select camera model
//#define CAMERA_MODEL_WROVER_KIT // Has PSRAM
//#define CAMERA_MODEL_ESP_EYE // Has PSRAM
//#define CAMERA_MODEL_M5STACK_PSRAM // Has PSRAM
//#define CAMERA_MODEL_M5STACK_V2_PSRAM // M5Camera version B Has PSRAM
//#define CAMERA_MODEL_M5STACK_WIDE // Has PSRAM
//#define CAMERA_MODEL_M5STACK_ESP32CAM // No PSRAM
#define CAMERA_MODEL_AI_THINKER // Has PSRAM
//#define CAMERA_MODEL_TTGO_T_JOURNAL // No PSRAM

#include "camera_pins.h"
const char* ssidName = "iPhone 12";
const char* pw = "howtomake";
void startCameraServer();

void setup() {
  Serial.begin(115200);
  //Serial.setDebugOutput(true);
  Serial.println("in setup");
  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;
  
  // if PSRAM IC present, init with UXGA resolution and higher JPEG quality
  //                      for larger pre-allocated frame buffer.
  if(psramFound()){
    config.frame_size = FRAMESIZE_UXGA;
    config.jpeg_quality = 10;
    config.fb_count = 2;
  } else {
    config.frame_size = FRAMESIZE_SVGA;
    config.jpeg_quality = 12;
    config.fb_count = 1;
  }

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

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

  sensor_t * s = esp_camera_sensor_get();
  // initial sensors are flipped vertically and colors are a bit saturated
  if (s->id.PID == OV3660_PID) {
    s->set_vflip(s, 1); // flip it back
    s->set_brightness(s, 1); // up the brightness just a bit
    s->set_saturation(s, -2); // lower the saturation
  }
  // drop down frame size for higher initial frame rate
  s->set_framesize(s, FRAMESIZE_QVGA);

#if defined(CAMERA_MODEL_M5STACK_WIDE) || defined(CAMERA_MODEL_M5STACK_ESP32CAM)
  s->set_vflip(s, 1);
  s->set_hmirror(s, 1);
#endif
  Serial.println("Camera setup() completed. Waiting for wifi to be configured.");
  Serial.println((char *)ssidName);
  Serial.println((char*)pw);
}

void wifiConfig(char *ssid, char *password) {
  Serial.print("in wificonfig");
  
  WiFi.begin(ssid, password);

  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
  }
  Serial.println("");
  Serial.println("WiFi connected");

  startCameraServer();

  Serial.print("Camera Ready! Use 'http://");
  Serial.print(WiFi.localIP());
  Serial.println("' to connect");
}


#define UNDEFINED 0
#define START 1
#define END 2
#define CMDBUFSIZE 64    
uint8_t cmd_state = UNDEFINED;
uint8_t cmdbuf[CMDBUFSIZE + 2] = {0};   // to make room for the terminating 0
uint8_t cindx = 0;


void loop() {
  //Serial.println("in loop");
  
  uint8_t ch;
  int result = 0;
    
  // Forward from BLEUART to HW Serial
  Serial.println(Serial.available());
  if ( Serial.available() ) {
    Serial.println("serial is available");

    ch = (uint8_t) Serial.read();

    if (cmd_state == UNDEFINED) {
      // UNDEFINED----------------------
      if ((char)ch != '(' ) {
        //Serial.print("\nUndefined ERROR: expecting (, instead  ");Serial.println((char) ch);
        //Serial.println((char *)cmdbuf);
        cindx = 0;
        memset(cmdbuf, 0, sizeof(cmdbuf));
      } else {
        cmd_state = START;
        cindx = 0;
      }
    } else if (cmd_state == START) {
      // START ------------------------
      Serial.println("first else if");
      if (cindx >= CMDBUFSIZE ) {
        Serial.print("Start Error: cmd buf overflow: ");Serial.println((char *)cmdbuf);
        Serial.print("Last char: "); Serial.println(ch);
        cindx = 0;
        memset(cmdbuf, 0, sizeof(cmdbuf));
      } else if ((char)ch == ')' ) {
        // ------- DO COMMAND -------------------
        //Serial.println("test");
        //Serial.println((char *)testbuf);
        //Serial.println("Start before do_cmd");
        //Serial.println((char *)cmdbuf);
        
        result = do_cmd(cmdbuf);
        
        cmd_state = END;
        cindx = 0;
        memset(cmdbuf, 0, sizeof(cmdbuf));        
      } else if ((char)ch == '(' ) {
        //Serial.print("Start ERROR: new ( before end: ");Serial.println(ch);
        //Serial.println((char *)cmdbuf);
        cindx = 0;
        memset(cmdbuf, 0, sizeof(cmdbuf));
      } else {
        Serial.println("in else if");
        //Serial.println("tick");
        cmdbuf[cindx] = ch;
        cindx++ ;
      }
    } else if (cmd_state == END) {
      Serial.println("in else if");
      // END -------------------------
      if ((char)ch != '(' ) {
        //Serial.print("End ERROR: not ) "); Serial.println(ch);
        //Serial.println((char *)cmdbuf);
        cindx = 0;
        memset(cmdbuf, 0, sizeof(cmdbuf));
      } else {
        cmd_state = START;
        cindx = 0;
        memset(cmdbuf, 0, sizeof(cmdbuf));
      }      
    } 
  }
  delay(100);
}

uint16_t do_cmd(uint8_t cmd_buffer[]) {
  Serial.println("in uint16");
  Serial.print("in do_cmd");
  if ((char)cmd_buffer[0] == 'g') {
    Serial.print("(c,ipaddr=");
    Serial.print(WiFi.localIP());
    Serial.println(")\n");
  } else if ((char)cmd_buffer[0] == 'k') {
    Serial.println("in k");
    ssidConfig(cmd_buffer);
  } else if ((char)cmd_buffer[0] == 'q') {
    lightSleep();
  }
  return 1;
}

void ssidConfig(uint8_t cmd_buffer[]) {
  Serial.print("in ssidConfig");
  char *target = NULL;
  char *start, *end, *endCMD;
  uint8_t ssidName[CMDBUFSIZE +2] = {0};
  uint8_t pw[CMDBUFSIZE +2] = {0};

  // Parse cmd_buffer
  Serial.println("start ssidconfig");
  if (start = strstr((char *)cmd_buffer, ",") ) {
    Serial.println("found first , ");
    start += 1;
    if (end = strstr(start, ",") ){
      memcpy(ssidName, start, end - start);
      ssidName[end - start] = '\0';
    } else {
      Serial.println("No Second comma");
      return;
    }
  } else {
    Serial.println("Did not find first comma");
    return;
  }
  end += 1;
  strcpy((char *)pw,end);
  Serial.println((char *)ssidName);
  Serial.println((char*)pw);
  wifiConfig((char *)ssidName, (char *)pw);
}

void lightSleep() {
  Serial.print("in lightSleep");
  esp_err_t result;

  //esp_sleep_enable_timer_wakeup(5000000);

  //Configure GPIO33 as ext0 wake up source for HIGH logic level
  esp_sleep_enable_ext0_wakeup(GPIO_NUM_14,1);

  result = uart_set_wakeup_threshold(UART_NUM_0, 3);
  if (result != ESP_OK) {
    Serial.print("ESP UART0 threshold failed. "); Serial.println(result);
    return;
  }
  result = esp_sleep_enable_uart_wakeup(UART_NUM_0);
  if (result != ESP_OK) {
    Serial.print("ESP UART0 wakeup failed. "); Serial.println(result);
    return;
  }
  //result = uart_set_wakeup_threshold(UART_NUM_1, 3);
  //if (result != ESP_OK) {
  //  Serial.print("ESP UART1 threshold failed. "); Serial.println(result);
  //  return;
  //}  
  //result = esp_sleep_enable_uart_wakeup(UART_NUM_1);
  //if (result != ESP_OK) {
  //  Serial.print("ESP UART1 wakeup failed. "); Serial.println(result);
  //  return;
  //}  
  //result = esp_sleep_enable_wifi_wakeup();
  //if (result != ESP_OK) {
  //  Serial.print("ESP WiFi wakeup failed. "); Serial.println(result);
  //  return;
  //}  
  result = esp_light_sleep_start() ;    
  if (result != ESP_OK) {
    Serial.print("...ESP light sleep failed. "); Serial.println(result);
  }
}