/* video tracking based on rob hodgin's code (http://www.proce55ing.net/software/tracking/)
   new features:
      - colour tracking instead of luminance only
      - rewritten as class, so multiple instances can be used to track more than one object/colour
      - optimized performance
      
   see comments and example below for intended usage
   comments & improvements please send to: info@toxi.co.uk, robert@flight404.com
*/

VTracker[] vTrackers;
TrackCursor[] cursors;

boolean colour_pick=false;
int     currTracker=0;

void setup() {
  size(320, 240);
  noBackground();
  ellipseMode(CENTER_DIAMETER);
  noStroke();

  beginVideo(width,height,15);
  
  // new vTracker objects to track several instances
  vTrackers = new VTracker[2];
  // first tracker to track white areas with a tolerance of 15
  vTrackers[0] = new VTracker(2,0xffffff,15,0.75);
  // another tracker to track bright red areas with a tolerance of 10
  vTrackers[1] = new VTracker(2,0xff0000,10,0.3);
  // attach independent cursors to the trackers
  cursors = new TrackCursor[2];
  cursors[0] = new TrackCursor(vTrackers[0],10);
  cursors[1] = new TrackCursor(vTrackers[1],5);
}

void loop() {
  vTrackers[0].analyse(true); // only display one copy of the video
  vTrackers[1].analyse(false);
  cursors[0].update();
  cursors[1].update();
  // if mouse has been pressed, use new colour under mouse position for the next tracker
  if (colour_pick) {
    vTrackers[currTracker++].trackingNewColour(pixels[mouseY*width+mouseX]);
    currTracker%=vTrackers.length;
    colour_pick=false;
  }
}

void mousePressed() {
  colour_pick=true;
}

void videoEvent(){
  // send new frame event to all trackers
  for(int i=0; i<vTrackers.length; i++) vTrackers[i].newFrame();
}

// a track cursor can be assigned to a particular vTracker instance
// to show the position of the tracked object/colour area
// param s = size of the crosshair used
class TrackCursor {
  int sz;
  VTracker parent;
  
  TrackCursor (VTracker p, int s) {
    parent=p;
    sz=s;
  }
  
  void update() {
    // use tracker's complimentary colour for cursor
    stroke(255-parent.trackR,255-parent.trackG,255-parent.trackB);
    int xx = parent.trackingX;
    int yy = parent.trackingY;
    line(xx - sz, yy, xx + sz, yy);
    line(xx, yy - sz, xx, yy + sz);
    noStroke();
  }
}


// VTracker class v1.04
// author: info@toxi.co.uk (based on code by robert@flight404.com)
// date:   08/09/03

class VTracker {

  // res = number of pixels skipped in x/y direction when analysing image
  // trackCol = packed colour integer, e.g. 0xffcc33 for some bright yellow
  // tolerance = value between [0..255] to control the tracking accuracy (values between 5..20 work best)
  // trk_follow = value between 0.0...1.0 used for interpolated movement of the tracking coordinates (values closer to 1.0 = more snappy)

    VTracker(int rez, int trackCol, int tol, float trk_follow) {
    vWidth=video.width/rez;
    vHeight=video.height/rez;
    vW2=vWidth/2;
    vH2=vHeight/2;
    rowHeight=video.width*rez;
    similaritiesX=new int[vWidth];
    similaritiesY=new int[vHeight];
    res=rez;
    this.trackingNewColour(trackCol);
    tolerance=tol;
    trackFactor=trk_follow;
    this.reset();
  }
  
  // notify tracker about new video frame/data
  void newFrame() {
    if (!newFrame) newFrame=true;
  }
  
  // set a new tracking colour for this instance
  void trackingNewColour(int c) {
    trackR=c>>16&0xff;
    trackG=c>>8&0xff;
    trackB=c&0xff;
    println("tracking new colour: "+trackR+","+trackG+","+trackB);
  }
  
  // parse the current video image and try to find hotspot
  // needs to be called every frame, but will only fully execute if there's a new video frame present
  // boolean display is used to disable drawing the video image (important when using several instances)
  // see above example
  
  // note: quality of tracking will improve once the new imaging functions are implemented in P5
  // at the moment only single pixels instead of averaged pixelblocks are used for the analysis

  void analyse(boolean display) {
    if (newFrame) {
      int yoffset=0;
      int col,lum;
      int xx=0;
      int yy=0;
      int r,g,b;
      int deltaCol;
      
      // parse the video pixel buffer in the set (scaled down) resolution
      for(int y=0; y<vHeight; y++){
        xx = 0;
        yy = y*res;
        for(int x=0; x<vWidth; x++) {
          col = video.pixels[yoffset+xx];
          r = col>>16&0xff;
          g = col>>8&0xff;;
          b = col&0xff;

          // compute difference to tracked colour
          deltaCol=(abs(trackR-r)+abs(trackG-g)+abs(trackB-b))/3 - tolerance;
          
          // if within tolerance, add value to find X and Y based sums
          if (deltaCol<=0){
            similaritiesX[x] -= deltaCol;
            similaritiesY[y] -= deltaCol;
          }
          
          // only display if we were told to...
          if (display) {
            fill(r,g,b);
            rect(xx, yy, res, res);
          }
          xx+=res;
        }
        yoffset+=rowHeight;
      }
      
      // now on to finding the peaks in horizontal and vertical direction
      // if no peak is found (or in other words, the tracked colour is not
      // present in the video image) the peak coodinates will be negative
      // (your code could check for that, for instance...)
      int peakXPos=-1,peakYPos=-1;
      
      for(int x=0; x<vWidth; x++){
        if (similar_xMax < similaritiesX[x]){
          similar_xMax = similaritiesX[x];
          peakXPos = x;
        }
      }

      for(int y=0; y<vHeight; y++){
        if (similar_yMax < similaritiesY[y]){
          similar_yMax = similaritiesY[y];
          peakYPos = y;
        }
      }

      // update the peak coordinates with the set interpolation speed
      trackingX -= (trackingX - (int)((peakXPos+0.5) * res)) * trackFactor;
      trackingY -= (trackingY - (int)((peakYPos+0.5) * res)) * trackFactor;

      this.reset();
    }
  }

  // clear all values before processing a new frame
  void reset(){
    similar_xMax = 0;
    similar_yMax = 0;
    for(int x=0; x<vWidth; x++)  similaritiesX[x] = 0;
    for(int y=0; y<vHeight; y++) similaritiesY[y] = 0;
    newFrame=false;
  }
  
  // class fields...
  
  int trackingX,trackingY;
  int vWidth,vW2;
  int vHeight,vH2;
  int rowHeight;
  int res;
  boolean newFrame;
  float trackFactor;
  int trackR,trackG,trackB;
  int tolerance;
  int similar_xMax,similar_yMax;
  int[] similaritiesX,similaritiesY;
}