import org.openkinect.freenect.*;
import org.openkinect.processing.*;
import processing.serial.*;

//zadig 2.0.1 libusbK  https://zadig.akeo.ie/downloads/

Serial sPort;

Kinect kinect;

// Depth image
PImage depthImg;
PImage depthImgRaw;
PImage videoImg;

public enum Mode
{
  DEPTHCOLOR, NEARESTIMAGECOLOR
}

Mode mode=Mode.DEPTHCOLOR;

boolean leftview_depth=true;
boolean enable_mirrorImage=true;
boolean enable_IRImage=false;
boolean enable_colordepth=false;

// Which pixels do we care about?
int minDepth =  0;
int maxDepth = 910;

// What is the kinect's angle
float angle;

static int boxnum=50; //maximum boxes
int boxnumloaded=boxnum; //can be changes if less boxes are loaded from config
color box[] = new color[boxnum];
int boxid[] = new int[boxnum];
PVector boxpos[] = new PVector[boxnum]; //center position of box
PVector boxsize[] = new PVector[boxnum]; //size of box (width,height)
boolean boxenabled[] = new boolean[boxnum];
float[] depthLookUp = new float[2048];

static int FRAMERATE=20;
//int fadetime=1000/FRAMERATE /10; //fadetime in centiseconds
int fadetime=1000/FRAMERATE /10 *4; //fadetime in centiseconds  SMOOOOTH

int editbox=0; //num of box
boolean editboxpos=false; //editmode

int infomessagecounter=0; //counts every frame
String infomessage="";


void setup() {
  size(1280, 480);
  frameRate(FRAMERATE);
  
  colorMode(HSB, 255);
  
  kinect = new Kinect(this);
  kinect.initDepth();
  kinect.initVideo();
  
  kinect.enableMirror(enable_mirrorImage);
  kinect.enableIR(enable_IRImage);
  kinect.enableColorDepth(enable_colordepth);
  angle = kinect.getTilt();
  

  // Blank image
  depthImgRaw = new PImage(kinect.width, kinect.height);
  depthImg = new PImage(kinect.width, kinect.height);
  
  for (int i=0;i<boxnum;i++) { //initialize boxpositions PVector
    boxpos[i]=new PVector();
    boxsize[i]=new PVector();
    boxenabled[i]=false;
  }
  
  //default config
  /*
  for (int i=0;i<boxnum;i++) { //initialize boxsizes
    boxsize[i]=new PVector();
    //boxsize[i].x=kinect.width/(boxnum+1);
    //boxsize[i].y=kinect.width/(boxnum+1);
    boxsize[i].x=kinect.width/(10+1);
    boxsize[i].y=kinect.width/(10+1);
  }*/
  
  /* for testing
  int _boxesperrow=10;
  for (int j=0;j<=int(boxnum/_boxesperrow);j++) {
    int _boxesthisrow=min(_boxesperrow,boxnum-j*_boxesperrow);
    for (int i=0;i<_boxesthisrow;i++) { //initialize boxpositions
      boxpos[i+j*10].x=(i+1)*(kinect.width/(_boxesthisrow+1)); //divide screen equally
      boxpos[i+j*10].y=kinect.height/2-boxsize[i+j*10].y*j;
      boxenabled[i+j*10]=true;
    }
  }
  */
  
  boxid[0]=15;
  boxid[1]=11;
  boxid[2]=3;
  boxid[3]=18;
  boxid[4]=2;
  boxid[5]=30;
  boxid[6]=7;
  boxid[7]=1;
  boxid[8]=27;
  boxid[9]=23;
  boxid[10]=6;
  boxid[11]=22;
  boxid[12]=14;
  boxid[13]=26;
  boxid[14]=4;
  boxid[15]=16;
  boxid[16]=28;
  boxid[17]=12;
  boxid[18]=21;
  
  String portName = Serial.list()[0];
  sPort = new Serial(this, portName, 115200);
  
  for (int i = 0; i < depthLookUp.length; i++) {
    depthLookUp[i] = rawDepthToMeters(i);
  }
  
  
  loadBoxConfig();
}

