#include <TaskScheduler.h>



#include <Adafruit_GFX.h>
#include <Adafruit_NeoPixel.h>
#include <Adafruit_NeoMatrix.h>

#include "RGB.h"

#define CAMERA_MODEL_AI_THINKER

#define PIN 13

Adafruit_NeoMatrix matrix = Adafruit_NeoMatrix(8, 8, PIN,
    NEO_MATRIX_TOP + NEO_MATRIX_LEFT + NEO_MATRIX_ROWS + NEO_MATRIX_PROGRESSIVE,
    NEO_GRB + NEO_KHZ800);


#include "camera_pins.h" // Must include this after setting
#include "esp_camera.h"
#include "FS.h"     // SD Card ESP32
#include "SD_MMC.h" // SD Card ESP32


#define FRAME_SIZE FRAMESIZE_QVGA // Frambuffer size (matches resolution below)
#define WIDTH 320                 // Resolution Width
#define HEIGHT 240                // Resolution height
#define BLOCK_SIZE 4              // Size of each sensor block on the display (reduced granularity for speed)
#define W (WIDTH / BLOCK_SIZE)
#define H (HEIGHT / BLOCK_SIZE)
#define BLOCK_DIFF_THRESHOLD 1.5
#define IMAGE_DIFF_THRESHOLD 0.1
#define DEBUG 0                            // Good for making changes
#define INFO 1                             // Good to see whats happening (turn off to save CPU)
#define VIEWPORT_PIXELS WIDTH / BLOCK_SIZE // 320/4 = 80 Positions to look at in left to right view plane [-------X------] X is where motion exists
#define STEPS_PER_DEGREE 6  

#define FLASH_PIN 4                        // Pin of ESP32 Flash (Led)
#define FLASH_MODE 0                       // 0 = 0ff , 1 = flash, 2 = steady




uint16_t prev_frame[H][W] = {0};
uint16_t current_frame[H][W] = {0};
uint16_t empty_frame[H][W] = {0};
long motionView[VIEWPORT_PIXELS];

int moveTo = 0;
int currentPos = 0;


// Scheduler
Scheduler tasks;

//trigger LED
void animation();
Task tmvStepper(250, TASK_FOREVER, &animation, &tasks, true);

void fadePixel(int x, int y, RGB startColor, RGB endColor, int steps, int wait) {
  for(int i = 0; i <= steps; i++) 
  {
     int newR = startColor.r + (endColor.r - startColor.r) * i / steps;
     int newG = startColor.g + (endColor.g - startColor.g) * i / steps;
     int newB = startColor.b + (endColor.b - startColor.b) * i / steps;

     matrix.drawPixel(x, y, matrix.Color(newR, newG, newB));
     matrix.show();
     delay(wait);
  }
}

// Fade full screen  from startColor to endColor
void crossFade(RGB startColor, RGB endColor, int steps, int wait) {
  for(int i = 0; i <= steps; i++) 
  {
     int newR = startColor.r + (endColor.r - startColor.r) * i / steps;
     int newG = startColor.g + (endColor.g - startColor.g) * i / steps;
     int newB = startColor.b + (endColor.b - startColor.b) * i / steps;

     matrix.fillScreen(matrix.Color(newR, newG, newB));
     matrix.show();
     delay(wait);
  }
}

void colorWipe(RGB color, uint8_t wait) {
  for(uint16_t row=0; row < 8; row++) {
    for(uint16_t column=0; column < 8; column++) {
      matrix.drawPixel(column, row, matrix.Color(color.r, color.g, color.b));
      matrix.show();
      delay(wait);
    }
  }
}

void animation(){

  if (currentPos > moveTo) 
  {
//    currentPos -+ ceil((currentPos - moveTo) /2);
//    if (ceil((currentPos - moveTo) / 2) == 0)
//    currentPos = moveTo;
  colorWipe(red, 50);
   delay(1000);
  crossFade(red, white, 120,5);
  crossFade(white, off, 120, 5);
  delay (1000);
  }
  else if (currentPos < moveTo)
  { 
//     currentPos += floor((moveTo - currentPos) / 2);
//     if (floor((moveTo - currentPos) / 2) == 0)
//        currentPos = moveTo;
 colorWipe(blue, 50);
   delay(1000);
  crossFade(blue, white, 120,5);
  crossFade(white, off, 120, 5);
  delay (1000);
  }
  else
  {
    currentPos = moveTo;
  }
//#if INFO
//    if (currentPos != moveTo)
//    {
//        Serial.print("MOVETO: ");
//        Serial.print(moveTo);
//        Serial.print(" CURRENT: ");
//        Serial.println(currentPos);
//    }
}

