#include <SPI.h>
#include <Ethernet.h>
#include <WiFi.h>
#include "Adafruit_VC0706.h"
#include "SPI.h"
#include "SD.h"
#include "FS.h"

#define AUDIO_ON true


// Digital I/O used
#define ETH_CS        33
#define SD_CS         4
#define SPI_MOSI      18
#define SPI_MISO      19
#define SPI_SCK       5
#define I2S_DOUT      25
#define I2S_BCLK      27
#define I2S_LRC       26

#if AUDIO_ON
  #include "src/Audio.h"
  Audio audio;
#endif

#define cameraconnection Serial1

Adafruit_VC0706 cam = Adafruit_VC0706(&cameraconnection);

// Enter a MAC address for your controller below.
// Newer Ethernet shields have a MAC address printed on a sticker on the shield
byte mac[] = {
  0x00, 0xAA, 0xBB, 0xCC, 0xDE, 0x02
};


// Initialize the Ethernet server library
// with the IP address and port you want to use
// (port 80 is default for HTTP):
//EthernetServer server(80);

TaskHandle_t Task1;
TaskHandle_t Task2;

void setup() {
    // Open serial communications and wait for port to open:
  Serial.begin(115200);
  pinMode(SD_CS, OUTPUT);
  pinMode(ETH_CS, OUTPUT);
  digitalWrite(SD_CS, HIGH);
  digitalWrite(ETH_CS, HIGH);
  
  while (!Serial) {
    ; // wait for serial port to connect. Needed for native USB port only
  }
  // You can use Ethernet.init(pin) to configure the CS pin
  SPI.begin(SPI_SCK, SPI_MISO, SPI_MOSI);
  Ethernet.init(ETH_CS);
  if (!SD.begin(SD_CS)) Serial.println("Card failed or not present");
  else Serial.println("Card present");
  
  // Set WiFi to station mode and disconnect from an AP if it was previously connected
  WiFi.mode(WIFI_STA);
  WiFi.disconnect();
  Serial.println("Setup WiFi done");  
  
  // start the Ethernet connection:
  Serial.println("Initialize Ethernet with DHCP:");
  if (Ethernet.begin(mac) == 0) {
    Serial.println("Failed to configure Ethernet using DHCP");
    if (Ethernet.hardwareStatus() == EthernetNoHardware) {
      Serial.println("Ethernet shield was not found.  Sorry, can't run without hardware. :(");
    } else if (Ethernet.linkStatus() == LinkOFF) {
      Serial.println("Ethernet cable is not connected.");
    }
  }
  
  // print your local IP address:
  Serial.print("My IP address: ");
  Serial.println(Ethernet.localIP());  
  
#if AUDIO_ON
  Serial.println("Initialize I2S Sound.");
  audio.setPinout(I2S_BCLK, I2S_LRC, I2S_DOUT);
  audio.setVolume(12); // 0...21
#endif

  Serial.println("Initialize camera.");
  if (cam.begin()) {
    Serial.println("Camera Found:");
  } else {
    Serial.println("No camera found?");
    return;
  }
  // Print out the camera version information (optional)
  char *reply = cam.getVersion();
  if (reply == 0) {
    Serial.print("Failed to get version");
  } else {
    Serial.println("-----------------");
    Serial.print(reply);
    Serial.println("-----------------");
  }
  
//  //create a task that will be executed in the Task1code() function, with priority 1 and executed on core 0
//  xTaskCreatePinnedToCore(
//                    Task1code,   /* Task function. */
//                    "Task1",     /* name of task. */
//                    10000,       /* Stack size of task */
//                    NULL,        /* parameter of the task */
//                    1,           /* priority of the task */
//                    &Task1,      /* Task handle to keep track of created task */
//                    0);          /* pin task to core 0 */                  
//  delay(500); 

  //create a task that will be executed in the Task2code() function, with priority 1 and executed on core 1
  xTaskCreatePinnedToCore(
                    Task2code,   /* Task function. */
                    "Task2",     /* name of task. */
                    10000,       /* Stack size of task */
                    NULL,        /* parameter of the task */
                    1,           /* priority of the task */
                    &Task2,      /* Task handle to keep track of created task */
                    1);          /* pin task to core 1 */
  delay(500); 

//  // start the server
//  server.begin();
//  Serial.print("server is at ");
//  Serial.println(Ethernet.localIP());
  
}

