/*************************************************************************************************************************************************
 * ESP32cam test servo driver
 * 
 * if receives a serial byte from PC
 *   send it to Nano
 *   if it's 'p': take a photo and send to PC over serial
 *   if it's 'q': take a photo and display on ILI9341
 *   
 * if receives a byte from the Nano
 *   send it to PC over serial
 *   
 *************************************************************************************************************************************************/

#include "esp_camera.h"
#include "SPI.h"
#include "driver/rtc_io.h"
#include "SimpleILI9341sw.h"
#include "TBIB_Master.h"
#define CAMERA_MODEL_AI_THINKER
#include "camera_pins.h"

#define SERVOMIN  150 // This is the 'minimum' pulse length count (out of 4096)
#define SERVOMAX  600 // This is the 'maximum' pulse length count (out of 4096)
#define USMIN  600 // This is the rounded 'minimum' microsecond length based on the minimum pulse of 150
#define USMAX  2400 // This is the rounded 'maximum' microsecond length based on the maximum pulse of 600
#define SERVO_FREQ 50 // Analog servos run at ~50 Hz updates

const int pinTFTCS = 12;

camera_fb_t * fbSend = NULL; // buffer for picture data 

//-----------------------------------------------------------
// esp_err_t initialise_camera
//-----------------------------------------------------------
esp_err_t initialise_camera() {
  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_RGB565;  
//  config.pixel_format = PIXFORMAT_GRAYSCALE;  
  config.frame_size = FRAMESIZE_QVGA;
//  config.frame_size = FRAMESIZE_QQVGA;
  config.jpeg_quality = 10;
  config.fb_count = 1;

  //initialize camera
  return esp_camera_init(&config);
}

//-----------------------------------------------------------
// take_photo
//-----------------------------------------------------------
void take_photo() {
//  pinMode(4, OUTPUT);              //LED flash
//  digitalWrite(4, HIGH);            //flash LED ON 
  //take photo

  if (fbSend != NULL) 
    esp_camera_fb_return(fbSend);
  
  fbSend = esp_camera_fb_get();
  if (!fbSend) 
  {
    Serial.println("photo failed");
    while(1);   //stop
  }
  digitalWrite(4, LOW);            //flash LED OFF
}

//-----------------------------------------------------------
// display_photo
//-----------------------------------------------------------
void display_photo() {
  while (!PollTBIB()) {}; // only use SPI when both clocks are low

  pinMode(pinTFTCS, OUTPUT);
  pinMode(pinMISOa, OUTPUT);
  pinMode(pinSCLK, OUTPUT);
  pinMode(pinMOSIa, OUTPUT);
  digitalWrite(pinTFTCS, HIGH);
  digitalWrite(pinMISOa, HIGH);
  digitalWrite(pinSCLK, HIGH);
  digitalWrite(pinMOSIa, HIGH);
  delay(1);

  if (fbSend->format == PIXFORMAT_RGB565)
    DrawBitmapArrayReverse(0, 0, 320, 240, (unsigned byte *)(fbSend->buf));
//    DrawBitmapArrayReverse(0, 0, 320, 240, (unsigned short *)(fbSend->buf));
//    DrawBitmapArray(0, 0, 320, 240, (unsigned short *)(fbSend->buf));
    
  if (fbSend->format == PIXFORMAT_GRAYSCALE)
    DrawBitmapArrayByte(0, 0, 320, 240, (unsigned byte *)(fbSend->buf));

  InitTBIBpins();
}

//-----------------------------------------------------------
// camera_settings
//-----------------------------------------------------------
void camera_settings() {
  sensor_t * s = esp_camera_sensor_get();

  s->set_gain_ctrl(s, 0); // auto gain off (1 or 0)
  s->set_exposure_ctrl(s, 1); // auto exposure  (1 or 0)
  s->set_agc_gain(s, 0); // set gain manually (0 - 30)
  s->set_aec_value(s, 300); // set exposure manually (0-1200)
  s->set_contrast(s, -2); // (-2 to 2)

//  s->set_brightness(s, -2);     // -2 to 2
  
  // s->set_brightness(s, 0); // (-2 to 2) - set brightness
  // s->set_awb_gain(s, 0); // Auto White Balance?
  // s->set_lenc(s, 0); // lens correction? (1 or 0)
  // s->set_raw_gma(s, 1); // (1 or 0)?
  // s->set_quality(s, 10); // (0 - 63)
  // s->set_whitebal(s, 1); // white balance
  // s->set_wb_mode(s, 1); // white balance mode (0 to 4)
  // s->set_aec2(s, 0); // automatic exposure sensor? (0 or 1)
  // s->set_aec_value(s, 0); // automatic exposure correction? (0-1200)
  // s->set_saturation(s, 0); // (-2 to 2)
  // s->set_hmirror(s, 0); // (0 or 1) flip horizontally
  // s->set_gainceiling(s, GAINCEILING_32X); // Image gain (GAINCEILING_x2, x4, x8, x16, x32, x64 or x128)
  // s->set_sharpness(s, 0); // (-2 to 2)
  // s->set_colorbar(s, 0); // (0 or 1) - testcard
  // s->set_special_effect(s, 0);
  // s->set_ae_level(s, 0); // auto exposure levels (-2 to 2)
  // s->set_bpc(s, 0); // black pixel correction
  // s->set_wpc(s, 0); // white pixel correction
  // s->set_dcw(s, 1); // downsize enable? (1 or 0)?
}

