import processing.video.*;
//on linux: sudo apt-get install libgstreamer-plugins-base0.10-dev libgstreamer0.10-dev libglib2.0-dev
//          sudo apt-get install gstreamer0.10-plugins-good

import processing.serial.*;
Serial sPort;

Capture cam;

PImage cam_reference;
PImage cam_currentbox;
PGraphics canvas;


int currentID=0;
//int MAXID = 50; //inclusive
int MAXID = 9;
int searchstate=0; //0=get reference image and ping box, 1=wait, 2=get image to compare and turn box off, 3=wait
long starttime_wait=0;

boolean running=false;

color testcolor=color(255,255,255);
color offcolor=color(0,0,0);
color successcolor=color(0,255,0);
color failcolor=color(255,0,0);

static int boxnum=19; //how many boxes are visible?
ArrayList<Box> boxes=new ArrayList<Box>();
//result will be written to the following arrays
//int boxid[] = new int[boxnum]; //for found boxes
//PVector boxpos[] = new PVector[boxnum]; //center position of box
//PVector boxsize[] = new PVector[boxnum]; //size of box (width,height)

//Settings
//int FINDBOXES_MINIMUMBRIGHTNESS; //brightness difference required
int FINDBOXES_MAXIMUMPIXELDISTANCE; //distance to add pixel to existing box
PVector MINBOXSIZE; //minimum box size to accept box
int TIME_FIND = 500; //time in ms to find box in image
int TIME_RESULT = 250; //time in ms to show success/fail
int TIME_TURNOFF = 250; //time in ms to wait to turn off
float MINBRIGHTNESS_PERCENTILE=0.98; //percentile for minbrightness calculation (how many pixels will be considered). between 0(all) and 1(none). 0=all pixels. ->1=only bright pixels 
int MINIMUMALLOWEDBRIGHTNESS=50; //minimum brightness. will be limited to this lower bound if no box in image (percentile calculation yields low value)

void setup() {
  size(640, 480);
  frameRate(30);

  String portName = Serial.list()[0];
  sPort = new Serial(this, portName, 115200);


  canvas = createGraphics(width, height);
  
  //FINDBOXES_MINIMUMBRIGHTNESS=60; //brightness difference required
  FINDBOXES_MAXIMUMPIXELDISTANCE=width/200; //distance to add pixel to existing box
  MINBOXSIZE = new PVector(width/300,width/300); //minimum box size to accept box

  String[] cameras = Capture.list();

  if (cameras.length == 0) {
    println("There are no cameras available for capture.");
    exit();
  } else {
    println("Available cameras:");
    for (int i = 0; i < cameras.length; i++) {
      println(i+"->"+cameras[i]);
    }

    // The camera can be initialized directly using an
    // element from the array returned by list():
    //cam = new Capture(this, cameras[1]); //windows
    cam = new Capture(this, cameras[1]); //linux
    cam.start();
  }
  cam_reference=cam.copy();
}

void draw() {

  if (cam.available() == true) {
    cam.read();
  }
  //image(cam, 0, 0);
  // The following does the same, and is faster when just drawing the image
  // without any additional resizing, transformations, or tint.
  //set(0, 0, cam);

  //Subtract images
  canvas.beginDraw();
  canvas.background(0);
  canvas.blendMode(NORMAL);
  canvas.image(cam, 0, 0);
  canvas.blendMode(SUBTRACT);
  canvas.image(cam_reference, 0, 0);
  canvas.endDraw();

  image(canvas,0,0); //show camera image

  if (running) {
    checkState();
  }

  textSize(12);
  fill(0,255,150); //textcolor
  text("id: " + currentID, 10, 20);
  text("state: " + searchstate, 10, 20*2);
  
  //draw boxes
  for (Box box : boxes) {
    PVector boxcenter = box.getBoxCenter();
    PVector boxsize = box.getBoxSize();
    
    rectMode(CENTER);
    noFill();
    fill(255,255,0,100);
    stroke(255,0,0);
    rect(boxcenter.x,boxcenter.y, boxsize.x,boxsize.y);
    textSize(12);
    fill(0,255,150); //textcolor
    text(box.getBoxID(), boxcenter.x,boxcenter.y);
  }
}

