/*
 * Scan class.
 * Subclass of the KinectPoints class.
 *
 * Implements some functions to create, save and represent a mesh 
 * of the scanned points.
 */

class Scan extends KinectPoints{
  HE_Mesh mesh;
  color[] colors;
  PVector scanCenter;


  //
  // Constructors 
  //

  Scan(KinectPoints kp, ScanBox sb){
    super(kp.map3D,kp.rgbImg,kp.consImg);
    scanCenter = sb.boxCenter.get();
    
    float delta = sb.boxSize;
    for(int i = 0; i < nPoints; i++){
      consImg[i] = consImg[i] && (map3D[i].x > (scanCenter.x-0.5*delta)) && (map3D[i].x < (scanCenter.x+0.5*delta)) 
                              && (map3D[i].y > (scanCenter.y-0.5*delta)) && (map3D[i].y < (scanCenter.y+0.5*delta)) 
                              && (map3D[i].z > (scanCenter.z-0.5*delta)) && (map3D[i].z < (scanCenter.z+0.5*delta));
    }
  }

  Scan(PVector[] tempMap3D, PImage tempRGBimg, boolean[] tempConsImg, PVector tempScanCenter){
    super(tempMap3D,tempRGBimg,tempConsImg);
    scanCenter = tempScanCenter.get();
  }


  //
  // Class Methods 
  //
  
  // createMesh
  void createMesh(){
    ArrayList triangleList = new ArrayList(); 
    ArrayList colorList = new ArrayList();
    float maxSep = 90;  // Maximum separation allowed between two consecutive points
    
    // Populate triangleList and colorList
    rgbImg.loadPixels();
    for(int y = 0; y < ySize-1; y++){
      for(int x = 0; x < xSize-1; x++){
        int index = x + y*xSize;
        // First triangle
        if(consImg[index] && consImg[index+1] && consImg[index+xSize]){
          PVector p1 = map3D[index].get();
          PVector p2 = map3D[index+1].get();
          PVector p3 = map3D[index+xSize].get();
          float dist1 = p1.dist(p2);
          float dist2 = p1.dist(p3);
          float dist3 = p2.dist(p3);
          boolean cond = (dist1 < maxSep) && (dist2 < maxSep) && (dist3 < maxSep) &&
                         (dist1 != 0) && (dist2 != 0) && (dist3 != 0);
          if(cond){
            WB_Point3d hp1 = new WB_Point3d(p1.x,p1.y,p1.z);
            WB_Point3d hp2 = new WB_Point3d(p2.x,p2.y,p2.z);
            WB_Point3d hp3 = new WB_Point3d(p3.x,p3.y,p3.z);
            triangleList.add(new WB_ExplicitTriangle(hp1,hp2,hp3));
            colorList.add((color) rgbImg.pixels[index]);
          }
        }
        // Second triangle
        if(consImg[index+1] && consImg[index+1+xSize] && consImg[index+xSize]){
          PVector p1 = map3D[index+1].get();
          PVector p2 = map3D[index+1+xSize].get();
          PVector p3 = map3D[index+xSize].get();
          float dist1 = p1.dist(p2);
          float dist2 = p1.dist(p3);
          float dist3 = p2.dist(p3);
          boolean cond = (dist1 < maxSep) && (dist2 < maxSep) && (dist3 < maxSep) &&
                         (dist1 != 0) && (dist2 != 0) && (dist3 != 0);
          if(cond){
            WB_Point3d hp1 = new WB_Point3d(p1.x,p1.y,p1.z);
            WB_Point3d hp2 = new WB_Point3d(p2.x,p2.y,p2.z);
            WB_Point3d hp3 = new WB_Point3d(p3.x,p3.y,p3.z);
            triangleList.add(new WB_ExplicitTriangle(hp1,hp2,hp3));
            colorList.add((color) rgbImg.pixels[index]);
          }
        }
      }
    }
    println("Mesh info: "+triangleList.size()+" triangles");
   
    // Create the mesh from the triangleList
    HEC_FromTriangles creator = new HEC_FromTriangles();
    creator.setTriangles(triangleList);
    mesh = new HE_Mesh(creator); 
        
    // Transform colorList into a color Array (faster?) 
    colors = new color[colorList.size()];
    for(int i = 0; i < colors.length; i++){
      colors[i] = (color)(Integer) colorList.get(i);
    }
  }


  // drawMesh
  void drawMesh(){
    noStroke();
    Iterator<HE_Face> iter = mesh.fItr();
    for(int i = 0; iter.hasNext(); i++){
      HE_Face f = iter.next();
      fill(colors[i]);
      render.drawFace(f);
    }
  }


  // drawMesh
  void drawMesh(color col){
    noStroke();
    fill(col);
    render.drawFaces(mesh);
  }
    
    
  // saveMesh
  void saveMesh(String fileName){
    // Center the mesh first
    mesh.moveTo(0,0,0);
 
    HET_Export.saveToHemesh(mesh,sketchPath(fileName+".hemesh")); 
    HET_Export.saveToOBJ(mesh,sketchPath(fileName+".obj")); 
    println("Mesh saved in "+fileName+".hemesh and "+fileName+".obj");

    String[] s = new String[colors.length];
    for(int i = 0; i < s.length; i++){
      s[i] = red(colors[i])+" "+green(colors[i])+" "+blue(colors[i]);
    }
    saveStrings(fileName+".col",s);
    println("Mesh colors saved in "+fileName+".col");
  }
 
 
  // rotate
  void rotate(float rot){
    for(int i = 0; i < nPoints; i++){
      if(consImg[i]){
        map3D[i].sub(scanCenter);
        map3D[i].set(cos(rot)*map3D[i].x - sin(rot)*map3D[i].z, map3D[i].y, sin(rot)*map3D[i].x + cos(rot)*map3D[i].z);
        map3D[i].add(scanCenter);
      }  
    }
  }
 
 
  // crop
  Scan crop(){
    // Find the region in the image where the data is
    int xIni = xSize-1;
    int xEnd = 0;
    int yIni = ySize-1;
    int yEnd = 0;

    for(int y = 0; y < ySize; y++){
      for(int x = 0; x < xSize; x++){
        int index = x + y*xSize;
        if(consImg[index]){
          if(x < xIni) xIni = x;     
          if(x > xEnd) xEnd = x;     
          if(y < yIni) yIni = y;     
          if(y > yEnd) yEnd = y;     
        }
      }
    }

    // Dimensions of the cropped scan
    int xSizeCropped = xEnd - xIni + 1;
    int ySizeCropped = yEnd - yIni + 1;
    int nPointsCropped = xSizeCropped*ySizeCropped;
    PVector[] croppedMap3D = new PVector[nPointsCropped];
    PImage croppedRGBimg = createImage(xSizeCropped,ySizeCropped,RGB);
    boolean[] croppedConsImg = new boolean[nPointsCropped]; 

    // Populate the arrays
    croppedRGBimg.loadPixels();
    rgbImg.loadPixels();
    for(int y = 0; y < ySizeCropped; y++){
      for(int x = 0; x < xSizeCropped; x++){
        int index = (xIni+x) + (yIni+y)*xSize;
        int indexCropped = x + y*xSizeCropped;
        croppedMap3D[indexCropped] = map3D[index].get();
        croppedRGBimg.pixels[indexCropped] = rgbImg.pixels[index];
        croppedConsImg[indexCropped] = consImg[index];
      }
    }
    croppedRGBimg.updatePixels();

    return new Scan(croppedMap3D,croppedRGBimg,croppedConsImg,scanCenter);
  }
  
  
  // savePoints
  void savePoints(String fileName){
    // We crop first the scan to avoid writing unnecessary empty data points
    Scan cScan = this.crop();
    
    String[] s = new String[cScan.nPoints+1];
    s[0] = cScan.xSize+" "+cScan.ySize;
    
    cScan.rgbImg.loadPixels();
    for(int i = 0; i < s.length-1; i++){
      if(cScan.consImg[i]){
        PVector p = cScan.map3D[i].get();
        p.sub(scanCenter);
        color col = cScan.rgbImg.pixels[i];
        s[i+1] = p.x+" "+p.y+" "+p.z+" "+red(col)+" "+green(col)+" "+blue(col);
      }
      else{
        s[i+1] = "-99"+" "+"-99"+" "+"-99"+" "+"-99"+" "+"-99"+" "+"-99";
      }
    }
    
    saveStrings(fileName+".points",s);
    println("3D points saved in "+fileName+".points");
  }

}