float rawDepthToMeters(int depthValue) {
  if (depthValue < 2047) {
    return (float)(1.0 / ((double)(depthValue) * -0.0030711016 + 3.3309495161));
  }
  return 0.0f;
}


void draw() {
  videoImg=kinect.getVideoImage();
  // Draw the raw image
  if (leftview_depth) {
    image(kinect.getDepthImage(), 0, 0);
  } else {
    image(videoImg, 0, 0);
  }
  
  
  // Threshold the depth image
  int[] rawDepth = kinect.getRawDepth();
  for (int i=0; i < rawDepth.length; i++) {
    float depthMeters=rawDepthToMeters(rawDepth[i]);
    if (rawDepthToMeters(rawDepth[i])<=0.0) { //depth can not be determined
      depthMeters=rawDepthToMeters(maxDepth); //set to maximum
    }
    depthImg.pixels[i] = color(constrain(map(depthMeters,rawDepthToMeters(minDepth),rawDepthToMeters(maxDepth),255,0) ,0,255)) ; //depth by brightness
  }
  
  

  // Draw the thresholded image
  depthImg.updatePixels();
  image(depthImg, kinect.width, 0);
  
  
  

  for (int i=0;i<boxnumloaded;i++) {
    float cpmean=0;
    int cpmeancounter=0;
    float cpmax=0;
    
    float videocolor_nearestR=0;
    float videocolor_nearestG=0;
    float videocolor_nearestB=0;
    float videocolor_nearest_counter=0;
    
    PVector _boxsize=boxsize[i];
    for (int offy=int(-_boxsize.y/2);offy<_boxsize.y/2;offy++) {
      for (int offx=int(-_boxsize.x/2);offx<_boxsize.x/2;offx++) {
        PVector _pos=new PVector(boxpos[i].x,boxpos[i].y);
        _pos.x+=offx;
        _pos.y+=offy;
      
        color cp = depthImg.get(int(_pos.x),int(_pos.y));
        if (cpmax<=brightness(cp)){
          cpmax=brightness(cp);
        }
        cpmean+=brightness(cp); //the brighter the closer
        cpmeancounter++;
        
        //video image
        color _vidcolor=videoImg.get(int(_pos.x),int(_pos.y));
        videocolor_nearestR+=red(_vidcolor)*(brightness(cp)/255.0);
        videocolor_nearestG+=green(_vidcolor)*(brightness(cp)/255.0);
        videocolor_nearestB+=blue(_vidcolor)*(brightness(cp)/255.0);
        videocolor_nearest_counter++;
      }
    }
    cpmean/=cpmeancounter;
    videocolor_nearestR/=videocolor_nearest_counter;
    videocolor_nearestG/=videocolor_nearest_counter;
    videocolor_nearestB/=videocolor_nearest_counter;
    colorMode(RGB, 255);
    color videocolor_nearest=color(videocolor_nearestR,videocolor_nearestG,videocolor_nearestB);
    colorMode(HSB, 255);
    
    
    //draw box
    noFill();
    stroke(150,255,100);
    if (!boxenabled[i]){
      stroke(0,255,100); //different rect color if disabled
    }
    rectMode(CENTER);
    textSize(12);
    textAlign(LEFT,TOP);
    rect(kinect.width+boxpos[i].x,boxpos[i].y, boxsize[i].x,boxsize[i].y); //draw on right image
    if (editboxpos && editbox==i) { //highlight current edited box
      line(kinect.width+boxpos[i].x-boxsize[i].x/2,boxpos[i].y-boxsize[i].y/2,kinect.width+boxpos[i].x+boxsize[i].x/2,boxpos[i].y+boxsize[i].y/2);
      line(kinect.width+boxpos[i].x+boxsize[i].x/2,boxpos[i].y-boxsize[i].y/2,kinect.width+boxpos[i].x-boxsize[i].x/2,boxpos[i].y+boxsize[i].y/2);
    }
    
    fill(box[i]); //preview boxcolor
    noStroke();
    rectMode(CENTER);
    rect(kinect.width+boxpos[i].x,boxpos[i].y, boxsize[i].x/8,boxsize[i].y/8); //draw on right image
    
    fill(150,255,255); //textcolor
    text(""+i+"|"+boxid[i],kinect.width+boxpos[i].x-boxsize[i].x/2,boxpos[i].y-boxsize[i].y/2);
    println("boxpos["+i+"].x="+boxpos[i].x+",boxpos["+i+"].y="+boxpos[i].y+", boxsize["+i+"].x="+boxsize[i].x+",boxsize["+i+"].y="+boxsize[i].y);
    
    
    int brightness=0;
    if (cpmax>0){
      //brightness=255;
      brightness=int(255-(cpmax-cpmean));
    }
    if (mode.equals(Mode.DEPTHCOLOR)){ //depth color
      box[i]=color(map(cpmax,0,255, 0,255),255,brightness);
    }else if(mode.equals(Mode.NEARESTIMAGECOLOR)) { //image color
      box[i]=color(hue(videocolor_nearest),saturation(videocolor_nearest),brightness);
    }
    if (i==5){
      println("cpmean="+cpmean);
      println("cpmax="+cpmax);
    }
    
  }

  if (editboxpos) {
    box[editbox]=color(0,0,255); //highlight current edited box 
  }
  sendToWuerfels();

  textSize(12);
  fill(0,255,150); //textcolor
  text("TILT: " + angle, 10, 20);
  text("THRESHOLD: [" + minDepth + ", " + maxDepth + "]", 10, 20+1*16);
  if(editboxpos){
    text("Edit Boxpos box="+editbox, 10, 20+2*16);
  }
  
  if (infomessagecounter>0){
    fill(100,50,255); //textcolor
    text(infomessage,10,height-20);
    infomessagecounter--;
  }
}