long viewPortToRegion(long mv[])
{
  int maxVal = 0;
  int region = 0;
  int tmpVal = 0;
  char str_tmp[9];
  // Fill each char array with the 8bits of the 10 regions
  for (int i = 0; i <10; i++)
  {
    for (int j = 0; j <8; j++)
    {
      str_tmp[j] = (mv[((i *8) + j)] == 1) ? '1' : '0';
      //Serial.println(((i*8)+j));
    }
//   // tmpVal = readAsBinaryString(str_tmp);
//#if INFO
//        Serial.print("Block: ");
//        Serial.print(i);
//        Serial.print(" STR=");
//        Serial.print(str_tmp);
//        Serial.print(" Value: ");
//        Serial.println(tmpVal);
//#endif
//        if (tmpVal > maxVal)
//        {
//            maxVal = tmpVal; // Set new uppper mark
//            region = i;      // Which viewport has the most movement.
////        }
//    
//  }
////    Serial.print("Most activity in block region:");
////    Serial.println(region);
////    return region;
//
}
      
}



void setup() {
  // put your setup code here, to run once:

    //initialize pixel brightness, text color, text wrapping options
  matrix.begin();
  matrix.setBrightness(30);
  matrix.setTextColor( matrix.Color(255, 255, 255) );
  matrix.setTextWrap(false);

  
 uint32_t Freq = 0;
    Serial.begin(115200);
    Freq = getCpuFrequencyMhz();
    Serial.print("CPU Freq = ");
    Serial.print(Freq);
    Serial.println(" MHz");
    Freq = getXtalFrequencyMhz();
    Serial.print("XTAL Freq = ");
    Serial.print(Freq);
    Serial.println(" MHz");
    Freq = getApbFrequency();
    Serial.print("APB Freq = ");
    Serial.print(Freq);
    Serial.println(" Hz");
    Serial.println("Begin Setup...");
    for (uint16_t i = 0; i < VIEWPORT_PIXELS; i++)
    {
        motionView[i] = 0; // Setup Pixel Array
    }
    pinMode(FLASH_PIN, OUTPUT);
    //    Serial.println(setup_camera(FRAME_SIZE,    PIXFORMAT_JPEG) ? "OK" : "ERR INIT");
    Serial.println(setup_camera(FRAME_SIZE, PIXFORMAT_GRAYSCALE) ? "OK" : "ERR INIT");

     Serial.println("End Setup...");
    tasks.startNow();

}



void loop() {
  // put your main code here, to run repeatedly:

  if (FLASH_MODE == 2)
        digitalWrite(FLASH_PIN, HIGH);
    if (!capture_still())
    {
#if INFO
        Serial.println("Failed capture");
#endif
        return;
    }

    if (motion_detect())
    {
#if INFO
        Serial.println("Motion detected");
#endif
    }
    update_frame();

    tasks.execute();
}

/**
 * Capture image and do down-sampling
 */