void checkState() {
  switch(searchstate) {
    case 0: //0=get reference image and ping box
      cam_reference=cam.copy();
      println("Searching BoxID "+str(currentID));

      
      colorBox(currentID,testcolor);

      starttime_wait=millis();
      searchstate++;
      break;

    case 1: //1=wait
      if (millis()>starttime_wait + TIME_FIND) { //wait some time
          searchstate++;
      }
      break;

    case 2: //2=get image to compare and turn box off
      cam_currentbox=canvas.copy();

      Box box = findSingleBox(cam_currentbox); //find single box in this image
      if (box!=null && (box.getBoxSize().x<MINBOXSIZE.x || box.getBoxSize().y<MINBOXSIZE.y)) { //box too small
        box=null;
      }
      if (box!=null) { //if box found
        PVector boxsize = box.getBoxSize();
        float boxarea = boxsize.x * boxsize.y;
        if (boxarea >= pow(cam_currentbox.width/40,2)) { //if box is big enough
          box.setBoxID(currentID);
          boxes.add(box);
        }
      }
      
      if (box!=null) {
        println("Found box");
        println("Pos: "+box.getBoxCenter().x+", "+box.getBoxCenter().y+" | size: "+box.getBoxSize().x+", "+box.getBoxSize().y);
        colorBox(currentID, successcolor);
      }else {
        println("No Box Found!");
        colorBox(currentID, failcolor);
      } 
      
      starttime_wait=millis();
      searchstate++;
      break;

    case 3: //3=wait, color box fail or success
      if (millis()>starttime_wait + TIME_RESULT) { //wait some time
          colorBox(currentID,offcolor); //turn box off
          starttime_wait=millis();
          searchstate++;
      }
      break;
      
    case 4: //wait for box to turn off completely
      if (millis()>starttime_wait + TIME_TURNOFF) { //wait some time
          starttime_wait=millis();
          searchstate++;
      }
      break;

    case 5: //next box
      searchstate=0;
      currentID++;
      if (currentID>MAXID) {
        running=false;
      }
      break;
  }
}

void keyPressed() {
  if (key == 'n' ){
    cam_reference=cam.copy();
    println("New Reference Set");

  } else if (key == 's' ){
    running=true;
    colorAllOff(); //turn box off
    currentID=0; //reset
    searchstate=0;
    boxes.clear(); //reset found boxes
    println("Search started");
  }

}

int getPercentileBrightness(PImage img, float percentile) { //percentile between 0 and 1
  println("percentile="+str(percentile));
  percentile=min(max(percentile,0.0),1.0); //limit to 0 to 1
  int[] brightness=new int[img.pixels.length];
  for (int i=0;i<brightness.length;i++) {
    brightness[i] = int(brightness(img.pixels[i]));
  }
  int[] sortedbrightness=sort(brightness);
  return sortedbrightness[int(percentile*sortedbrightness.length)];
}

ArrayList<Box> findBoxes(PImage img) {
  ArrayList<Box> foundboxes=new ArrayList<Box>();
  int minimumbrightness=getPercentileBrightness(img, MINBRIGHTNESS_PERCENTILE); //calculate brightness by percentile based on minimum box size
  
  minimumbrightness=max(minimumbrightness, MINIMUMALLOWEDBRIGHTNESS); //limit lower value 
  
  
  println(" minimumbrightness="+minimumbrightness);
  for (int y=0;y<img.height;y++) {
      for (int x=0;x<img.width;x++) {
        color pcolor = img.get(x,y);
        
        if (brightness(pcolor)>minimumbrightness) { //pixel has changed
            Box nearestBox=null;
            float nearestBoxDistance=MAX_FLOAT;
            for (Box cb : foundboxes) { //check all known boxes
                float cdist = cb.getDistanceTo(new PVector(x,y));
                if (cdist<=FINDBOXES_MAXIMUMPIXELDISTANCE && cdist<nearestBoxDistance) { //is near this box and closer than to another box before
                  nearestBox=cb; //remeber box
                  nearestBoxDistance=cdist; //remember distance
                }
            }
            if (nearestBox==null) { //no box near this pixel
              Box newbox=new Box(); //create new box
              newbox.addPixel(new PVector(x,y));
              foundboxes.add(newbox); //add
            } else {
              nearestBox.addPixel(new PVector(x,y)); //add this pixel to near box
            }
        }
      }
    }
    
    println(" findBoxes(): foundboxes.size="+foundboxes.size());
    return foundboxes;
}