// Adjust the angle and the depth threshold min and max
void keyPressed() {
  if (key == CODED && !editboxpos) {
    if (keyCode == UP) {
      angle++;
    } else if (keyCode == DOWN) {
      angle--;
    }
    angle = constrain(angle, 0, 30);
    kinect.setTilt(angle);
  } else if (key == 'x') {
    minDepth = constrain(minDepth+10, 0, maxDepth);
  } else if (key == 'y') {
    minDepth = constrain(minDepth-10, 0, maxDepth);
  } else if (key == 'v') {
    maxDepth = constrain(maxDepth+10, minDepth, 2047);
  } else if (key =='c') {
    maxDepth = constrain(maxDepth-10, minDepth, 2047);
  } else if (key =='e') { //toggle edit boxpos mode
    editboxpos=!editboxpos;
    infomessagecounter=FRAMERATE*1;
    infomessage="editboxpos="+editboxpos;
  } else if (key =='s') { //save boxes
    saveBoxConfig();
  } else if (key =='l') { //load boxes
    loadBoxConfig();
  } else if (key =='u') { //toggle left view
    leftview_depth=!leftview_depth;
  } else if (key =='m') { // mirror
    enable_mirrorImage=!enable_mirrorImage;
    kinect.enableMirror(enable_mirrorImage);
  } else if (key =='i') { // toggle IR
    enable_IRImage=!enable_IRImage;
    kinect.enableIR(enable_IRImage);
  } else if (key =='k') { // toggle color as depth
    enable_colordepth=!enable_colordepth;
    kinect.enableColorDepth(enable_colordepth);
  } else if (key =='w') { // switch mode
    if (mode.equals(Mode.DEPTHCOLOR)){
      mode=Mode.NEARESTIMAGECOLOR;
    }else if (mode.equals(Mode.NEARESTIMAGECOLOR)){
      mode=Mode.DEPTHCOLOR;
    }
    infomessagecounter=FRAMERATE*2;
    infomessage="Mode="+mode.toString();
  } 
  
  
  
  
  if (editboxpos){ //in boxposedit mode
    if (key == '-') { //change boxedit id
      if (editbox>0){
        editbox--;
      }
    } else if (key == '+') { //change boxedit id
      if (editbox<boxnum-1){
        editbox++;
      }
    } else if (key == '4') { //change boxsize 
      boxsize[editbox].x--;
    } else if (key == '6') { //change boxsize 
      boxsize[editbox].x++;
    } else if (key == '2') { //change boxsize 
      boxsize[editbox].y--;
    } else if (key == '8') { //change boxsize 
      boxsize[editbox].y++;
    } else if (key == '*') { //apply this size to all boxes
      for (int ii=0;ii<boxnum;ii++) {
        if (ii!=editbox) {
          boxsize[ii].x=boxsize[editbox].x;
          boxsize[ii].y=boxsize[editbox].y;
        }
      }
    }
    if (key == CODED) {
      if (keyCode == LEFT) {
        boxpos[editbox].x-=1;
      } else if (keyCode == RIGHT) {
        boxpos[editbox].x+=1;
      }else if (keyCode == UP) {
        boxpos[editbox].y-=1;
      } else if (keyCode == DOWN) {
        boxpos[editbox].y+=1;
      }
    }
    
    infomessagecounter=FRAMERATE*2;
    infomessage=boxpos[editbox].x+", "+boxpos[editbox].y+": "+boxsize[editbox].x+", "+boxsize[editbox].y;
  }
}