/*
 * Peasycam style scene controls
 */

void mouseDragged(){
  rotX += map(mouseY-pmouseY,0,height,0,TWO_PI);
  rotY += map(mouseX-pmouseX,0,width,0,TWO_PI);
}

void keyPressed(){
  switch(keyCode){
    case UP:
      zoom *= 1.05;
      break;
    case DOWN:
      zoom /= 1.05;
      break;
  }
}

/*
 * This function combines a list of slits to create a single scan.
 */

Scan combineSlits(ArrayList slitList, boolean circular, boolean commonCenter){
  // Get the last slit to calculate the dimensions of the arrays.
  // It assumes all slits have the same dimensions. 
  Slit sLast = (Slit) slitList.get(slitList.size()-1);  
  boolean vertical = sLast.vertical;
  PVector scanCenter = sLast.slitCenter.get();
  
  int xSize, ySize;
  if(vertical){
    xSize = slitList.size();
    ySize = sLast.ySize;
  }
  else{
    xSize = sLast.xSize;
    ySize = slitList.size();
  }
  int nPoints = xSize*ySize;
  PVector[] map3D = new PVector[nPoints];    
  PImage rgbImg = createImage(xSize,ySize,RGB);     
  boolean[] consImg = new boolean[nPoints];  
  
  // Populate the arrays
  rgbImg.loadPixels();
  if(vertical){
    for(int x = 0; x < xSize; x++){
      Slit s = (Slit) slitList.get(x);
      s.rgbImg.loadPixels();  
      for(int y = 0; y < ySize; y++){
        int index = x + y*xSize;
        map3D[index] = s.map3D[y].get();
        rgbImg.pixels[index] = s.rgbImg.pixels[y];
        consImg[index] = s.consImg[y];
        if(consImg[index]){
          // Rotate along the y axis in case is requested
          if(circular){
            float rot = 4*(xSize - x)*TWO_PI/360; 
            map3D[index].sub(s.slitCenter);
            map3D[index].set(cos(rot)*map3D[index].x - sin(rot)*map3D[index].z, map3D[index].y, sin(rot)*map3D[index].x + cos(rot)*map3D[index].z);
            map3D[index].add(s.slitCenter);
          }
          // Or just shift it in x direction
          else{
            map3D[index].add(new PVector((xSize - x)*5,0,0));
          }
          // Reffer all slits to the same center
          if(commonCenter){
            map3D[index].sub(s.slitCenter);
            map3D[index].add(scanCenter);
          }
        }
      }
    }
  }
  else{
    for(int y = 0; y < ySize; y++){
      Slit s = (Slit) slitList.get(y);
      s.rgbImg.loadPixels();  
      for(int x = 0; x < xSize; x++){
        int index = x + y*xSize;
        map3D[index] = s.map3D[x].get();
        rgbImg.pixels[index] = s.rgbImg.pixels[x];
        consImg[index] = s.consImg[x];
        if(consImg[index]){
          // Rotate along the x axis in case is requested
          if(circular){
            float rot = 4*(ySize - y)*TWO_PI/360; 
            map3D[index].sub(s.slitCenter);
            map3D[index].set(map3D[index].x, cos(rot)*map3D[index].y - sin(rot)*map3D[index].z, sin(rot)*map3D[index].y + cos(rot)*map3D[index].z);
            map3D[index].add(s.slitCenter);
          }
          // Or just shift it in y direction
          else{
            map3D[index].add(new PVector(0,(ySize - y)*5,0));
          }
          // Reffer all slits to the same center
          if(commonCenter){
            map3D[index].sub(s.slitCenter);
            map3D[index].add(scanCenter);
          }
        }
      }
    }
  }
  rgbImg.updatePixels();
  
  return new Scan(map3D,rgbImg,consImg,scanCenter);  
}

/*
 * KinectPoints class.
 *
 * Simple class to save and draw the kinect points.
 */

class KinectPoints{
  int xSize;          // Hozizontal dimension
  int ySize;          // Vertical dimension
  int nPoints;        // Number of points
  PVector[] map3D;    // 3D points
  PImage rgbImg;      // Point colors 
  boolean[] consImg;  // Valid points 


  //
  // Constructors 
  //
  
  KinectPoints(PVector[] tempMap3D, PImage tempRGBimg, int[] tempDepthMap){
    xSize = tempRGBimg.width;
    ySize = tempRGBimg.height;
    nPoints = xSize*ySize;
    map3D = new PVector[nPoints];
    rgbImg = createImage(xSize,ySize,RGB); 
    consImg = new boolean[nPoints]; 
  
    rgbImg.loadPixels();
    tempRGBimg.loadPixels();
    for(int i = 0; i < nPoints; i++){
      map3D[i] = tempMap3D[i].get();
      rgbImg.pixels[i] = tempRGBimg.pixels[i];
      consImg[i] = (tempDepthMap[i] > 0);
    }
    rgbImg.updatePixels();
  }

  KinectPoints(PVector[] tempMap3D, PImage tempRGBimg, boolean[] tempConsImg){
    xSize = tempRGBimg.width;
    ySize = tempRGBimg.height;
    nPoints = xSize*ySize;
    map3D = new PVector[nPoints];
    rgbImg = createImage(xSize,ySize,RGB); 
    consImg = new boolean[nPoints]; 
  
    rgbImg.loadPixels();
    tempRGBimg.loadPixels();
    for(int i = 0; i < nPoints; i++){
      map3D[i] = tempMap3D[i].get();
      rgbImg.pixels[i] = tempRGBimg.pixels[i];
      consImg[i] = tempConsImg[i];
    }
    rgbImg.updatePixels();
  }  
  
