//=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-
//BlobDetection by v3ga <http://processing.v3ga.net>
//May 2005
//
//
// Adopted by Thomas M. Brand, March-April 2007, http://www.lowres.ch
// - .mov or capture processing
// - displaying center position of blob and channel texutally
// - render result window to new movie (mjpeg)
// - advanced sending osc: gate on/off based on previous 
//   blob positions -> blobs must be 'tracked' in order 
//   to assign the same channel to the same finger in the 
//   next frame. if the deviation of the previous position
//   is too high, a gate off is sent (finger up/scratch finished).
// - handling of assignment of available voices (poliphony) to blobs
// - adjusting parameters relevant for recognition while processing (with gui)
// - show/hide controls (standard feature of controlP5!!)
// - show calibration help for beamer and camera adjustment
//
// oscP5 and controlP5 by Andreas Schlegel, http://www.sojamo.de
// these libs are really GREAT!! 

// still needs testing and refactoring..

//=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-


import processing.video.*;
import blobDetection.*;
import oscP5.*;
import pressure.*;
import controlP5.*;

// some configuration

int iSW=640; //sreen
int iSH=480;
int iCW=320; //capture
int iCH=240;
int iMW=320; //movie
int iMH=240;

boolean Rotate180=false;
boolean ShowDebug=false;
boolean FromWebcam=false;
boolean OscEnabled=false;
boolean CreateMovie=false;
boolean BinCut=true;
boolean ShowCalibrationHelp=false;

Capture cam;
int iFramerate=5; //for capture cam and moviemaker

Movie mov;
String sMov="tangible-test.mov";

MovieMaker movmaker;
String sNewMov="tangible-test-created.jpeg";

//set this to a high value (20 = 2 * 10 fingers)
//if using osc, synth must provide 20 independant tone generators/ oscilators
int iPolyphony=20;
int[] iFreeChannels;

OscP5 oscP5;
int receiveAtPort=3333;
int sendToPort=10000;
String host="127.0.0.1";
String oscP5event="oscEvent";

ControlP5 controlP5;

int sliderBinCut=160;
float sliderBlobLuminosity=0.85;
int sliderMinBlobSize=0;
int sliderMaxBlobSize=150;
int sliderBlurRadius=3;

BlobDetection theBlobDetection;
boolean newFrame=false;
PImage img;
PFont font;
String sFont="BitstreamVeraSansMono-Roman-20.vlw";
int iFontSize=10;

//how many pixels a pressure point can deviate from previous one to be
//recognized as pressed from the same finger
int iDeviation=3;
Pressure p;
Vector vPressure,vPressurePrevious;

// ==================================================
// setup()
// ==================================================
void setup()
{
  // Size of applet
  size(iSW,iSH);

  System.out.println("key combinations:");
  System.out.println("ALT+h hide and show controllers");
  System.out.println("ALT+s save controllers to an xml file");
  System.out.println("ALT+l load controllers from an xml file");
  System.out.println("ALT+k bring the key menu to the front. use the arrow keys ..");
  System.out.println("hold the ALT key and move controllers by dragging them.");

  if(OscEnabled)
  {
    initOsc();
  }
  if(CreateMovie)
  {
    initMovieMaker();  
  }
  
  //build control and calibration help gui
  initGui();

  //used for channel assignement / tone on/off handling
  vPressure=new Vector();
  vPressurePrevious=new Vector();

  iFreeChannels = new int[iPolyphony];
  initChannels();

  font = loadFont(sFont); 
  textFont(font, iFontSize); 

  if(FromWebcam)
  {
    img = new PImage(iCW,iCH);
    theBlobDetection = new BlobDetection(iCW, iCH);
    cam = new Capture(this, iCW, iCH, iFramerate);
  }
  else
  {  
    img = new PImage(iMW,iMH);
    mov = new Movie(this,sMov);
    theBlobDetection = new BlobDetection(iMW,iMH);
    mov.loop();
  }

  theBlobDetection.setPosDiscrimination(true);
  theBlobDetection.setBlobDimensionMin(sliderMinBlobSize,sliderMinBlobSize);
  theBlobDetection.setBlobDimensionMax(sliderMaxBlobSize,sliderMaxBlobSize);
  theBlobDetection.setBlobMaxNumber(200);

  theBlobDetection.setThreshold(sliderBlobLuminosity); // will detect bright areas whose luminosity > x.xf;

}


