/* Moving Extraction example with OpenCV */
/* Public Domain             */

#include <Arduino.h>
#include <Camera.h>
#include <opencv.hpp>
#include <SD.h>
#include <SPI.h>
#include <RTC.h>
RTC rtc;

#define INTERVAL 1000
#define TH1 700 //しきい値１　警告
#define TH2 1000 //しきい値２　電源オフ　
#define LEDR 12 //Red LED 
#define LEDY 9 //Yellow Led
#define LEDG 11//Green Led
#define SW01 8//POWER SW

#define IMAGE_HW 320
#define IMAGE_VW 240
using namespace cv;

static Camera camera(IMAGE_HW, IMAGE_VW);

void dateTime(uint16_t* date, uint16_t* time) {
  int year, mon, day, hour, min, sec, week;
  rtc.getDateTime(year, mon, day, hour, min, sec, week);
  *date = FAT_DATE(year, mon, day);
  *time = FAT_TIME(hour, min, sec);
}

void setup() {

  Serial.begin(9600);
  Serial.println("start");
  pinMode(PIN_SW, INPUT);
  pinMode(PIN_LED_GREEN, OUTPUT);
  pinMode(PIN_LED_BLUE, OUTPUT);
  pinMode(PIN_LED_RED, OUTPUT);

  pinMode(LEDR  , OUTPUT);
  pinMode(LEDY  , OUTPUT);
  pinMode(LEDG  , OUTPUT);
  pinMode(SW01  , OUTPUT);

  digitalWrite(SW01, 1);

  digitalWrite(LEDG, 1);
  delay(1000);
  digitalWrite(LEDY, 1);
  delay(1000);
  digitalWrite(LEDR, 1);
  delay(3000);
  digitalWrite(LEDR, 0);
  digitalWrite(LEDY, 0);
  digitalWrite(LEDG, 0);
  delay(500);
  digitalWrite(LEDG, 1);


  camera.begin();
  rtc.begin();

  rtc.setDateTime(2017, 6, 17, 14, 1, 0);
  SdFile::dateTimeCallback(&dateTime);

  if (!SD.begin()) {
    Serial.println("Card failed, or not present.");
    digitalWrite(PIN_LED_RED, HIGH);
    while (1)
      ;
  } 
  else {
    Serial.println("Card founded.");
  }
  Serial.println("System start.");
  digitalWrite(PIN_LED_GREEN, HIGH);

}

void loop() {
  Mat img_raw(IMAGE_VW, IMAGE_HW, CV_8UC2, camera.getImageAdr());
  Mat src, diff, srcFloat, dstFloat, diffFloat,im1,im2;

  static int count = 0;

  while(1) { 
    if( count == 0){
      cvtColor(img_raw, im1, COLOR_YUV2GRAY_YUYV); //covert from YUV to GRAY
      delay(INTERVAL); 
    } 
    cvtColor(img_raw, im2, COLOR_YUV2GRAY_YUYV); //covert from YUV to GRAY
    delay(INTERVAL);  
    absdiff(im1, im2,diff);//初期画像im1と現在の画像im2を比較
    threshold(diff, diff, 128, 255, THRESH_BINARY); //白黒（2値化する）
    size_t jpegSize = camera.createJpeg(IMAGE_HW, IMAGE_VW, diff.data,Camera::FORMAT_GRAY);

    int sa = countNonZero(diff);			// 白色領域の面積(ピクセル数)を計算
    Serial.println(sa);	
    if (sa > TH2){
      Serial.println("bad");
      digitalWrite(SW01, 0);
      digitalWrite(LEDR, 1);
      digitalWrite(LEDY, 0);
      digitalWrite(LEDG, 0);
    }
    else     if (sa > TH1){
      Serial.println("warnig");
      digitalWrite(SW01, 1);
      digitalWrite(LEDR, 0);
      digitalWrite(LEDY, 1);
      digitalWrite(LEDG, 0);
    }
    else{
      Serial.println("good");
      digitalWrite(SW01, 1);
      digitalWrite(LEDR, 0);
      digitalWrite(LEDY, 0);
      digitalWrite(LEDG, 1);

    }

    char filename[13];
    sprintf(filename, "image%d.jpg", count);
    File file = SD.open(filename, FILE_WRITE);
    if (file) {
      digitalWrite(PIN_LED_BLUE, HIGH);
      uint8_t* adr = camera.getJpegAdr();
      for (size_t i = 0; i < jpegSize; i++) {

        file.write(*adr);
        adr++;
      }
      file.close();
      Serial.print("Saved a picture as ");
      Serial.println(filename);
      digitalWrite(PIN_LED_BLUE, LOW);
      count++;
    } 
    else {
      Serial.println("Failed to open file.");
      digitalWrite(PIN_LED_RED, HIGH);
      while (1)
        ;
    }




  }

}