import JMyron.*;


final int NROWS = 50;
final int NCOLS = 50;
final float LIMIT = 20.0;

JMyron m;
Region reg1;
Region reg2;

PImage img1, img2;
int w, h;

void setup() {
  size(320,240);
  m = new JMyron();
  m.start(width,height);
  m.findGlobs(0);
  img1 = createImage(width,height,ARGB);
  img2 = createImage(width,height,ARGB);
  noFill();
  stroke(255,200,0);
  smooth();
  frameRate(15);
  w = 4;
  h = 4;

  reg1 = new Region(0,0,20,20);
  reg2 = new Region(20,20,40,40);

  
  
  m.update();
  m.imageCopy(img1.pixels);
  m.imageCopy(img2.pixels);
  img1.updatePixels();
  img2.updatePixels();
}

void draw() {
  background(0);
  img2.copy(img1,0,0,width,height,
    0,0,width,height);
  img2.updatePixels();
  m.update();
  m.imageCopy(img1.pixels);
  img1.updatePixels();
  image(img1,0,0);
  
  reg1.render();
  reg1.update();
  reg2.render();
  reg2.update();
 
}

boolean matchCol(color c1, color c2) {
  float d = dist(red(c1),green(c1),blue(c1),
    red(c2),green(c2),blue(c2));
  return (d<LIMIT);
}
 


void stop() {

  m.stop();

  super.stop();

}

class Region {
               
//  left, top, right, bottom define the region rectangle.
//  xOff, yOff are half the size of the boundary for the neighbours.
//  vx, vy define the x and y components of the velocity.
//  rW, rH are the width and height of the region.

  int left, top, right, bottom;
  int xOff, yOff;
  float vx, vy;
  int rW, rH;
  int centerX, centerY;
  int xdirection = 1;
  int ydirection = 1;

  Region(int _l, int _t, int _r, int _b) {
    left = _l;
    top = _t;
    right = _r;
    bottom = _b;
    xOff = w/2;
    yOff = h/2;
    vx = 0.0;
    vy = 0.0;
    rW = right - left;
    rH = bottom - top;
    centerX = (right/2) + (left/2);
    centerY = (bottom/2) + (top/2);
    xdirection = 1;
    ydirection = 1;
  }

  void update() { 

    centerX = (right/2) + (left/2);
    centerY = (bottom/2) + (top/2);
    
    //  Calculate the flow information within the region.
 
    if (left < 1){
      left += 1;
      right += 1;
    }
    if (top < 1) {
      bottom += 1;
      top += 1;
    }
    for (int r=top;r<bottom;r+=h) {
      for (int c=left;c<right;c+=w) {
        Point p1 = new Point(0,0);
        p1.x = constrain(c,0,img1.width-1);
        p1.y = constrain(r,0,img1.height-1);
        Point p2 = findPoint(p1);
   
// p1 is the pixel in img1 - current image
// p2 is the corresponding point in img2 - previous image
// vx, vy - velocity is computed by the sum of difference
// between the current position and previous position.

        vx += (p1.x-p2.x);
        vy += (p1.y-p2.y);
        drawFlow(p1,p2);
   
      }
    }
    moveRegion();
  }

  Point findPoint(Point _p) {
    Point p = new Point(_p.x,_p.y);
    color col1 = img1.pixels[_p.y*img1.width+_p.x];
    color col2 = img2.pixels[_p.y*img2.width+_p.x];
    if (!matchCol(col1,col2)) {
      float minDiff = 9999999999.0;
      int l = constrain(_p.x-xOff,0,img2.width-1);
      int t = constrain(_p.y-yOff,0,img2.height-1);
      int r = constrain(_p.x+xOff,0,img2.width-1);
      int b = constrain(_p.y+yOff,0,img2.height-1);
      for (int j=t;j<b;j++) {
        for (int i=l;i<r;i++) {
          col2 = img2.pixels[j*img2.width+i];
          float d = dist(red(col1),green(col1),blue(col1),
            red(col2),green(col2),blue(col2));
          if (d<minDiff) {
            minDiff = d;
            p.x = i;
            p.y = j;
          }
        }
      }
    }
    return p;
  }

  void drawFlow(Point p1, Point p2) {
    stroke(0);
    if (p1.x!=p2.x || p1.y!=p2.y) {
      line(p1.x,p1.y,p2.x,p2.y);
      float ang = atan2(p2.y-p1.y,p2.x-p1.x);
      float ln = w/3.0;
      float tx = p1.x + ln*cos(ang-PI/6);
      float ty = p1.y + ln*sin(ang-PI/6);
      line(p1.x,p1.y,tx,ty);
      tx = p1.x + ln*cos(ang+PI/6);
      ty = p1.y + ln*sin(ang+PI/6);
      line(p1.x,p1.y,tx,ty);
    } else {
      line(p1.x,p1.y,p2.x,p2.y);
    }
  }

  void moveRegion() {
    println(vx+ " " +vy);
    if (vx < 1 && vy < 1){
      top += 1;
      bottom += 1;
    }
    left += vx * xdirection;
    right += vx * xdirection;
    top += vy * ydirection;
    bottom += vy * ydirection;

    if (left<0) {
      xdirection *= -1;
      left = 0;
      right = rW;
    }
    if (right>width) {
      xdirection *= -1;
      right = width;
      left = right - rW;
    }
    if (top<0) {
      ydirection *= -1;
      top = 0;
      bottom = rH;
    }
    if (bottom>height) {
      ydirection *= -1;
      bottom = height;
      top = height - rH;
    }
   centerX = (right/2) + (left/2);
   centerY = (bottom/2) + (top/2); 
   
    vx = vx*0.482;
    vy = vy*0.482;
     
  }
  
  void render() {
    rectMode(CENTER);
    stroke(255,0,0);
    fill(255,255,0,100);
    pushMatrix();
    translate((left+right)/2,(top+bottom)/2);
    rect(0,0,rW,rH);
   
    popMatrix();
  }
}

class Point {
  int x, y;
  
  Point(int _x, int _y) {
    x = _x;
    y = _y;
  }
}