// ==================================================
// draw()
// ==================================================
void draw()
{
  if (newFrame)
  {
    newFrame=false; 
    //img = new PImage(iCW, iCH);
    //img = new PImage(iSW, iSH);        

    if(FromWebcam)
    {
      img = new PImage(iCW, iCH);
    }
    else
    {
      img = new PImage(iMW,iMH);
    }

    if(Rotate180) //copy each pixel to its 180 degrees rotated place in the pixel array   
    {
      int h2=0;
      int hh=0;
      int ww=0;

      if(FromWebcam)
      {
        hh=iCH;
        ww=iCW;
      }
      else
      {
        hh=iMH;
        ww=iMW;
      }

      for (int h=hh; h>0;h--)
      {
        int w2=0;
        for (int w=ww; w>0;w--)                  
        {                    
          if(FromWebcam)
          {
            img.copy(cam,w,h,1,1,
            w2,h2,1,1);
          }
          else
          {
            img.copy(mov,w,h,1,1,
            w2,h2,1,1);
          }
          w2++;                    
        }
        h2++;                   
      }      
    }
    else //copy as a whole withouth rotation
    {
      if(FromWebcam)
      {
        img.copy(cam, 0, 0, iCW, iCH, 
        0, 0, iCW, iCH);
      }
      else
      {
        img.copy(mov, 0, 0, iMW, iMH, 
        0, 0, iMW, iMH);
      }
    }

    img.updatePixels();

    //prepare image for better blobdetection results
    //using radius from slider
    fastblur(img, sliderBlurRadius);

    if(BinCut)
    {
      binaryCut();
    }

    //show image (scale to applet size)
    image(img,0,0,iSW,iSH);

    //set params from slider    
    theBlobDetection.setThreshold(sliderBlobLuminosity);
    theBlobDetection.setBlobDimensionMin(sliderMinBlobSize,sliderMinBlobSize);
    theBlobDetection.setBlobDimensionMax(sliderMaxBlobSize,sliderMaxBlobSize);

    theBlobDetection.computeBlobs(img.pixels);
    drawBlobsAndEdges(false,true);
    handlePressureMatrix();

    //check if calibration should be displayed
    if(ShowCalibrationHelp)
    {
      strokeWeight(30);
      stroke(255,50,50);
      noFill();
      rect( 0, 0, width, height );

      strokeWeight(5);
      line( 0, 0, width, height );
      line( 0, height, width, 0 );
      line( width/4, 0, width/4, height );
      line( width/2, 0, width/2, height );
      line( 3*width/4, 0, 3*width/4, height );
      line( 0, height/2, width, height/2 );
      
    }

    //check if we have to write a movie
    if(CreateMovie)
    {
      if(movmaker==null)
      {
        //not jet initialized
        initMovieMaker();  
      }
      //add window's pixels to movie cool!
      movmaker.add();
    }
    else
    {
      //check if was started and we have to stop
      if(movmaker!=null)
      {
        movmaker.stop();
        System.out.println("movie recording finished!");
        movmaker=null; 
      }
    }
  }

}

