import kinect4WinSDK.Kinect;
import processing.net.*;
import kinect4WinSDK.SkeletonData;

Kinect kinect;
color trackColor;
float threshold = 80;
PImage img;

void setup()
{
  size(1280, 960);
  background(0);
  kinect = new Kinect(this);
   trackColor = color(255, 0, 0);
   img = createImage(100, 100, RGB);
}


void draw()
{
  background(0);
  img = kinect.GetImage();
  image(img, 0, 0, 1000, 745);

  float avgX = 0;
  float avgY = 0;

  int count = 0;

  img.loadPixels();
  // Begin loop to walk through every pixel
  for (int x = 0; x < img.width; x++ ) {
    for (int y = 0; y < img.height; y++ ) {
      int loc = x + y * img.width;
      // What is current color
      color currentColor = img.pixels[loc];
      float r1 = red(currentColor);
      float g1 = green(currentColor);
      float b1 = blue(currentColor);
      float r2 = red(trackColor);
      float g2 = green(trackColor);
      float b2 = blue(trackColor);

      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++;
      }
    }
  }

  // We only consider the color found if its color distance is less than 10.
  // This threshold of 10 is arbitrary and you can adjust this number depending on how accurate you require the tracking to be.
  if (count > 0) {
    avgX = avgX / count;
    avgY = avgY / count;
    // Draw a circle at the tracked pixel
    fill(255);
    strokeWeight(4.0);
    stroke(0);
    ellipse(avgX, avgY, 24, 24);
  }
}

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;
}

void mousePressed() {
  // Save color where the mouse is clicked in trackColor variable
  int loc = mouseX + mouseY * img.width;
  trackColor = img.pixels[loc];
}