volatile bool take = false;

void loop() {
  if(Serial.available()){ // put streamURL in serial monitor
            #if AUDIO_ON
              audio.stopSong();
            #endif
            String r=Serial.readString(); r.trim();
            if(r.length()>=1)
              {
                if (r=="0") 
                  {
                    #if AUDIO_ON
                      Serial.println("Playing....");
                      audio.connecttoSD("/game-thrones-song.mp3");
                    #endif
                  }
                else  if (r=="1")  wifi_scan();
                else  if (r=="2")  take=true;
              }
        }
   #if AUDIO_ON
      audio.loop();
   #endif
}

void takePicture()
{
    // Set the picture size - you can choose one of 640x480, 320x240 or 160x120 
  // Remember that bigger pictures take longer to transmit!
  
  cam.setImageSize(VC0706_640x480);        // biggest
  //cam.setImageSize(VC0706_320x240);        // medium
  //cam.setImageSize(VC0706_160x120);          // small

  // You can read the size back from the camera (optional, but maybe useful?)
  uint8_t imgsize = cam.getImageSize();
  Serial.print("Image size: ");
  if (imgsize == VC0706_640x480) Serial.println("640x480");
  if (imgsize == VC0706_320x240) Serial.println("320x240");
  if (imgsize == VC0706_160x120) Serial.println("160x120");

//  Serial.println("Snap in 3 secs...");
//  delay(3000);

  if (! cam.takePicture()) 
    Serial.println("Failed to snap!");
  else 
    Serial.println("Picture taken!");
  
  // Create an image with the name IMAGExx.JPG
  char filename[14];
  strcpy(filename, "/IMAGE00.JPG");
  for (int i = 0; i < 100; i++) {
    filename[6] = '0' + i/10;
    filename[7] = '0' + i%10;
    // create if does not exist, do not open existing, write, sync after write
    if (! SD.exists(filename)) {
      break;
    }
  }
  
  // Open the file for writing
  File imgFile = SD.open(filename, FILE_APPEND);
  if(!imgFile) {
    Serial.println("Failed to open file for writing");
    return;
  }
  Serial.print("Creating file ");
  Serial.println(&filename[1]);
  // Get the size of the image (frame) taken  
  uint16_t jpglen = cam.frameLength();
  Serial.print("Storing ");
  Serial.print(jpglen, DEC);
  Serial.println(" byte image.");

  int32_t time = millis();
  
  // Read all the data up to # bytes!
  byte wCount = 0; // For counting # of writes
  while (jpglen > 0) {
    // read 32 bytes at a time;
    uint8_t *buffer;
    uint8_t bytesToRead = 64; //min(32, jpglen); // change 32 to 64 for a speedup but may not work with all setups!
    if (jpglen < bytesToRead)  bytesToRead = jpglen;
    buffer = cam.readPicture(bytesToRead);
    imgFile.write(buffer, bytesToRead);
    if(++wCount >= 64) { // Every 2K, give a little feedback so it doesn't appear locked up
      Serial.print('.');
      wCount = 0;
    }
    //Serial.print("Read ");  Serial.print(bytesToRead, DEC); Serial.println(" bytes");
    jpglen -= bytesToRead;
  }
  imgFile.close();
  cam.resumeVideo();
  time = millis() - time;
  Serial.println("done!");
  Serial.print(time); Serial.println(" ms elapsed");
}

void wifi_scan()
  {
    Serial.println("scan start");
      // WiFi.scanNetworks will return the number of networks found
      int n = WiFi.scanNetworks();
      Serial.println("scan done");
      if (n == 0) {
          Serial.println("no networks found");
      } else {
          Serial.print(n);
          Serial.println(" networks found");
          for (int i = 0; i < n; ++i) {
              // Print SSID and RSSI for each network found
              Serial.print(i + 1);
              Serial.print(": ");
              Serial.print(WiFi.SSID(i));
              Serial.print(" (");
              Serial.print(WiFi.RSSI(i));
              Serial.print(")");
              Serial.println((WiFi.encryptionType(i) == WIFI_AUTH_OPEN)?" ":"*");
              delay(10);
          }
      }
      Serial.println("");
  }

//void Task1code( void * pvParameters ){
//  for(;;){
//    
//  }
//}

void Task2code( void * pvParameters ){
  for(;;){
    if (take) 
      {
        takePicture();
        take = false;
      }
  }
}