  KinectPoints(){
    // Just define the global variables
  }  
  
  
  //
  // Class Methods 
  //
  
  // resize
  void resize(int n){
    if(n > 1){
      // Calculate the dimensions of the resized scan
      int xSizeRes = xSize/n;
      int ySizeRes = ySize/n;
      int nPointsRes = xSizeRes*ySizeRes;
      PVector[] resMap3D = new PVector[nPointsRes];
      PImage resRGBimg = createImage(xSizeRes,ySizeRes,RGB);
      boolean[] resConsImg = new boolean[nPointsRes]; 

      // Populate the arrays
      resRGBimg.loadPixels();
      rgbImg.loadPixels();
      for(int y = 0; y < ySizeRes; y++){
        for(int x = 0; x < xSizeRes; x++){
          int index = x*n + y*n*xSize;
          int indexRes = x + y*xSizeRes;
          resMap3D[indexRes] = map3D[index].get();
          resRGBimg.pixels[indexRes] = rgbImg.pixels[index];
          resConsImg[indexRes] = consImg[index];
        }
      }
      resRGBimg.updatePixels();
    
      // Update the scan to the new resolution
      nPoints = nPointsRes;
      xSize = xSizeRes;
      ySize = ySizeRes;
      map3D = resMap3D;
      rgbImg = resRGBimg;
      consImg = resConsImg;
    }
  }


  // constrain
  void constrain(float xMin, float xMax, float yMin, float yMax, float zMin, float zMax){
    for(int i = 0; i < nPoints; i++){
      consImg[i] = consImg[i] && (map3D[i].x > xMin) && (map3D[i].x < xMax) 
                              && (map3D[i].y > yMin) && (map3D[i].y < yMax) 
                              && (map3D[i].z > zMin) && (map3D[i].z < zMax);
    } 
  }
  
    
  // drawAsPixels
  void drawAsPixels(int pSize){
    strokeWeight(pSize);
    rgbImg.loadPixels();
    for(int i = 0; i < nPoints; i++){
      if(consImg[i]){
        stroke(rgbImg.pixels[i]);
        point(map3D[i].x,map3D[i].y,map3D[i].z);
      }  
    }
  }

  
  // drawAsPixels
  void drawAsPixels(int pSize, color col){
    strokeWeight(pSize);
    stroke(col);
    for(int i = 0; i < nPoints; i++){
      if(consImg[i]) point(map3D[i].x,map3D[i].y,map3D[i].z);  
    }
  }


  // drawAsBands
  void drawAsBands(){
    float maxSep = 90;         // Maximum separation allowed between two consecutive points
    boolean started = false;   // Controls when a band has been started
    
    noStroke();
    rgbImg.loadPixels();
    for(int y = 0; y < ySize-1; y++){
      for(int x = 0; x < xSize; x++){
        int index = x + y*xSize;
        if(consImg[index]){
          // Upper point
          fill(rgbImg.pixels[index]);
          PVector p = map3D[index].get();
          if(!started){
            beginShape(TRIANGLE_STRIP);
            vertex(p.x,p.y,p.z);
            started = true;
          }
          else if(p.dist(map3D[index-1]) < maxSep){  
            vertex(p.x,p.y,p.z);         
          }
          else{
            endShape();
            started = false;
            x--;  // It's a good point, use it in the next loop as starting point for a new band
            continue;
          }
          // Lower point         
          if(consImg[index+xSize]){
            PVector lp = map3D[index+xSize].get();
            if(p.dist(lp) < maxSep){
              fill(rgbImg.pixels[index+xSize]);
              vertex(lp.x,lp.y,lp.z);
            } 
            else{
              vertex(p.x,p.y,p.z);
            }            
          }
          else{
            vertex(p.x,p.y,p.z);
          }            
          // Finish the band if it is the last point in the row
          if(x == xSize-1){
            endShape();
            started = false;
          }
        }
        else if(started){
          // Upper point is not valid, let's see if we can use the lower point
          // for the last point in the band
          if(consImg[index+xSize]){
            PVector lp = map3D[index+xSize].get();
            if(lp.dist(map3D[index-1]) < maxSep){
              fill(rgbImg.pixels[index+xSize]);
              vertex(lp.x,lp.y,lp.z);
            }
          }
          endShape();
          started = false;
        }
      }
    }
  }

  
  // drawAsBands
  void drawAsBands(color col){
    float maxSep = 90;         // Maximum separation allowed between two consecutive points
    boolean started = false;   // Controls when a band has been started
    
    noStroke();
    fill(col);
    for(int y = 0; y < ySize-1; y++){
      for(int x = 0; x < xSize; x++){
        int index = x + y*xSize;
        if(consImg[index]){
          // Upper point
          PVector p = map3D[index].get();
          if(!started){
            beginShape(TRIANGLE_STRIP);
            vertex(p.x,p.y,p.z);
            started = true;
          }
          else if(p.dist(map3D[index-1]) < maxSep){  
            vertex(p.x,p.y,p.z);
          }
          else{
            endShape();
            started = false;
            x--;  // It's a good point, use it in the next loop as starting point for a new band
            continue;
          }
          // Lower point         
          if(consImg[index+xSize]){
            PVector lp = map3D[index+xSize].get();
            if(p.dist(lp) < maxSep){
              vertex(lp.x,lp.y,lp.z);
            } 
            else{
              vertex(p.x,p.y,p.z);
            }            
          }
          else{
            vertex(p.x,p.y,p.z);
          }            
          // Finish the band if it is the last point in the row
          if(x == xSize-1){
            endShape();
            started = false;
          }
        }
        else if(started){
          // Upper point is not valid, let's see if we can use the lower point
          // as the last point in the band
          if(consImg[index+xSize]){
            PVector lp = map3D[index+xSize].get();
            if(lp.dist(map3D[index-1]) < maxSep){
              vertex(lp.x,lp.y,lp.z);
            }
          }
          endShape();
          started = false;
        }
      }
    }
  }

 
  // getPoints
  PVector[] getPoints(){
    ArrayList pointList = new ArrayList();
    for(int i = 0; i < nPoints; i++){
      if(consImg[i]) pointList.add(map3D[i].get());
    }
    return (PVector[]) pointList.toArray(new PVector[pointList.size()]);
  }

}

/*
 * Slit class.
 * Subclass of the KinectPoints class.
 *
 * Contains the data points from a vertical or horizontal slit.
 */

class Slit extends KinectPoints{
  boolean vertical;     // false = horizontal
  PVector slitCenter;
    
    
  //
  // Constructor
  //
    
