// Modification of Example: ArduCAM_Mini_5MP_Plus_Multi_Capture2SD
#include "memorysaver.h"
#include <ArduCAM.h>
#include <SPI.h>
#include <Wire.h>
//This demo can only work on OV5640_MINI_5MP_PLUS or OV5642_MINI_5MP_PLUS platform.
#if !(defined(OV5640_MINI_5MP_PLUS) || defined(OV5642_MINI_5MP_PLUS))
#error Please select the hardware platform and camera module in the ../libraries/ArduCAM/memorysaver.h file
#endif

#define FRAMES_NUM 0x00
// set pin 7 as the slave select for the digital port for ArduCam:
const int CS = 7;
#define BYTE_PACKET_SIZE 256

bool is_header = false;
int total_time = 0;
#if defined(OV5640_MINI_5MP_PLUS)
ArduCAM myCAM(OV5640, CS);
#else
ArduCAM myCAM(OV5642, CS);
#endif

uint8_t read_fifo_burst(ArduCAM myCAM);

void dump_buffer(byte buffer[], int index);

int add_to_buffer(byte buffer[], int index, byte elem);

void setup() {
  // put your setup code here, to run once:
  uint8_t vid, pid;
  uint8_t temp;
#if defined(__SAM3X8E__)
  Wire1.begin();
#else
  Wire.begin();
#endif
  Serial.begin(115200);
  Serial.println(F("ArduCAM Start!"));

  // set the CS as an output:
  pinMode(CS, OUTPUT);
  digitalWrite(CS, HIGH);

  // initialize SPI:
  SPI.begin();
  //Reset the CPLD
  myCAM.write_reg(0x07, 0x80);
  delay(100);
  myCAM.write_reg(0x07, 0x00);
  delay(100);

  while (1) {
    //Check if the ArduCAM SPI bus is OK
    myCAM.write_reg(ARDUCHIP_TEST1, 0x55);
    temp = myCAM.read_reg(ARDUCHIP_TEST1);
    if (temp != 0x55) {
      Serial.println(F("SPI interface Error!"));
      delay(1000);
      continue;
    } else {
      Serial.println(F("SPI interface OK."));
      break;
    }
  }
#if defined(OV5640_MINI_5MP_PLUS)
  while (1) {
    //Check if the camera module type is OV5640
    myCAM.rdSensorReg16_8(OV5640_CHIPID_HIGH, &vid);
    myCAM.rdSensorReg16_8(OV5640_CHIPID_LOW, &pid);
    if ((vid != 0x56) || (pid != 0x40)) {
      Serial.println(F("Can't find OV5640 module!"));
      delay(1000);
      continue;
    } else {
      Serial.println(F("OV5640 detected."));
      break;
    }
  }
#else
  while (1) {
    //Check if the camera module type is OV5642
    myCAM.rdSensorReg16_8(OV5642_CHIPID_HIGH, &vid);
    myCAM.rdSensorReg16_8(OV5642_CHIPID_LOW, &pid);
    if ((vid != 0x56) || (pid != 0x42)) {
      Serial.println(F("Can't find OV5642 module!"));
      delay(1000);
      continue;
    } else {
      Serial.println(F("OV5642 detected."));
      break;
    }
  }
#endif
  //Change to JPEG capture mode and initialize the OV5640 module
  myCAM.set_format(JPEG);
  myCAM.InitCAM();
  myCAM.set_bit(ARDUCHIP_TIM, VSYNC_LEVEL_MASK);
  myCAM.clear_fifo_flag();
  myCAM.write_reg(ARDUCHIP_FRAMES, FRAMES_NUM);
}

void loop() {

  // put your main code here, to run repeatedly:
  myCAM.flush_fifo();
  myCAM.clear_fifo_flag();

  //ETCG Notes -- Change Resolution in set_JPEG_size
#if defined(OV5640_MINI_5MP_PLUS)
  myCAM.OV5640_set_JPEG_size(OV5640_2592x1944);
  delay(1000);
#else
  myCAM.OV5642_set_JPEG_size(OV5642_320x240);
  delay(1000);
#endif
  if (Serial.available() && Serial.read() == 65) {
    //Start capture
    myCAM.start_capture();
    Serial.println(F("start capture."));
    total_time = millis();
    while (!myCAM.get_bit(ARDUCHIP_TRIG, CAP_DONE_MASK))
      ;
    Serial.println(F("CAM Capture Done."));
    total_time = millis() - total_time;
    Serial.print(F("capture total_time used (in miliseconds):"));
    Serial.println(total_time, DEC);
    total_time = millis();
    read_fifo_burst(myCAM);
    total_time = millis() - total_time;
    Serial.print(F("save capture total_time used (in miliseconds):"));
    Serial.println(total_time, DEC);
    //Clear the capture done flag
    myCAM.clear_fifo_flag();
    delay(5000);
  } else {
    Serial.println("Waiting for command (A to start capture)!");
    delay(1000);
  }
}

uint8_t read_fifo_burst(ArduCAM myCAM) {
  uint8_t currByte = 0;
  uint8_t prevByte = 0;
  uint32_t length = myCAM.read_fifo_length();
  static int i = 0;
  byte buffer[BYTE_PACKET_SIZE];

  Serial.print("The FIFO length is: ");
  Serial.println(length, DEC);

  if (length >= MAX_FIFO_SIZE) {
    Serial.println("Over size image.");
    return 0;
  } else if (length == 0) {
    Serial.println("Empty image");
    return 0;
  } else {
    Serial.println("Begin image processing");

    myCAM.CS_LOW();
    myCAM.set_fifo_burst();

    while (length--) {
      prevByte = currByte;
      currByte = SPI.transfer(0x00);
      Serial.println(currByte, HEX);
      if ((currByte == 0xD9) && (prevByte == 0xFF)) {
        // buffer[i++] = currByte;
        i = add_to_buffer(buffer, i, currByte);
        myCAM.CS_HIGH();
        dump_buffer(buffer, i);

        Serial.println("End read");
        is_header = false;
        myCAM.CS_LOW();
        myCAM.set_fifo_burst();
        i = 0;
        myCAM.CS_HIGH();
        return 1;
      } else if ((currByte == 0xD8) && (prevByte == 0xFF)) {
        Serial.println("Found header");

        is_header = true;
        myCAM.CS_HIGH();
        myCAM.CS_LOW();
        myCAM.set_fifo_burst();
        // buffer[i++] = prevByte;
        // buffer[i++] = currByte;
        i = add_to_buffer(buffer, i, prevByte);
        i = add_to_buffer(buffer, i, currByte);
      } else if (is_header) {
        myCAM.CS_HIGH();
        // buffer[i++] = currByte;
        i = add_to_buffer(buffer, i, currByte);
        myCAM.CS_LOW();
        myCAM.set_fifo_burst();
      }
    }
    return 0;
  }
}

void dump_buffer(byte buffer[], int index) {
  for (int i = 0; i < index; i++) {
    Serial.println(buffer[i], HEX);
  }
}

int add_to_buffer(byte buffer[], int index, byte elem) {
  if (index < BYTE_PACKET_SIZE) {
    buffer[index] = elem;
    return index + 1;
  } else {
    dump_buffer(buffer, BYTE_PACKET_SIZE);
    buffer[0] = elem;
    return 1;
  }
}