// Daniel Shiffman
// http://codingtra.in
// http://patreon.com/codingtrain
// Code for: https://youtu.be/QLHMtE5XsMs

import processing.video.*;
import processing.serial.*;

Capture video;
Serial myPort;

PImage pprev;
PImage prev;
PImage comprasion; //This will be the result of combining pprev with prev

float threshold = 60;

float motionX = 0;
float motionY = 0;

float lerpX = 0;
float lerpY = 0;


void setup()
{
  size(640, 480);
  //myPort = new Serial(this, "COM1", 9600);
  //String[] cameras = Capture.list();
  //printArray(cameras);
  video = new Capture(this, width, height, 30);
  video.start();
  pprev = createImage(640, 480, RGB);
  prev = createImage(640, 480, RGB);
  comprasion = createImage(640, 480, RGB);
  // Start off tracking for red
}


void captureEvent(Capture video)
{
  prev.copy(video, 0, 0, video.width, video.height, 0, 0, prev.width, prev.height);
  prev.updatePixels();

  video.read(); //loading first frame
  pprev.copy(video, 0, 0, video.width, video.height, 0, 0, prev.width, prev.height);
  pprev.updatePixels();

  video.read(); //loading second frame
}

void draw()
{
  video.loadPixels();
  pprev.loadPixels();
  prev.loadPixels();
  comprasion.loadPixels();

  image(video, 0, 0);

  //threshold = map(mouseX, 0, width, 0, 100);
  threshold = 40;
  int count = 0;
  float avgX = 0;
  float avgY = 0;

  loadPixels();

  for (int x = 0; x < video.width; x++ )
  {
    for (int y = 0; y < video.height; y++ )
    {
      int loc = x + y * video.width;

      comprasion = GetFrame(pprev, prev);

      color currentColor = video.pixels[loc];
      float r1 = red(currentColor);
      float g1 = green(currentColor);
      float b1 = blue(currentColor);

      color prevColor = comprasion.pixels[loc];
      float r2 = red(prevColor);
      float g2 = green(prevColor);
      float b2 = blue(prevColor);


      float d = distSq(r1, g1, b1, r2, g2, b2);

      if (d > threshold*threshold) {
        //stroke(255);
        //strokeWeight(1);
        //point(x, y);
        avgX += x;
        avgY += y;
        count++;
        //myPort.write('1');
        //pixels[loc] = color(0);
      } else {
        //pixels[loc] = color(255);
      }
    }
  }
  updatePixels();

  //We decide to detect motion or not
  if (count > 4000)
  {
    //myPort.write('1');
    motionX = avgX / count;
    motionY = avgY / count;
    // Draw a circle at the tracked pixel


  lerpX = lerp(lerpX, motionX, 0.2);
  lerpY = lerp(lerpY, motionY, 0.2);

  fill(255, 50, 50);
  strokeWeight(2.0);
  stroke(0);
  ellipse(lerpX, lerpY, 36, 36);
  }
  else
  {
    //myPort.write('0');
  }
  //image(video, 0, 0);y
  //image(prev, 100, 0, 100, 100);

  //println(mouseX, threshold);
}

float distSq(float x1, float y1, float z1, float x2, float y2, float z2)
{
  float d = (x2-x1)*(x2-x1) + (y2-y1)*(y2-y1) +(z2-z1)*(z2-z1);
  return d;
}

PImage GetFrame (PImage a, PImage b) //This fuction should return an average frame of two previous frames.
{
  PImage c = createImage(640, 480, RGB);

  a.loadPixels();
  b.loadPixels();
  c.loadPixels();

   for (int x = 0; x < video.width; x++ )
   {
    for (int y = 0; y < video.height; y++ )
    {
      int loc = x + y * video.width;

      c.pixels[loc] = a.pixels[loc]*(99/100) + b.pixels[loc]*(1/100); //This should fill c image with correct pixels.

    }
   }
   updatePixels();
  return c;
}