void handlePressureMatrix()
{
  vPressurePrevious = vPressure;
  vPressure = new Vector();

  noFill();
  Blob b;
  EdgeVertex eA,eB;

  int iPressurePreviousSize = vPressurePrevious.size();

  for (int n=0 ; n<theBlobDetection.getBlobNb() ; n++)
  {

    b=theBlobDetection.getBlob(n);

    int iXMinAbsolute = (int)(b.xMin*width);
    int iYMinAbsolute = (int)(b.yMin*height);
    int iXCenterAbsolute = (int)(b.x*width);
    int iYCenterAbsolute = (int)(b.y*height);
    int iWAbsolute = (int)(b.w*width);
    int iHAbsolute = (int)(b.h*height);

    strokeWeight(0.2);
    stroke(255,255,80);
    noFill();
    rect( iXMinAbsolute, iYMinAbsolute, iWAbsolute, iHAbsolute );

    //check if previous pressure was at same position (ignoring previously blocked)
    Pressure pTest = getMatchingPressurePreviousXYDeviation(iXCenterAbsolute,iYCenterAbsolute,iDeviation); 
    //if found
    if (pTest != null)
    {
      int iChannel = pTest.iChannel;
      //create new pressure, assing previous channel, no ungate
      Pressure p = new Pressure(iXCenterAbsolute,iYCenterAbsolute,iWAbsolute*iHAbsolute,iChannel,true);
      //update freechannel
      setChannel(iChannel,0);
      vPressure.add(p);
      debug("Found previous Pressure for current Point:"+iXCenterAbsolute+"/"+iYCenterAbsolute+" Channel was: "+iChannel);
    }
    else
    {
      //try to get free channel and create new pressure
      int iFree = getFreeChannel();
      if (iFree > 0)
      {
        Pressure p = new Pressure(iXCenterAbsolute,iYCenterAbsolute,iWAbsolute*iHAbsolute,iFree,false);
        //update freechannel
        setChannel(iFree,0);
        vPressure.add(p);
        debug("New Pressure Point:"+iXCenterAbsolute+"/"+iYCenterAbsolute+" Channel assigned: "+iFree);
      }
      else
      {
        //blocked, polyphony reached, set channel to 0
        //Pressure p = new Pressure(iXCenterAbsolute,iYCenterAbsolute,iWAbsolute*iHAbsolute,0,true);
        //vPressure.add(p);
        debug("Pressure Point:"+iXCenterAbsolute+"/"+iYCenterAbsolute+" accepted but polyphony reached! not added to vector.");
      }
    }
  }

  debug("Pressure Points in this frame: "+vPressure.size()+" diff to previous: "+(vPressure.size()-iPressurePreviousSize));

  //now handle current pressure vector
  handlePressureVectorChannels();
  debug("updated channels.");
  if (OscEnabled)
  {
    if(oscP5 == null)
    {
      //not yet initialized
      initOsc();
    }
    handlePressureVectorOsc();
    debug("osc done..");
  }

  debug("frame done.");
}

void handlePressureVectorOsc()
{
  //handle pitch and cutoff
  int i=0;  
  for (Enumeration e = vPressure.elements(); e.hasMoreElements(); i++)
  {
    Pressure p = (Pressure)e.nextElement();
    int iChannel=p.iChannel;
    float fPitch = (float)p.x/iSW;    
    sendOsc("/Pitch"+iChannel,fPitch);
    debug("sent osc pitch"+iChannel+" "+fPitch);
    float fCutoff = (float)p.y/iSH;    
    sendOsc("/P Cutoff"+iChannel,fCutoff);
    debug("sent osc cutoff"+iChannel+" "+fCutoff);
  }

  //handle gate on/off
  for (i=0;i<iPolyphony;i++)
  { 
    int iFree=iFreeChannels[i];

    //debug("channel "+(i+1)+": "+iFree+" - ");

    //if free send gate off anytime
    if(iFree==1)
    {       
      sendOsc("/Gate"+(i+1),0);
      debug("sent osc gate"+(i+1)+":"+0+" - ");
    }
    //if used, gate or leave as is based on previous blob-binding
    else
    {    
      Pressure p = getMatchingPressureByChannel(i+1);
      //if was not previously gated
      if(!p.bPrev)
      {
        sendOsc("/Gate"+(i+1),1);
        debug("sent osc gate"+(i+1)+":"+1);    
      }
      //otherwise, let adsr do its thing (don't gate)    
      else
      {
        debug("no need to gate channel "+(i+1));
      } 
    }
  }

}


