#include <Arduino.h>
#include <XModem.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"
#include <EEPROM.h>            // read and write from flash memory
#include <Wire.h>
#include <BME280_t.h>                                                   // import BME280 template library

char bufout[10];
BME280<> BMESensor;                             

// 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;
camera_fb_t * fb = NULL;
XModem xmodem(&Serial, ModeYModem);

typedef void (*action_func_t)(char *aLine);

typedef struct {
  char command[7+1];    // max 7 characters plus '\0' terminator
  action_func_t action;
} command_action_t;


void toLower(char *s) {
  while (*s) {
    if (isupper(*s)) {
      *s += 0x20;
    }
    s++;
  }
}

void print_commands(char *aLine);

void reboot_esp32(char *aLine)
{
  Serial.println("[OK] GOING TO REBOOT");
  delay(1000);
  ESP.restart();
}

void set_speed(char *aLine)
{
  char *cmd = strtok(aLine, " \t");
  char *speed = strtok(NULL," \t");
  Serial.updateBaudRate(strtoul(speed,nullptr,10));
  Serial.print("[OK] SETTING SPEED");
}


void capture_image(char *aLine)
{
  fb = esp_camera_fb_get(); 
  Serial.println("[OK] CAPTURED IMAGE");
}

void free_image(char *aLine)
{
  esp_camera_fb_return(fb); 
  Serial.println("[OK] FREE CAPTURED IMAGE");
}

void recv_ymodem(char *aLine)
{
  xmodem.sendFile(fb->buf, fb->len, "camera.jpg");
  Serial.println("[OK] SEND IMAGE");
  free_image(aLine);
}

void print_measurement(char *aLine)
{
  BMESensor.refresh();                                                  // read current sensor dat
  Serial.printf("temperature=%.2f humidity=%.2f pressure=%.2f\n",BMESensor.temperature,BMESensor.humidity,BMESensor.pressure  / 100.0F);
  return;
}

void set_brightness(char *aLine)
{
  char *cmd = strtok(aLine, " \t");
  char *str = strtok(NULL," \t");
  sensor_t * s = esp_camera_sensor_get();
  s->set_brightness(s, strtol(str,nullptr,10));     // -2 to 2
  Serial.print("[OK] SETTING BRIGHTNESS");
}

void set_saturation(char *aLine)
{
  char *cmd = strtok(aLine, " \t");
  char *str = strtok(NULL," \t");
  sensor_t * s = esp_camera_sensor_get();
  s->set_saturation(s, strtol(str,nullptr,10));     // -2 to 2
  Serial.print("[OK] SETTING SATURATION");
}

void set_contrast(char *aLine)
{
  char *cmd = strtok(aLine, " \t");
  char *str = strtok(NULL," \t");
  sensor_t * s = esp_camera_sensor_get();
  s->set_contrast(s, strtol(str,nullptr,10));     // -2 to 2
  Serial.print("[OK] SETTING CONTRAST");
}

const command_action_t commands[] = {
  // Name of command user types, function that implements the command.
  {"reboot", reboot_esp32},
  {"bright",set_brightness},
  {"satur",set_saturation},
  {"contr",set_contrast},
  {"speed", set_speed},
  {"capture", capture_image},
  {"free", free_image},
  {"measure", print_measurement},
  {"rb", recv_ymodem},
  {"help", print_commands},
  {"?", print_commands},
};

void print_commands(char *aLine) {
  Serial.print(commands[0].command);
  for (size_t i = 1; i < sizeof(commands)/sizeof(commands[0]); i++) {
    Serial.print(','); Serial.print(commands[i].command);
  }
  Serial.println();
}


void execute(char *aLine) {
  if (aLine == NULL || *aLine == '\0') return;
  char *cmd = strtok(aLine, " \t");
  if (cmd == NULL || *cmd == '\0') return;
  toLower(cmd);
  for (size_t i = 0; i < sizeof(commands)/sizeof(commands[0]); i++) {
    if (strcmp(cmd, commands[i].command) == 0) {
      commands[i].action(aLine);
      return;
    }
  }
  Serial.println("command not found");
}


void setup() {
  WRITE_PERI_REG(RTC_CNTL_BROWN_OUT_REG, 0); //disable brownout detector

  Serial.begin(460800);
  while (!Serial) {
    ; // wait for serial port to connect. Needed for Leonardo only
  }

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(psramFound()){
    config.frame_size = FRAMESIZE_UXGA; // FRAMESIZE_ + QVGA|CIF|VGA|SVGA|XGA|SXGA|UXGA
    config.jpeg_quality = 7;
    config.fb_count = 2;
  } else {
    config.frame_size = FRAMESIZE_SVGA;
    config.jpeg_quality = 10;
    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;
  }
  
  sensor_t * s = esp_camera_sensor_get();
  s->set_brightness(s, 0);     // -2 to 2
  s->set_contrast(s, 0);       // -2 to 2
  s->set_saturation(s, 0);     // -2 to 2
  s->set_special_effect(s, 0); // 0 to 6 (0 - No Effect, 1 - Negative, 2 - Grayscale, 3 - Red Tint, 4 - Green Tint, 5 - Blue Tint, 6 - Sepia)
  s->set_whitebal(s, 1);       // 0 = disable , 1 = enable
  s->set_awb_gain(s, 1);       // 0 = disable , 1 = enable
  s->set_wb_mode(s, 0);        // 0 to 4 - if awb_gain enabled (0 - Auto, 1 - Sunny, 2 - Cloudy, 3 - Office, 4 - Home)
  s->set_exposure_ctrl(s, 1);  // 0 = disable , 1 = enable
  s->set_aec2(s, 0);           // 0 = disable , 1 = enable
  s->set_ae_level(s, 0);       // -2 to 2
  s->set_aec_value(s, 300);    // 0 to 1200
  s->set_gain_ctrl(s, 1);      // 0 = disable , 1 = enable
  s->set_agc_gain(s, 0);       // 0 to 30
  s->set_gainceiling(s, (gainceiling_t)0);  // 0 to 6
  s->set_bpc(s, 0);            // 0 = disable , 1 = enable
  s->set_wpc(s, 1);            // 0 = disable , 1 = enable
  s->set_raw_gma(s, 1);        // 0 = disable , 1 = enable
  s->set_lenc(s, 1);           // 0 = disable , 1 = enable
  s->set_hmirror(s, 0);        // 0 = disable , 1 = enable
  s->set_vflip(s, 0);          // 0 = disable , 1 = enable
  s->set_dcw(s, 1);            // 0 = disable , 1 = enable
  s->set_colorbar(s, 0);       // 0 = disable , 1 = enable


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

  Wire.begin(14,15);

  BMESensor.begin();

  print_commands("");
          
}

uint8_t bytesIn;
char aLine[80+1];


void loop() {
  if (Serial.available() > 0) {
    int b = Serial.read();
    if (b != -1) {
      switch (b) {
        case '\n':
          if (bytesIn && aLine[bytesIn-1] == '\0') break;
        case '\r':
          Serial.println();
          aLine[bytesIn] = '\0';
          execute(aLine);
          bytesIn = 0;
          break;
        default:
          aLine[bytesIn++] = (char)b;
          if (bytesIn >= sizeof(aLine)-1) {
            aLine[bytesIn] = '\0';
            execute(aLine);
            bytesIn = 0;
          }
          break;
      }
    }
  }
}