  Slit(KinectPoints kp, ScanBox sb, boolean tempVertical){
    super(); // Necessary for the variable definitions
    vertical = tempVertical;
    slitCenter = sb.boxCenter.get();
 
    // Define the dimensions of the arrays   
    if(vertical){
      xSize = 1;
      ySize = kp.ySize;
    }
    else{
      xSize = kp.xSize;
      ySize = 1;
    }
    nPoints = xSize*ySize;
    map3D = new PVector[nPoints];
    rgbImg = createImage(xSize,ySize,RGB);
    consImg = new boolean[nPoints];
    
    rgbImg.loadPixels();
    kp.rgbImg.loadPixels();
    if(vertical){
      // Find the x coordinate of the vertical slit     
      int slitPos = -1;
      float minDistance = 1000;
      float delta = sb.boxSize;
      for(int y = 0; y < kp.ySize; y++){
        for(int x = 0; x < kp.xSize; x++){
          int index = x + y*kp.xSize; 
          boolean cond = kp.consImg[index] && (kp.map3D[index].x > (slitCenter.x-0.5*delta)) && (kp.map3D[index].x < (slitCenter.x+0.5*delta)) 
                                           && (kp.map3D[index].y > (slitCenter.y-0.5*delta)) && (kp.map3D[index].y < (slitCenter.y+0.5*delta)) 
                                           && (kp.map3D[index].z > (slitCenter.z-0.5*delta)) && (kp.map3D[index].z < (slitCenter.z+0.5*delta));
          if(cond){
            float distance = abs(kp.map3D[index].x - slitCenter.x);
            if((distance < minDistance) && (distance < 5)){
              slitPos = x;
              minDistance = distance;
            }
          }
        }
      }
      // Populate the vertical slit
      for(int y = 0; y < ySize; y++){
        if(slitPos >= 0){
          int index = slitPos + y*kp.xSize;
          boolean cond = kp.consImg[index] && (kp.map3D[index].x > (slitCenter.x-0.5*delta)) && (kp.map3D[index].x < (slitCenter.x+0.5*delta)) 
                                           && (kp.map3D[index].y > (slitCenter.y-0.5*delta)) && (kp.map3D[index].y < (slitCenter.y+0.5*delta)) 
                                           && (kp.map3D[index].z > (slitCenter.z-0.5*delta)) && (kp.map3D[index].z < (slitCenter.z+0.5*delta));
          if(cond){
            map3D[y] = kp.map3D[index].get();
            rgbImg.pixels[y] = kp.rgbImg.pixels[index];
            consImg[y] = kp.consImg[index];
          }
          else{
            map3D[y] = new PVector(0,0,0);
            rgbImg.pixels[y] = color(0);
            consImg[y] = false;
          }
        }
        else{
          map3D[y] = new PVector(0,0,0);
          rgbImg.pixels[y] = color(0);
          consImg[y] = false;
        }
      }
    }
    else{
      // Find the y coordinate of the horizontal slit     
      int slitPos = -1;
      float minDistance = 1000;
      float delta = sb.boxSize;
      for(int y = 0; y < kp.ySize; y++){
        for(int x = 0; x < kp.xSize; x++){
          int index = x + y*kp.xSize; 
          boolean cond = kp.consImg[index] && (kp.map3D[index].x > (slitCenter.x-0.5*delta)) && (kp.map3D[index].x < (slitCenter.x+0.5*delta)) 
                                           && (kp.map3D[index].y > (slitCenter.y-0.5*delta)) && (kp.map3D[index].y < (slitCenter.y+0.5*delta)) 
                                           && (kp.map3D[index].z > (slitCenter.z-0.5*delta)) && (kp.map3D[index].z < (slitCenter.z+0.5*delta));
          if(cond){
            float distance = abs(kp.map3D[index].y - slitCenter.y);
            if((distance < minDistance) && (distance < 5)){
              slitPos = y;
              minDistance = distance;
            }
          }
        }
      }
      // Populate the horizontal slit
      for(int x = 0; x < xSize; x++){
        if(slitPos >= 0){
          int index = x + slitPos*kp.xSize;
          boolean cond = kp.consImg[index] && (kp.map3D[index].x > (slitCenter.x-0.5*delta)) && (kp.map3D[index].x < (slitCenter.x+0.5*delta)) 
                                           && (kp.map3D[index].y > (slitCenter.y-0.5*delta)) && (kp.map3D[index].y < (slitCenter.y+0.5*delta)) 
                                           && (kp.map3D[index].z > (slitCenter.z-0.5*delta)) && (kp.map3D[index].z < (slitCenter.z+0.5*delta));
          if(cond){
            map3D[x] = kp.map3D[index].get();
            rgbImg.pixels[x] = kp.rgbImg.pixels[index];
            consImg[x] = kp.consImg[index];
          }
          else{
            map3D[x] = new PVector(0,0,0);
            rgbImg.pixels[x] = color(0);
            consImg[x] = false;
          }
        }
        else{
          map3D[x] = new PVector(0,0,0);
          rgbImg.pixels[x] = color(0);
          consImg[x] = false;
        }
      }
    }  
    rgbImg.updatePixels();
  }

}

/*
 * This function calculates the initial limits of the scene
 */

void calculateLimits(KinectPoints kp){
  xmin = ymin = zmin = 100000;
  xmax = ymax = zmax = -100000;
  
  for(int i = 0; i < kp.nPoints; i++){
    if(kp.consImg[i]){
      PVector p = kp.map3D[i].get();
      if(p.x < xmin) xmin = p.x;
      if(p.x > xmax) xmax = p.x;
      if(p.y < ymin) ymin = p.y;
      if(p.y > ymax) ymax = p.y;
      if(p.z < zmin) zmin = p.z;
      if(p.z > zmax) zmax = p.z;
    }
  }
  
  // Extend a little the range
  float deltaX = 0.1*(xmax - xmin);
  float deltaY = 0.1*(ymax - ymin);
  float deltaZ = 0.1*(zmax - zmin);
  xmin -= deltaX;
  xmax += deltaX;
  ymin -= deltaY;
  ymax += deltaY;
  zmin -= deltaZ;
  zmax += deltaZ;
}  

/*
 * ControlP5 controls
 */