void handlePressureVectorChannels()
{
  initChannels();

  int i=0;  
  for (Enumeration e = vPressure.elements(); e.hasMoreElements(); i++)
  {
    Pressure p = (Pressure)e.nextElement();
    int iChannel=p.iChannel;
    iFreeChannels[iChannel-1]=0;

    //display at bottom center of rect
    String sText=""+p.x+"/"+p.y+"/"+iChannel;
    fill(255, 255, 255);
    text(sText, p.x, p.y);
  }

  debugnonl("freechannels is: ");
  for(i=0;i<iPolyphony;i++)
  {
    debugnonl(""+iFreeChannels[i]+"/");
  }
  debug("");

}

Pressure getMatchingPressureByChannel(int iChannel)
{
  int i=0;

  for (Enumeration e = vPressure.elements(); e.hasMoreElements(); i++)
  {
    Pressure p = (Pressure)e.nextElement();

    if ( p.iChannel == iChannel)
    {
      return p;
    }       
  }
  return null;

}

Pressure getMatchingPressurePreviousXYDeviation(int iX, int iY, int iMaxDeviation)
{
  int i=0;

  for (Enumeration e = vPressurePrevious.elements(); e.hasMoreElements(); i++)
  {
    Pressure p = (Pressure)e.nextElement();

    if ( Math.abs(iX - p.x)<=iMaxDeviation ||  Math.abs(iY - p.y)<=iMaxDeviation && !p.bProcessed)
    {
      //flag processed true in vector so not used twice (larger blobs containing smaller ones)
      //vPressurePrevious.remove(i);
      p.setProcessed(true);
      //put back to vector
      vPressurePrevious.set(i,p);
      return p;
    }    
  }
  return null;

}

//---------------------GUI
void initGui()
{
  int iSliderCount=5;
  int iSliderWidth=(int)((iSW/iSliderCount)-5);
  int iGap=5; //horizontal
  int iSliderHeight=(int)iSH/25;
  int iYPos=iSH-iSliderHeight-12;

  controlP5 = new ControlP5(this);
  //addSlider(theName, theMin, theMax, theDefaultValue, theX, theY, theWidth, theHeight);
  controlP5.addSlider("sliderBinCut",0,255,sliderBinCut,iGap,iYPos,iSliderWidth,iSliderHeight);
  controlP5.addSlider("sliderBlobLuminosity",0,1,sliderBlobLuminosity,iSliderWidth+2*iGap,iYPos,iSliderWidth,iSliderHeight);
  controlP5.addSlider("sliderMinBlobSize",0,50,sliderMinBlobSize,2*iSliderWidth+3*iGap,iYPos,iSliderWidth,iSliderHeight);
  controlP5.addSlider("sliderMaxBlobSize",0,iSW,sliderMaxBlobSize,3*iSliderWidth+4*iGap,iYPos,iSliderWidth,iSliderHeight);
  controlP5.addSlider("sliderBlurRadius",0,20,sliderBlurRadius,4*iSliderWidth+3*iGap,iYPos,iSliderWidth,iSliderHeight);

  int iToggleSize=iSliderHeight;
  int iToggleGap=15*iGap;
  controlP5.addToggle("BinCut",BinCut,iGap,iGap,iToggleSize,iToggleSize);
  controlP5.addToggle("OscEnabled",OscEnabled,iToggleSize + iGap+iToggleGap,iGap,iToggleSize,iToggleSize);
  controlP5.addToggle("CreateMovie",CreateMovie,2*iToggleSize + iGap+2*iToggleGap,iGap,iToggleSize,iToggleSize);
  controlP5.addToggle("Rotate180",Rotate180,3*iToggleSize + iGap+3*iToggleGap,iGap,iToggleSize,iToggleSize);
  controlP5.addToggle("ShowCalibrationHelp",ShowCalibrationHelp,4*iToggleSize + iGap+4*iToggleGap,iGap,iToggleSize,iToggleSize);
  controlP5.addToggle("ShowDebug",ShowDebug,5*iToggleSize + iGap+5*iToggleGap,iGap,iToggleSize,iToggleSize);
} 

