#include "Arduino.h"
#include "soc/soc.h"           // Disable brownour problems
#include "soc/rtc_cntl_reg.h"  // Disable brownour problems
#include "driver/rtc_io.h"
#include "esp_camera.h"
#include <JPEGDEC.h>
#include <Arduino_GFX_Library.h>
/* More data bus class: https://github.com/moononournation/Arduino_GFX/wiki/Data-Bus-Class */
Arduino_DataBus *bus = new Arduino_ESP32SPI(15 /*DF_GFX_DC*/, 2 /*DF_GFX_CS*/ , 14 /*DF_GFX_SCK*/, 13/*DF_GFX_MOSI*/, -1 /* DF_GFX_MISO*/, VSPI /* spi_num */);
/* More display class: https://github.com/moononournation/Arduino_GFX/wiki/Display-Class */
Arduino_GFX *gfx = new Arduino_GC9A01(bus, -1 /* RST */, 0 /* rotation */, true /* IPS */);


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


// variables
JPEGDEC jpeg;
byte t = 0;
byte takeNextPhotoFlag = 1;



void setup() {
  // for cam init
  WRITE_PERI_REG(RTC_CNTL_BROWN_OUT_REG, 0); //disable brownout detector // NEEDED ????

  // start serial
  Serial.begin(115200);
  Serial.println("System startred");

  // start gfx
  Serial.print("Starting gfx ..... ");
  gfx->begin();
  gfx->fillScreen(BLACK);
  gfx->setTextColor(RED);
  gfx->setTextSize(2, 2, 1);
  gfx->setCursor(45, 75);
  gfx->println("GFX -> OK");
  Serial.println("Done");

  // start camera
  Serial.print("Starting camera init ..... ");
  gfx->setCursor(45, 110);
  gfx->println("Cam ->");
  initCamera();
  gfx->setCursor(95, 110);
  gfx->println("OK");
  Serial.println("Done");


  //  gfx->setCursor(25, 145);
  //  gfx->println("Watching!");




  delay(4000); // 4 seconds
}



void loop() {


  // if takeNextPhotoFlag is set
  if (takeNextPhotoFlag)
  {
    takePhoto();
//    takeNextPhotoFlag = 0;
  }


  // we only read serial if we use the uart
  if (Serial.available())
  {
    switch (Serial.read())
    {
      case 'p':
      case 'P':
        takeNextPhotoFlag = 1;
        break;
      default:
        Serial.println("not supported!!!");
        break;
    } //end switch
  } //end if
} //end loop




/* ***************************************************************** */
/* jpegDrawCallback                                                      */
/* ***************************************************************** */
// pixel drawing callback
static int jpegDrawCallback(JPEGDRAW *pDraw)
{
  //Serial.printf("Draw pos = %d,%d. size = %d x %d\n", pDraw->x, pDraw->y, pDraw->iWidth, pDraw->iHeight);
  gfx->draw16bitBeRGBBitmap(pDraw->x, pDraw->y, pDraw->pPixels, pDraw->iWidth, pDraw->iHeight);
  return 1;
} //end draw pixwl




/* ***************************************************************** */
/* INIT CAMERA                                                       */
/* ***************************************************************** */
void initCamera()
{
  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;

  Serial.print("psramFound() = " + String(psramFound()));
  Serial.print("...");

  config.frame_size = FRAMESIZE_SVGA; //FRAMESIZE_UXGA; // FRAMESIZE_ + QVGA|CIF|VGA|SVGA|XGA|SXGA|UXGA //FRAMESIZE_QVGA
  config.jpeg_quality = 10;
  config.fb_count = 2;


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



/* ***************************************************************** */
/* TAKE PHOTO                                                        */
/* ***************************************************************** */
void takePhoto()
{
  delay(50);
  camera_fb_t * fb = NULL;

  // Take Picture with Camera
  fb = esp_camera_fb_get();
  if (!fb) {
    Serial.println("Camera capture failed");
    return;
  }
  else
  {
    Serial.println("photo taken ...");

//    long lTime;
    int stateOpen = jpeg.openRAM((uint8_t *)fb->buf, fb->len, jpegDrawCallback);
    //jpeg.setMaxOutputSize(1600);
    jpeg.setPixelType(RGB565_BIG_ENDIAN);
//    Serial.println(stateOpen);
    if (stateOpen)
    {
//      lTime = micros();
      if (jpeg.decode(20,45, JPEG_SCALE_QUARTER)) // JPEG_SCALE_HALF, JPEG_SCALE_QUARTER, JPEG_SCALE_EIGHTH
      {
//        lTime = micros() - lTime;
//        Serial.printf("%d x %d image, decode time = %d us\n", jpeg.getWidth(), jpeg.getHeight() , (int)lTime);
      }
      else
      {
        Serial.println("FAIL TO decode ????");
      }
      jpeg.close();
    }
    else
    {
      Serial.println("FAIL TO OPEN RAM ????");
    }



  } //end if not fail capture

  // clear
  esp_camera_fb_return(fb);

} //endd phototaken