void controlPanel(){
  int deltaX = 10;
  int deltaY = 10;
  int stepX = 110;
  int stepY = 30;
  
  // Group coordinates
  float xPos = deltaX;
  float yPos = deltaY;
  
  // Initialize controlP5
  cp5 = new ControlP5(this);
  
  ControlWindow cw = cp5.addControlWindow("Kinect control",0,0,400,680);
  cw.hideCoordinates();
  cw.setBackground(color(10));
  
  
  // Group 1: General parameters

  yPos = yPos + 25;
  
  Group g1 = cp5.addGroup("g1");
  g1.setPosition(xPos,yPos).setHeight(25).setWidth(380).setBackgroundHeight(deltaY+5*stepY);
  g1.setCaptionLabel("General parameters");
  g1.getCaptionLabel().align(LEFT,CENTER).setPaddingX(10);
  g1.setBackgroundColor(color(255,20));
  g1.moveTo(cw);
  
  cp5.addToggle("drawBands",drawBands,deltaX,deltaY,15,15);
  cp5.getController("drawBands").setCaptionLabel("Draw bands");
  cp5.getController("drawBands").getCaptionLabel().align(ControlP5.RIGHT_OUTSIDE,CENTER).setPaddingX(10);
  cp5.getController("drawBands").setGroup(g1);

  cp5.addToggle("drawPixels",drawPixels,deltaX+stepX,deltaY,15,15);
  cp5.getController("drawPixels").setCaptionLabel("Draw pixels");
  cp5.getController("drawPixels").getCaptionLabel().align(ControlP5.RIGHT_OUTSIDE,CENTER).setPaddingX(10);
  cp5.getController("drawPixels").setGroup(g1);

  cp5.addBang("pointColors",deltaX+2*stepX,deltaY,15,15);
  cp5.getController("pointColors").setCaptionLabel("Real colors");
  cp5.getController("pointColors").getCaptionLabel().align(ControlP5.RIGHT_OUTSIDE,CENTER).setPaddingX(10);
  cp5.getController("pointColors").setGroup(g1);

  cp5.addSlider("resolution",1,10,resolution,deltaX,deltaY+stepY,300,15);
  cp5.getController("resolution").setCaptionLabel("Resolution");
  cp5.getController("resolution").getCaptionLabel().align(ControlP5.RIGHT_OUTSIDE,CENTER).setPaddingX(10);
  cp5.getController("resolution").setGroup(g1);
    
  cp5.addRange("xRange",xmin-0.1*(xmax-xmin),xmax+0.1*(xmax-xmin),xmin,xmax,deltaX,deltaY+2*stepY,300,15);
  cp5.getController("xRange").setCaptionLabel("X limits");
  cp5.getController("xRange").getCaptionLabel().align(ControlP5.RIGHT_OUTSIDE,CENTER).setPaddingX(10);
  cp5.getController("xRange").setGroup(g1);

  cp5.addRange("yRange",ymin-0.1*(ymax-ymin),ymax+0.1*(ymax-ymin),ymin,ymax,deltaX,deltaY+3*stepY,300,15);
  cp5.getController("yRange").setCaptionLabel("Y limits");
  cp5.getController("yRange").getCaptionLabel().align(ControlP5.RIGHT_OUTSIDE,CENTER).setPaddingX(10);
  cp5.getController("yRange").setGroup(g1);

  cp5.addRange("zRange",zmin-0.1*(zmax-zmin),zmax+0.1*(zmax-zmin),zmin,zmax,deltaX,deltaY+4*stepY,300,15);
  cp5.getController("zRange").setCaptionLabel("Z limits");
  cp5.getController("zRange").getCaptionLabel().align(ControlP5.RIGHT_OUTSIDE,CENTER).setPaddingX(10);
  cp5.getController("zRange").setGroup(g1);


  // Group 2: Scan box

  yPos = yPos + deltaY + 5*stepY + 1.3*stepY;

  Group g2 = cp5.addGroup("g2");
  g2.setPosition(xPos,yPos).setHeight(25).setWidth(380).setBackgroundHeight(deltaY+5*stepY);
  g2.setCaptionLabel("Scan box");
  g2.getCaptionLabel().align(LEFT,CENTER).setPaddingX(10);
  g2.setBackgroundColor(color(255,20));
  g2.moveTo(cw);

  cp5.addBang("centerInFace",deltaX,deltaY,15,15);
  cp5.getController("centerInFace").setCaptionLabel("Center in face using OpenCV 2");
  cp5.getController("centerInFace").getCaptionLabel().align(ControlP5.RIGHT_OUTSIDE,CENTER).setPaddingX(10);
  cp5.getController("centerInFace").setGroup(g2);

  cp5.addSlider("boxSize",10,500,sBox.boxSize,deltaX,deltaY+stepY,300,15);
  cp5.getController("boxSize").setCaptionLabel("Size");
  cp5.getController("boxSize").getCaptionLabel().align(ControlP5.RIGHT_OUTSIDE,CENTER).setPaddingX(10);
  cp5.getController("boxSize").setGroup(g2);

  cp5.addSlider("xBox",xmin,xmax,sBox.boxCenter.x,deltaX,deltaY+2*stepY,300,15);
  cp5.getController("xBox").setCaptionLabel("X pos");
  cp5.getController("xBox").getCaptionLabel().align(ControlP5.RIGHT_OUTSIDE,CENTER).setPaddingX(10);
  cp5.getController("xBox").setGroup(g2);

  cp5.addSlider("yBox",ymin,ymax,sBox.boxCenter.y,deltaX,deltaY+3*stepY,300,15);
  cp5.getController("yBox").setCaptionLabel("Y pos");
  cp5.getController("yBox").getCaptionLabel().align(ControlP5.RIGHT_OUTSIDE,CENTER).setPaddingX(10);
  cp5.getController("yBox").setGroup(g2);

  cp5.addSlider("zBox",zmin,zmax,sBox.boxCenter.z,deltaX,deltaY+4*stepY,300,15);
  cp5.getController("zBox").setCaptionLabel("Z pos");
  cp5.getController("zBox").getCaptionLabel().align(ControlP5.RIGHT_OUTSIDE,CENTER).setPaddingX(10);
  cp5.getController("zBox").setGroup(g2);


  // Group 3: Scan 

  yPos = yPos + deltaY + 5*stepY + 1.3*stepY;

  Group g3 = cp5.addGroup("g3");
  g3.setPosition(xPos,yPos).setHeight(25).setWidth(380).setBackgroundHeight(deltaY+3*stepY);
  g3.setCaptionLabel("Scan");
  g3.getCaptionLabel().align(LEFT,CENTER).setPaddingX(10);
  g3.setBackgroundColor(color(255,20));
  g3.moveTo(cw);

  cp5.addSlider("framesPerScan",1,30,framesPerScan,deltaX,deltaY,260,15);
  cp5.getController("framesPerScan").setCaptionLabel("Frames per scan");
  cp5.getController("framesPerScan").getCaptionLabel().align(ControlP5.RIGHT_OUTSIDE,CENTER).setPaddingX(10);
  cp5.getController("framesPerScan").setGroup(g3);

  cp5.addBang("takeScan",deltaX,deltaY+stepY,15,15);
  cp5.getController("takeScan").setCaptionLabel("Take scan");
  cp5.getController("takeScan").getCaptionLabel().align(ControlP5.RIGHT_OUTSIDE,CENTER).setPaddingX(10);
  cp5.getController("takeScan").setGroup(g3);

  cp5.addToggle("drawScan",drawScan,deltaX+stepX,deltaY+stepY,15,15);
  cp5.getController("drawScan").setCaptionLabel("Draw scan");
  cp5.getController("drawScan").getCaptionLabel().align(ControlP5.RIGHT_OUTSIDE,CENTER).setPaddingX(10);
  cp5.getController("drawScan").setGroup(g3);

  cp5.addBang("saveScan",deltaX,deltaY+2*stepY,15,15);
  cp5.getController("saveScan").setCaptionLabel("Save scan");
  cp5.getController("saveScan").getCaptionLabel().align(ControlP5.RIGHT_OUTSIDE,CENTER).setPaddingX(10);
  cp5.getController("saveScan").setGroup(g3);

  cp5.addToggle("createScanMesh",createScanMesh,deltaX+stepX,deltaY+2*stepY,15,15);
  cp5.getController("createScanMesh").setCaptionLabel("Include Hemesh mesh when saving");
  cp5.getController("createScanMesh").getCaptionLabel().align(ControlP5.RIGHT_OUTSIDE,CENTER).setPaddingX(10);
  cp5.getController("createScanMesh").setGroup(g3);


  // Group 4: Slit scan 

  yPos = yPos + deltaY + 3*stepY + 1.3*stepY;

  Group g4 = cp5.addGroup("g4");
  g4.setPosition(xPos,yPos).setHeight(25).setWidth(380).setBackgroundHeight(deltaY+3*stepY);
  g4.setCaptionLabel("Slit scan");
  g4.getCaptionLabel().align(LEFT,CENTER).setPaddingX(10);
  g4.setBackgroundColor(color(255,20));
  g4.moveTo(cw);

  cp5.addBang("orientationSlitScan",deltaX,deltaY,15,15);
  cp5.getController("orientationSlitScan").setCaptionLabel("Vertical slit");
  cp5.getController("orientationSlitScan").getCaptionLabel().align(ControlP5.RIGHT_OUTSIDE,CENTER).setPaddingX(10);
  cp5.getController("orientationSlitScan").setGroup(g4);

  cp5.addToggle("rotateSlitScan",rotateSlitScan,deltaX+stepX,deltaY,15,15);
  cp5.getController("rotateSlitScan").setCaptionLabel("Rotate");
  cp5.getController("rotateSlitScan").getCaptionLabel().align(ControlP5.RIGHT_OUTSIDE,CENTER).setPaddingX(10);
  cp5.getController("rotateSlitScan").setGroup(g4);

  cp5.addToggle("centerSlitScan",centerSlitScan,deltaX+2*stepX,deltaY,15,15);
  cp5.getController("centerSlitScan").setCaptionLabel("Move with the box");
  cp5.getController("centerSlitScan").getCaptionLabel().align(ControlP5.RIGHT_OUTSIDE,CENTER).setPaddingX(10);
  cp5.getController("centerSlitScan").setGroup(g4);

  cp5.addToggle("takeSlitScan",takeSlitScan,deltaX,deltaY+stepY,15,15);
  cp5.getController("takeSlitScan").setCaptionLabel("Start slit scan");
  cp5.getController("takeSlitScan").getCaptionLabel().align(ControlP5.RIGHT_OUTSIDE,CENTER).setPaddingX(10);
  cp5.getController("takeSlitScan").setGroup(g4);

  cp5.addToggle("drawSlitScan",drawSlitScan,deltaX+stepX,deltaY+stepY,15,15);
  cp5.getController("drawSlitScan").setCaptionLabel("Draw slit scan");
  cp5.getController("drawSlitScan").getCaptionLabel().align(ControlP5.RIGHT_OUTSIDE,CENTER).setPaddingX(10);
  cp5.getController("drawSlitScan").setGroup(g4);
  
  cp5.addBang("clearSlitScan",deltaX+2*stepX,deltaY+stepY,15,15);
  cp5.getController("clearSlitScan").setCaptionLabel("Clear");
  cp5.getController("clearSlitScan").getCaptionLabel().align(ControlP5.RIGHT_OUTSIDE,CENTER).setPaddingX(10);
  cp5.getController("clearSlitScan").setGroup(g4);
  
  cp5.addBang("saveSlitScan",deltaX,deltaY+2*stepY,15,15);
  cp5.getController("saveSlitScan").setCaptionLabel("Save slit scan");
  cp5.getController("saveSlitScan").getCaptionLabel().align(ControlP5.RIGHT_OUTSIDE,CENTER).setPaddingX(10);
  cp5.getController("saveSlitScan").setGroup(g4);
 
  cp5.addToggle("createSlitScanMesh",createScanMesh,deltaX+stepX,deltaY+2*stepY,15,15);
  cp5.getController("createSlitScanMesh").setCaptionLabel("Include Hemesh mesh when saving");
  cp5.getController("createSlitScanMesh").getCaptionLabel().align(ControlP5.RIGHT_OUTSIDE,CENTER).setPaddingX(10);
  cp5.getController("createSlitScanMesh").setGroup(g4);
}