void setChannel(int iChannelNumber, int iStatus)
{
  iFreeChannels[iChannelNumber-1] = iStatus;
}

int getFreeChannel()
{
  for (int i=0;i<iPolyphony;i++)
  { 
    //check if free
    if(iFreeChannels[i]==1)
    {
      return i+1;
    }
  }
  return 0;
}


void initChannels()
{
  for (int i=0;i<iPolyphony;i++)
  { 
    //set free true
    iFreeChannels[i]=1;
  }  
}

void blockChannels()
{
  for (int i=0;i<iPolyphony;i++)
  { 
    //set free false
    iFreeChannels[i]=0;
  }  
}


void initMovieMaker() {
  movmaker = new MovieMaker(this, iSW, iSH, sNewMov,MovieMaker.JPEG, MovieMaker.HIGH,iFramerate);
  System.out.println("start recording movie! toggle to stop."); 
}

void initOsc() {
  oscP5 = new OscP5(this, host, sendToPort, receiveAtPort, oscP5event );
}

void sendOsc(String sPattern,float fValue)
{
  OscMessage oscMsg = oscP5.newMsg(sPattern);
  oscMsg.add(fValue);
  oscP5.send(oscMsg);
}

void debug(String sMessage)
{
  if(ShowDebug)
  {
    System.out.println(sMessage);
  }  
}

void debugnonl(String sMessage)
{
  if(ShowDebug)
  {
    System.out.print(sMessage);
  }  
}

void movieEvent(Movie m)
{
  m.read();
  newFrame = true;
}

// ==================================================
// captureEvent()
// ==================================================
void captureEvent(Capture cam)
{
  cam.read();
  newFrame = true;
}


void binaryCut()
{
  //calculates grey value of img and separates pixels to black or white at a certain level (given from gui slider)

  //modifiy image: greyvalues, binary cut
  /*
Well, as you only have 256 levels of red, green, and blue, an exact
   conversion is not possible. However, you can consult the CCIR
   Recommendations that deal with color weightings when converting RGB to
   greyscale. These are:
   
   CCIR 601-1: red = 0.299; green = 0.587; blue = 0.114;
   CCIR 709: red = 0.2125; green = 0.7154; blue = 0.721;
   
   You will get this correspondence (example for CCIR 601-1):
   
   red     green   blue    resulting grey level
   i       i       i       i + 0.000
   i       i      i+1      i + 0.114
   i+1      i       i       i + 0.299
   i+1      i      i+1      i + 0.413
   i      i+1      i       i + 0.587
   i      i+1     i+1      i + 0.701
   i+1     i+1      i       i + 0.886
   i+1     i+1     i+1      i + 1.000
   
   This gives you approx. 1750 possible pseudo grey values.
   
   */

  int iDim=img.width*img.height;
  int[] aPixels;
  aPixels = new int[iDim];
  for(int i=0; i<iDim; i++) 
  {
    int iPixel = img.pixels[i];
    float r=red(iPixel);
    float g=green(iPixel);
    float b=blue(iPixel);

    //red = 0.299; green = 0.587; blue = 0.114;
    float fGrey=r*0.299+g*0.587+b*0.114;

    //if(fGrey > 140)
    //get value from slider
    if(fGrey > sliderBinCut)
    {
      img.pixels[i] = color(255,255,255);
    }
    else
    {
      //leave as is for the moment
      img.pixels[i] = iPixel;  
    }   
  }
  img.updatePixels();
}