Box findSingleBox(PImage img) {
  ArrayList<Box> foundboxes = findBoxes(img);
  Box mostPixelBox=null;
  for (Box box : foundboxes) {
    if (mostPixelBox == null || mostPixelBox.getPixelNumber()<box.getPixelNumber()) { //found box with more pixels
      mostPixelBox=box;
    }
  }
  return mostPixelBox;
}

public class Box {
   ArrayList<PVector> boxpixels;
   int boxid;
   
   public Box() {
     boxpixels = new ArrayList<PVector>();
   }
   
   public void addPixel(PVector newpix) {
     boxpixels.add(newpix); 
   }
   
   public ArrayList<PVector> getPixels() {
     return boxpixels;
   }
   
   public void setBoxID(int pid) {
     this.boxid=pid;
   }
   public int getBoxID() {
     return this.boxid;
   }
   
   public float getDistanceTo(PVector pix) { //get distance from pix to nearest point of this box
     float mindist=MAX_FLOAT;
     for (PVector pbox : boxpixels) {
       float currentdist = sqrt(pow(pix.x-pbox.x,2)+pow(pix.y-pbox.y,2)); //calculate distance
       mindist=min(currentdist,mindist); //save new minimum
     }
     return mindist;
   }
   
   public PVector getMeanPos(){
     PVector meanvec= new PVector(0,0);
     for (PVector pbox : boxpixels) {
      meanvec.x+=pbox.x;
      meanvec.y+=pbox.y;
     }
     meanvec.x/=boxpixels.size();
     meanvec.y/=boxpixels.size();
     return meanvec;
   }
   
   public PVector getBoxSize(){ //returns widht and height as vector
     PVector upperleft= new PVector(MAX_FLOAT,MAX_FLOAT); //min x and y
     PVector lowerright= new PVector(0,0); //max x and y
     for (PVector pbox : boxpixels) {
       upperleft.x=min(upperleft.x, pbox.x);
       upperleft.y=min(upperleft.y, pbox.y);
       
       lowerright.x=max(lowerright.x, pbox.x);
       lowerright.y=max(lowerright.y, pbox.y);
     }
     PVector boxsize = new PVector(lowerright.x-upperleft.x, lowerright.y-upperleft.y);
     return boxsize;
   }
   
   public PVector getBoxCenter(){ //returns widht and height as vector
     PVector upperleft= new PVector(MAX_FLOAT,MAX_FLOAT); //min x and y
     PVector lowerright= new PVector(0,0); //max x and y
     for (PVector pbox : boxpixels) {
       upperleft.x=min(upperleft.x, pbox.x);
       upperleft.y=min(upperleft.y, pbox.y);
       
       lowerright.x=max(lowerright.x, pbox.x);
       lowerright.y=max(lowerright.y, pbox.y);
     }
     PVector boxcenter = new PVector((lowerright.x+upperleft.x)/2, (lowerright.y+upperleft.y)/2);
     return boxcenter;
   }
   
   public int getPixelNumber()  {
      return this.boxpixels.size(); 
   }
}

void colorBox(int id, color c) 
{
  sPort.write("B,"+str(id)+","+str(0)+","+int(red(c))+","+int(green(c))+","+int(blue(c))+"\n"); 
}
void colorAllOff() 
{
  sPort.write("A,0\n"); 
}