// Manage events
void controlEvent(ControlEvent event){

  // Group 1 events
  if(event.isFrom("pointColors")){
    colorIterator++;
    if(colorIterator > 3) colorIterator = 0;
    switch(colorIterator){
      case 0:
        monochrome = false;
        event.getController().setCaptionLabel("Real colors");
        break;
      case 1:
        monochrome = true;
        monochromeCol = color(50,50,220);
        event.getController().setCaptionLabel("Blue");
        break;
      case 2:
        monochrome = true;
        monochromeCol = color(50,220,50);
        event.getController().setCaptionLabel("Green");
        break;
      case 3:
        monochrome = true;
        monochromeCol = color(220,50,50);
        event.getController().setCaptionLabel("Red");
        break;
    }
  }
  else if(event.isFrom("xRange")){
    xmin = event.getController().getArrayValue(0);
    xmax = event.getController().getArrayValue(1);
  }
  else if(event.isFrom("yRange")){
    ymin = event.getController().getArrayValue(0);
    ymax = event.getController().getArrayValue(1);
  }
  else if(event.isFrom("zRange")){
    zmin = event.getController().getArrayValue(0);
    zmax = event.getController().getArrayValue(1);
  }
  
  // Group 2 events
  else if(event.isFrom("centerInFace")){
    sBox.centerInFace(kPoints);
    cp5.getController("xBox").setValue(sBox.boxCenter.x);
    cp5.getController("yBox").setValue(sBox.boxCenter.y);
    cp5.getController("zBox").setValue(sBox.boxCenter.z);
  }
  else if(event.isFrom("boxSize")){
    sBox.boxSize = event.getController().getValue();
  }
  else if(event.isFrom("xBox")){
    sBox.boxCenter.x = event.getController().getValue();
  }
  else if(event.isFrom("yBox")){
    sBox.boxCenter.y = event.getController().getValue();
  }
  else if(event.isFrom("zBox")){
    sBox.boxCenter.z = event.getController().getValue();
  }
  
  // Group 3 events
  else if(event.isFrom("takeScan")){
    takeScan = true;
  }
  else if(event.isFrom("saveScan")){
    saveScan = true;
  }
  
  // Group 4 events
  else if(event.isFrom("orientationSlitScan")){
    verticalSlitScan = !verticalSlitScan;
    if(verticalSlitScan) event.getController().setCaptionLabel("Vertical slit");
    else event.getController().setCaptionLabel("Horizontal slit");
  }
  else if(event.isFrom("rotateSlitScan") || event.isFrom("centerSlitScan")){
    if(slits.size() > 0){
      slitScan = combineSlits(slits,rotateSlitScan,centerSlitScan);
    }
  }  
  else if(event.isFrom("takeSlitScan")){
    if(takeSlitScan) event.getController().setCaptionLabel("Stop slit scan");
    else event.getController().setCaptionLabel("Restart slit scan");
  }
  else if(event.isFrom("clearSlitScan")){
    slits.clear();
    cp5.getController("takeSlitScan").setCaptionLabel("Start slit scan");
  }
  else if(event.isFrom("saveSlitScan")){
    saveSlitScan = true;
  }
}