// ==================================================
// drawBlobsAndEdges()
// ==================================================
void drawBlobsAndEdges(boolean drawBlobs, boolean drawEdges)
{
  noFill();
  Blob b;
  EdgeVertex eA,eB;

  int iPolyCount=0;

  for (int n=0 ; n<theBlobDetection.getBlobNb() ; n++)
  {
    //choose voice
    iPolyCount++;
    b=theBlobDetection.getBlob(n);
    if (b!=null)
    {
      // Edges
      if (drawEdges)
      {
        strokeWeight(3);
        stroke(0,255,0);
        for (int m=0;m<b.getEdgeNb();m++)
        {
          eA = b.getEdgeVertexA(m);
          eB = b.getEdgeVertexB(m);
          if (eA !=null && eB !=null)
            line(
            eA.x*width, eA.y*height, 
            eB.x*width, eB.y*height
              );                                          
        }
      }

      // Blobs
      if (drawBlobs)
      {
        strokeWeight(1);
        stroke(255,0,0);
        rect(
        b.xMin*width,b.yMin*height,
        b.w*width,b.h*height
          );

      }

    }

  }
}

// ==================================================
// Super Fast Blur v1.1
// by Mario Klingemann 
// <http://incubator.quasimondo.com>
// ==================================================
void fastblur(PImage img,int radius)
{

  if (radius<1){
    return;
  }
  int w=img.width;
  int h=img.height;
  int wm=w-1;
  int hm=h-1;
  int wh=w*h;
  int div=radius+radius+1;
  int r[]=new int[wh];
  int g[]=new int[wh];
  int b[]=new int[wh];
  int rsum,gsum,bsum,x,y,i,p,p1,p2,yp,yi,yw;
  int vmin[] = new int[max(w,h)];
  int vmax[] = new int[max(w,h)];
  int[] pix=img.pixels;
  int dv[]=new int[256*div];
  for (i=0;i<256*div;i++){
    dv[i]=(i/div);
  }

  yw=yi=0;

  for (y=0;y<h;y++){
    rsum=gsum=bsum=0;
    for(i=-radius;i<=radius;i++){
      p=pix[yi+min(wm,max(i,0))];
      rsum+=(p & 0xff0000)>>16;
      gsum+=(p & 0x00ff00)>>8;
      bsum+= p & 0x0000ff;
    }
    for (x=0;x<w;x++){

      r[yi]=dv[rsum];
      g[yi]=dv[gsum];
      b[yi]=dv[bsum];

      if(y==0){
        vmin[x]=min(x+radius+1,wm);
        vmax[x]=max(x-radius,0);
      }
      p1=pix[yw+vmin[x]];
      p2=pix[yw+vmax[x]];

      rsum+=((p1 & 0xff0000)-(p2 & 0xff0000))>>16;
      gsum+=((p1 & 0x00ff00)-(p2 & 0x00ff00))>>8;
      bsum+= (p1 & 0x0000ff)-(p2 & 0x0000ff);
      yi++;
    }
    yw+=w;
  }

  for (x=0;x<w;x++){
    rsum=gsum=bsum=0;
    yp=-radius*w;
    for(i=-radius;i<=radius;i++){
      yi=max(0,yp)+x;
      rsum+=r[yi];
      gsum+=g[yi];
      bsum+=b[yi];
      yp+=w;
    }
    yi=x;
    for (y=0;y<h;y++){
      pix[yi]=0xff000000 | (dv[rsum]<<16) | (dv[gsum]<<8) | dv[bsum];
      if(x==0){
        vmin[y]=min(y+radius+1,hm)*w;
        vmax[y]=max(y-radius,0)*w;
      }
      p1=x+vmin[y];
      p2=x+vmax[y];

      rsum+=r[p1]-r[p2];
      gsum+=g[p1]-g[p2];
      bsum+=b[p1]-b[p2];

      yi+=w;
    }
  }

  /*
 void oscEvent(OscIn oscIn) {
   println("received a message ... weeeee !!");
   }
   */

}