void sendToWuerfels() {
  for (int i=0;i<boxnumloaded;i++) {
    if (boxenabled[i]) {
      //print(str(i)+"="+str(brightness(box[i]))+", ");
      sPort.write("B,"+str(boxid[i])+","+str(fadetime)+","+int(red(box[i]))+","+int(green(box[i]))+","+int(blue(box[i]))+"\n");
      print("B,"+str(boxid[i])+","+str(fadetime)+","+int(red(box[i]))+","+int(green(box[i]))+","+int(blue(box[i]))+"\n");
    }
  }
  println();
}

void mouseClicked() {
  if (editboxpos) {
    int _newposx=(mouseX-kinect.width);
    int _newposy=mouseY;
    boxpos[editbox].x=_newposx;
    boxpos[editbox].y=_newposy;
    println("Click "+_newposx+", "+_newposy);
    infomessagecounter=FRAMERATE*2;
    infomessage="x="+_newposx+" y="+_newposy;
  }
}

void saveBoxConfig(){
  String[] boxconfig=new String[boxnumloaded];
  for (int i=0;i<boxnumloaded;i++) {
    boxconfig[i]=boxid[i]+";"+(1.0*boxpos[i].x/kinect.width)+";"+(1.0*boxpos[i].y/kinect.height)+";"+(1.0*boxsize[i].x/kinect.width)+";"+(1.0*boxsize[i].y/kinect.height);
  }
  saveStrings("boxconfig.txt", boxconfig);
  println("saved");
  infomessagecounter=FRAMERATE*2;
  infomessage="saved";
}


void loadBoxConfig(){
  String[] boxconfig = loadStrings("boxconfig.txt");
  for (int i=0;i<boxnum;i++) { //disable boxes
    boxenabled[i]=false;
  }
  boxnumloaded = boxconfig.length;
  for (int i=0;i<min(boxconfig.length,boxnum);i++) {
    String[] _boxconfsplit=split(boxconfig[i], ';');
    boxid[i]=int(_boxconfsplit[0]);
    boxpos[i].x=int(float(_boxconfsplit[1])*kinect.width);
    boxpos[i].y=int(float(_boxconfsplit[2])*kinect.height);
    boxsize[i].x=int(float(_boxconfsplit[3])*kinect.width);
    boxsize[i].y=int(float(_boxconfsplit[4])*kinect.height);
    boxenabled[i]=true;
  }
  println("loaded");
  infomessagecounter=FRAMERATE*2;
  infomessage="loaded";
  
}