/*
 * ScanBox class.
 *
 * Scans are taken inside this box. 
 *
 * Uses OpenCV 2 to center the box in the first detected face.
 * If you don't have OpenCV installed in your computer, you should 
 * comment the lines inside the centerInFace() funtion, but 
 * centerInFace() still needs to be defined. 
 */

class ScanBox{
  PVector boxCenter;
  float boxSize;


  //
  // Constructor
  //
  
  ScanBox(PVector tempBoxCenter, float tempBoxSize){
    boxCenter = tempBoxCenter.get();
    boxSize = tempBoxSize;
  }
    

  //
  // Class Methods 
  //

  // paint
  void paint(){
    smooth();
    noFill();
    stroke(255);
    strokeWeight(1);

    pushMatrix();
      translate(boxCenter.x,boxCenter.y,boxCenter.z);
      line(-boxSize,0,0,boxSize,0,0);
      line(0,-boxSize,0,0,boxSize,0);
      line(0,0,-boxSize,0,0,boxSize);
      box(boxSize,boxSize,boxSize);
    popMatrix();

    noSmooth();
    noStroke();
  }
   
  
  // centerInFace
  void centerInFace(KinectPoints kp){
    // Initialize OpenCV
    OpenCV opencv = new OpenCV();
    opencv.allocate(kp.xSize,kp.ySize); 
    opencv.copy(kp.rgbImg);
    
    // Detect faces in the image 
    boolean debug = false;
    opencv.cascade("FRONTALFACE_ALT",debug); 
    Rectangle[] faces = opencv.detect(debug);
    
    if(faces.length == 1) println("Center in face: "+faces.length+" face detected");
    else println("Center in face: "+faces.length+" faces detected");
    
    if(faces.length > 0){
      // Get the first face 3D position
      int x = faces[0].x + int(faces[0].width/2);
      int y = faces[0].y + int(faces[0].height/2);
      int index = x + y*kp.xSize;
      if(kp.consImg[index]){
        boxCenter = kp.map3D[index].get();
        boxCenter.add(new PVector(0,0,100));  // add some offset 
        println("Center in face: Done (centered in the first face)");
      }
      else{
        println("Center in face: Invalid point. Try again");
      }
    }
    else{
      println("Center in face: Nothing done. Try again");
    }  
  }

}

/*
 * Simple 3D scanner using the Kinect (JGC, version 2).
 *
 * Select the scan area with the controls (or use the "center in face" option) 
 * and press the "take scan" button to capture the 3D points inside the box.
 * Press "save scan" to create a mesh with the Hemesh library and saved it in 
 * the sketch directory. The colors of the mesh faces are saved in a different file.
 * Press the "take scan" button again to take more scans.
 *
 * Do the same for the slit scans.
 *
 * Use http://www.openprocessing.org/sketch/62533 to read and represent the meshes.
 */

import SimpleOpenNI.*;
import controlP5.*;
import wblut.core.processing.*;
import wblut.hemesh.tools.*;
import wblut.hemesh.creators.*;
import wblut.hemesh.core.*;
import wblut.geom.core.*;
import java.awt.Rectangle;
// Comment this line if OpenCV 2 is not installed. Read also the comments in scanBox.pde
import monclubelec.javacvPro.*;  


// Prefix for the scan files. 
// Will overwrite previous scans if it's not changed.
public String file = "data/scan";

public boolean drawBands = true;
public boolean drawPixels = false;
public boolean monochrome = false;
public color   monochromeCol = color(50,50,220);
public int     colorIterator = 0;
public int     resolution = 2;
public float   xmin, xmax, ymin, ymax, zmin, zmax; 
public int     framesPerScan = 1;
public boolean takeScan = false;
public boolean drawScan = false;
public boolean saveScan = false;
public boolean createScanMesh = false;
public boolean verticalSlitScan = true;
public boolean rotateSlitScan = false;
public boolean centerSlitScan = false;
public boolean takeSlitScan = false;
public boolean drawSlitScan = true;
public boolean saveSlitScan = false;
public boolean createSlitScanMesh = false;

public WB_Render render;
public SimpleOpenNI context;
public KinectPoints kPoints;
public ScanBox sBox;
public TransparentFloor transparentFloor;       
public ControlP5 cp5;
public Scan scan;
public Scan slitScan;
public int scanCounter = 0;
public int slitScanCounter = 0;
public ArrayList scansToAverage = new ArrayList();
public ArrayList slits = new ArrayList();
public int frameIterator = 0;

public float zoom = 0.35;
public float rotX = PI;
public float rotY = 0;


void setup(){
  size(1024,768,P3D);
  perspective(radians(45),float(width)/float(height),10.0,150000.0);

  // Render for the meshes
  render = new WB_Render(this);

  // Initialize the Kinect
  context = new SimpleOpenNI(this); 
  context.setMirror(true);
  context.enableDepth();  
  context.enableRGB();
  context.alternativeViewPointDepthToImage();
  
  // Update the kinect to calculate the scene limits
  context.update(); 
  kPoints = new KinectPoints(context.depthMapRealWorld(),context.rgbImage(),context.depthMap());
  calculateLimits(kPoints);

  // Initialize the scan box
  sBox = new ScanBox(new PVector((xmax+xmin)/2,(ymax+ymin)/2,(zmax+zmin)/2),400);
  
  // Initialize the semitransparent floor
  transparentFloor = new TransparentFloor(xmin,xmax,ymin,ymax,zmin,zmax,color(0));

  // Initialize the controlP5 window. 
  // This should come after calculateLimits() and the scan box definition
  controlPanel(); 
}