bool capture_still()
{

    if (FLASH_MODE == 1)
        digitalWrite(FLASH_PIN, HIGH);
    camera_fb_t *frame_buffer = esp_camera_fb_get();
    if (FLASH_MODE == 1)
        digitalWrite(FLASH_PIN, LOW);
    if (!frame_buffer)
        return false;
    // set all 0s in current frame
    memcpy(empty_frame, current_frame, sizeof(empty_frame)); // FAST! Memcopy vs iterations so much faster.

    // down-sample image in blocks
    for (uint32_t i = 0; i < WIDTH * HEIGHT; i++)
    {
        const uint16_t x = i % WIDTH;
        const uint16_t y = floor(i / WIDTH);
        const uint8_t block_x = floor(x / BLOCK_SIZE);
        const uint8_t block_y = floor(y / BLOCK_SIZE);
        const uint8_t pixel = frame_buffer->buf[i];
        const uint16_t current = current_frame[block_y][block_x];
        // average pixels in block (accumulate)
        current_frame[block_y][block_x] += pixel;
    }

    // average pixels in block (rescale)
    for (int y = 0; y < H; y++)
        for (int x = 0; x < W; x++)
            current_frame[y][x] /= BLOCK_SIZE * BLOCK_SIZE;

#if DEBUG
    Serial.println("Current frame:");
    print_frame(current_frame);
    Serial.println("---------------");
#endif

    esp_camera_fb_return(frame_buffer); // Needed to free up camera memory

    return true;
}

/**
 * Compute the number of different blocks
 * If there are enough, then motion happened
 */
bool motion_detect()
{
    uint16_t changes = 0;
    int lastBlock = 0;
    const uint16_t blocks = (WIDTH * HEIGHT) / (BLOCK_SIZE * BLOCK_SIZE);
    for (uint16_t y = 0; y < H; y++)
    {
        for (uint16_t x = 0; x < W; x++)
        {
            float current = current_frame[y][x];
            float prev = prev_frame[y][x];
            float delta = abs(current - prev) / prev;

            // Fill only those areas that meet the threashold.
            if (delta >= BLOCK_DIFF_THRESHOLD)
            {
#if DEBUG
                Serial.println(delta);
                Serial.print("diff\t");
                Serial.print(y);
                Serial.print('\t');
                Serial.println(x);
#endif

                motionView[x] = 1;
                changes++;
            }
        }
    }
    if (changes == 0)
        return false; // don't need to go any further

    // Change screen data into linear (left to right) expression of data.

    moveTo = ((10 - viewPortToRegion(motionView)) * 10) * STEPS_PER_DEGREE;

// Display updates for informational purposes.
#if INFO
    Serial.print(":::");
#endif
    // Clear viewport to zero for next detection phase
    for (uint16_t i = 0; i < VIEWPORT_PIXELS; i++)
    {
#if INFO
        Serial.print(motionView[i]);
#endif
        motionView[i] = 0;
    }
#if INFO
    Serial.println(":::");
    Serial.print("Changed ");
    Serial.print(changes);
    Serial.print(" out of ");
    Serial.println(blocks);

    Serial.print("MoveTo:");
    Serial.println(moveTo);
#endif
    return (1.0 * changes / blocks) > IMAGE_DIFF_THRESHOLD;
}

/**
 * Copy current frame to previous
 */
void update_frame()
{
    memcpy(prev_frame, current_frame, sizeof(prev_frame)); // FAST! Memcopy vs iterations so much faster.
}

/**
 * For serial debugging
 * @param frame
 */
void print_frame(uint16_t frame[H][W])
{
    for (int y = 0; y < H; y++)
    {
        for (int x = 0; x < W; x++)
        {
            Serial.print(frame[y][x]);
            Serial.print('\t');
        }

        Serial.println();
    }
}

/**
 *  Camera configuration values
 */
bool setup_camera(framesize_t frameSize, pixformat_t PIXEL_FORMAT)
{
    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 = PIXEL_FORMAT;
    config.frame_size = frameSize;
    config.jpeg_quality = 12;
    config.fb_count = 1;

    esp_camera_deinit();
    bool ok = esp_camera_init(&config) == ESP_OK;

    sensor_t *sensor = esp_camera_sensor_get();
    sensor->set_framesize(sensor, frameSize);

    return ok;
}
int readBinaryString(char *s)
{
    int result = 0;
    while (*s)
    {
        result <<= 1;
        if (*s++ == '1')
            result |= 1;
    }
    return result;
}

void captureSnapshotToSD()
{
    Serial.println(setup_camera(FRAME_SIZE, PIXFORMAT_JPEG) ? "OK" : "ERR INIT");
    esp_camera_fb_get();
    Serial.println(setup_camera(FRAME_SIZE, PIXFORMAT_GRAYSCALE) ? "OK" : "ERR INIT");
}