//-----------------------------------------------------------
// send_photo_to_PC()
//-----------------------------------------------------------
void send_photo_to_PC() {
  static uint8_t *pSend = NULL; // pointer used to send picture data to PC
  uint32_t i; // counter used to send picture data to PC

  pSend = fbSend->buf;
  Serial.print("\n\n\nImage ");
  Serial.print(fbSend->width); Serial.print(" ");
  Serial.print(fbSend->height); Serial.print(" ");
  Serial.print(fbSend->len); Serial.print(" ");
  Serial.print(fbSend->format); Serial.print(":");

  for (i = 0; i < fbSend->len; i++) {
    Serial.write(*pSend); 
    pSend++;
  }
}

//-----------------------------------------------------------
// recvByte()
//  wait for a byte from the PC
//-----------------------------------------------------------
byte recvByte() {
  while (! Serial.available()) ;
  byte b = Serial.read();
  return b;
}

//-----------------------------------------------------------
// pollSerial()
//   get command byte from the PC and act on it
//     if it's 'p': take a photo and send to PC over serial
//     if it's 'q': take a photo and display on ILI9341
//     else send it to Nano
//-----------------------------------------------------------
void pollSerial() {
  if (Serial.available() > 0) {
    byte b = recvByte();

    switch (b) {
      case 'p':
        Serial.println("Send Photo");
        take_photo();
        send_photo_to_PC(); 
        break;
      case 'q':
        {
          Serial.println("Display Photo");
        take_photo();
        unsigned long t = millis();
        display_photo();
        Serial.print(millis()-t); Serial.println("mS");
        }
        break;
      
      case '0':
      case '1':
      case '2':
      case '3':
      case '4':
      case '5':
      case '6':
      case '7':
        {
          byte pos = recvByte();
          Serial.print("set pos ");
          Serial.print((char)b);
          Serial.print(" = ");
          Serial.println(pos);
          SendByteToSlave(b);
          SendByteToSlave(pos);
        }
        break;

      default:
        SendByteToSlave(b);
        Serial.print("Tx from Serial ");
        Serial.println(b);
        break;
    }
  }
}

//-----------------------------------------------------------
// recvTBIB
//     send it to PC over serial
//-----------------------------------------------------------
void recvTBIB(byte b) {
  Serial.print("TRx ");
  Serial.println(b);
}

//-----------------------------------------------------------
// setup
//-----------------------------------------------------------
void setup() 
{
  Serial.begin(115200);
  Serial.println();
  Serial.println("Booting...");

//  pinMode(4, OUTPUT);              //LED flash
//  digitalWrite(4, HIGH);
  pinMode(4, INPUT);              //GPIO for LED flash
  digitalWrite(4, LOW);
  rtc_gpio_hold_dis(GPIO_NUM_4);  //disable pin hold if it was enabled before sleeping

  /**** this code magically makes the camera initialisation work reliably ****/
  /**** it turns the camera off and on ****/
  digitalWrite(PWDN_GPIO_NUM, LOW);
  delay(10);
  digitalWrite(PWDN_GPIO_NUM, HIGH);
  delay(10);

  esp_err_t err = initialise_camera();
  
  if (err != ESP_OK) 
  {
    Serial.printf("Camera init failed: 0x%x", err);
    while(1);   //stop
  }

  delay(100);

  camera_settings();
  Serial.println("camera init OK");

  ILI9341Begin(320, 240, ILI9341_Rotation3);

//  DrawBox(0  ,  0,  106,  120, TFT_BLUE   );
//  DrawBox(106,  0,  106,  120, TFT_GREEN  );
//  DrawBox(212,  0,  106,  120, TFT_CYAN   );
//  DrawBox(0  ,  120,  106,  120, TFT_RED    );
//  DrawBox(106,  120,  106,  120, TFT_MAGENTA);
//  DrawBox(212,  120,  106,  120, TFT_YELLOW );

  Serial.println("ILI9341 init OK");  

  InitTBIBpins();
  
  Serial.println("Initialised");
}

//-----------------------------------------------------------
// main loop
//-----------------------------------------------------------
void loop() {
  pollSerial();
  PollTBIB();

//  static uint32_t i = 0;
//  static byte j = 0;
//  i++;
//  if (i >= 20000) {      
//    i = 0;
//    j++;
//    SendByteToSlave(j);
//    Serial.print("tx ");
//    Serial.println(j);
//  }
}