void draw(){
  // Position the scene
  background(30);
  translate(width/2,height/2,0);
  rotateX(rotX);
  rotateY(rotY);
  scale(zoom);
  translate(0,0,-1500);

  // Draw the floor  
  transparentFloor.paint();
  
  // Draw the scan box
  sBox.paint();
 
  // Lights with real colors don't look very nice 
  if(monochrome) directionalLight(255,255,255,0,-0.2,1);

  // Update the kinect points
  context.update();
  kPoints = new KinectPoints(context.depthMapRealWorld(),context.rgbImage(),context.depthMap());
  kPoints.resize(resolution);
  kPoints.constrain(xmin,xmax,ymin,ymax,zmin,zmax);

  // Draw the kinect points
  if(drawBands && monochrome) kPoints.drawAsBands(monochromeCol);
  else if(drawBands) kPoints.drawAsBands();

  if(drawPixels && monochrome) kPoints.drawAsPixels(2,monochromeCol);
  else if(drawPixels) kPoints.drawAsPixels(2);

  // Take a scan
  if(takeScan){
    if(framesPerScan <= 1){
      scan = new Scan(kPoints,sBox);
      scanCounter++;
      takeScan = false;
      println("Take scan: Done (scan "+scanCounter+")");
    }
    else{
      if(frameIterator < framesPerScan){
        frameIterator++;
        scansToAverage.add(new Scan(kPoints,sBox));
        println("Take scan: Running (frame "+frameIterator+")");
      }
      else{
        scan = averageScans(scansToAverage);
        scansToAverage.clear();
        frameIterator = 0;
        scanCounter++;
        takeScan = false;
        println("Take scan: Done (scan "+scanCounter+")");
      }
    }
  }
  
  // Draw the last scan taken
  if(drawScan && (scanCounter > 0)){
    if(monochrome) scan.drawAsBands(monochromeCol);
    else scan.drawAsBands();
  }
  
  // Save the last scan taken
  if(saveScan && (scanCounter > 0)){
    String scanFileName = file+scanCounter;
    scan.savePoints(scanFileName);
    if(createScanMesh){
      scan.createMesh();
      scan.saveMesh(scanFileName);
    }
    saveScan = false;
  }
 
  // Take a slit scan
  if(takeSlitScan){
    slits.add(new Slit(kPoints,sBox,verticalSlitScan));
    slitScan = combineSlits(slits,rotateSlitScan,centerSlitScan);
    println("Take slit scan: Running ("+slits.size()+" slits)");
  }
  
  // Draw the last slit scan taken
  if(drawSlitScan && (slits.size() > 0)){
    if(monochrome) slitScan.drawAsBands(monochromeCol);
    else slitScan.drawAsBands();
  }
  
  // Save the last slit scan taken  
  if(saveSlitScan && (slits.size() > 0)){
    slitScanCounter++;
    String slitScanFileName = file+"-slit"+slitScanCounter; 
    slitScan.savePoints(slitScanFileName);
    if(createSlitScanMesh){
      slitScan.createMesh();
      slitScan.saveMesh(slitScanFileName);
    }
    saveSlitScan = false;
  }  

  // Write the frame rate in the screen title (thanks to amnon.owed for the tip!)
  frame.setTitle("Kinect 3D scanner // "+int(frameRate)+" fps");
}

/*
 * This function produces an average scan from a list of scans.
 *
 * Useful to reduce the noise in the data.
 * It produces also some funny/blurry effects if you move while the scans are taken.
 */

Scan averageScans(ArrayList scans){
  // Get the first scan to calculate the dimensions of the arrays.
  // It assumes all scans have the same dimensions. 
  Scan s0 = (Scan) scans.get(0);
  int xSize = s0.xSize;
  int ySize = s0.ySize;
  int nPoints = s0.nPoints;
  
  PVector[] map3D = new PVector[nPoints];
  PImage rgbImg = createImage(xSize,ySize,RGB); 
  boolean[] consImg = new boolean[nPoints]; 
  PVector scanCenter = new PVector(0,0,0);
  float[] n = new float[nPoints];  // number of points per pixel to average
  float[] r = new float[nPoints];  // red color 
  float[] g = new float[nPoints];  // green color
  float[] b = new float[nPoints];  // blue color
  
  // Initialize the arrays
  for(int i = 0; i < nPoints; i++){
    map3D[i] = new PVector(0,0,0);
    consImg[i] = false;
    n[i] = r[i] = g[i] = b[i] = 0;
  }
  
  // Loop over the scans and add the points and the colors
  for(int i = 0; i < scans.size(); i++){
    Scan s = (Scan) scans.get(i);
    scanCenter.add(s.scanCenter);
    s.rgbImg.loadPixels();
    for(int j = 0; j < nPoints; j++){
      if(s.consImg[j]){
        map3D[j].add(s.map3D[j]);
        consImg[j] = true;
        n[j]++;
        r[j] += red(s.rgbImg.pixels[j]);
        g[j] += green(s.rgbImg.pixels[j]);
        b[j] += blue(s.rgbImg.pixels[j]);
      }
    }
  }

  // Divide by the number of scans that contributed to the points
  scanCenter.div(scans.size());
  rgbImg.loadPixels();
  for(int i = 0; i < nPoints; i++){
    if(consImg[i]){
      map3D[i].div(n[i]);
      r[i] /= n[i];
      g[i] /= n[i];
      b[i] /= n[i];
      rgbImg.pixels[i] = color(r[i],g[i],b[i]);
    }
  }
  rgbImg.updatePixels();

  return new Scan(map3D,rgbImg,consImg,scanCenter);
}

/*
 * TransparentFloor class.
 *
 * Just a simple class for the floor. 
 */

class TransparentFloor{
  float xMin, xMax, yMin, yMax, zMin, zMax;
  PImage img;
  

  //
  // Constructor
  //
  
  TransparentFloor(float tempXmin, float tempXmax, float tempYmin, float tempYmax, float tempZmin, float tempZmax, color tempCol){
    xMin = tempXmin;
    xMax = tempXmax;
    yMin = tempYmin;
    yMax = tempYmax;
    zMin = tempZmin;
    zMax = tempZmax;

    // Create the image for the texture
    img = createImage(10,50,ARGB);
    img.loadPixels();
    for(int y = 0; y < img.height; y++){
      for(int x = 0; x < img.width; x++){
        float gradientAlpha = map(y,0,img.height,150,0);
        img.pixels[x + y*img.width] = color(red(tempCol),green(tempCol),blue(tempCol),gradientAlpha);
      }
    }
    img.updatePixels();
  }
    

  //
  // Class Methods 
  //

  // paint
  void paint(){
    smooth();
    beginShape();
      texture(img);
      vertex(xMin,yMin,zMin-0.25*(zMax-zMin),0,0);
      vertex(xMax,yMin,zMin-0.25*(zMax-zMin),img.width,0);
      vertex(xMax,yMin,zMax+0.5*(zMax-zMin),img.width,img.height);
      vertex(xMin,yMin,zMax+0.5*(zMax-zMin),0,img.height);
    endShape();
    noSmooth();
  }

}