/*
 * EmptyModifier class.
 *
 * Subclass of the ScanModifier class
 */

public class EmptyModifier extends ScanModifier{

  //
  // Constructor
  //
  
  public EmptyModifier(){

  }

}



/*
 * ConvolveModifier class.
 *
 * Subclass of the ScanModifier class
 */

public class ConvolveModifier extends ScanModifier{
  private float convolveRadius = 8;
  

  //
  // Constructor
  //
  
  public ConvolveModifier(){

  }
  
  
  //
  // Class Methods 
  //

  // update
  public void update(int t){
    for(int y = 0; y < ySize; y++){
      if(!empty[y]){
        for(int x = ini[y]; x <= end[y]; x++){
          int index = x + y*xSize;
          boolean cond = consImgOrig[index] && x - 1 >= 0 && x + 1 < xSize && y - 1 >= 0 && y + 1 < ySize &&
                         consImgOrig[index-1] && consImgOrig[index+1] && consImgOrig[index - xSize] && consImgOrig[index + xSize];   
          if(cond){
            map3D[index] = PVector.add(map3DOrig[index],PVector.mult(normalsOrig[index],convolveRadius*(cos(float(t)*0.07)+0.3)));
          }
          else{
            map3D[index] = map3DOrig[index].get();
            consImg[index] = false;
          }
        }
      }
    }
    calculateNormals();
    calculateBackSide();
  }
  
}



/*
 * BallModifier class.
 *
 * Subclass of the ScanModifier class
 */

public class BallModifier extends ScanModifier{
  private float ballSize = 25;
  

  //
  // Constructor
  //

  public BallModifier(){

  }
  

  //
  // Class Methods 
  //

  // update
  public void update(int t){
    int cursorIndex = pointUnderCursor();
    if(cursorIndex >= 0){
      PVector cursorPos = map3DOrig[cursorIndex].get();
      for(int y = 0; y < ySize; y++){
        if(!empty[y]){
          for(int x = ini[y]; x <= end[y]; x++){
            int index = x + y*xSize;
            if(consImgOrig[index]){
              PVector p = map3DOrig[index].get();
              float distSq = sq(p.x-cursorPos.x) + sq(p.y-cursorPos.y) + sq(p.z-cursorPos.z);
              if(distSq < sq(ballSize)){
                PVector n = new PVector(0,0,-1);
                for(int j = 0; j < 2*ballSize; j++){
                  PVector delta = PVector.mult(n,j);
                  PVector newP = PVector.add(p,delta);
                  distSq = sq(newP.x-cursorPos.x) + sq(newP.y-cursorPos.y) + sq(newP.z-cursorPos.z);
                  if(distSq > sq(ballSize)){
                    map3D[index] = newP.get();
                    break;
                  }
                }
              }
              else{
                map3D[index] = map3DOrig[index].get();
              }
            }
          }
        }
      }
    }
  }
  
  
  // pointUnderCursor
  public int pointUnderCursor(){
    // Perspective parameters
    float fov = PI/3.0;
    float cameraZ = (height/2.0)/tan(fov/2.0);
    
    int cursorIndex = -1;
    float maxZ = -1000;
    float maxDistSq = sq(2*zoom);
    for(int y = 0; y < ySize; y++){
      if(!empty[y]){
        for(int x = ini[y]; x <= end[y]; x++){
          int index = x + y*xSize;
          if(consImgOrig[index]){
            // Scale and rotate the point as is shown in the screen
            PVector p = map3DOrig[index].get();
            p.mult(zoom);
            p = new PVector(p.x*cos(rotY)+p.z*sin(rotY),p.y,-p.x*sin(rotY)+p.z*cos(rotY));
            p = new PVector(p.x,p.y*cos(rotX)-p.z*sin(rotX),p.y*sin(rotX)+p.z*cos(rotX));
            p.add(0,0,0);
            
            // Calculate the cursor position at the same z plane as the point
            float scaling = (cameraZ-p.z)/cameraZ;
            PVector cursorPos = new PVector((mouseX-width/2.0)*scaling,(mouseY-height/2.0)*scaling,p.z);
            
            // Find the closest point to the cursor
            float distSq = sq(p.x-cursorPos.x) + sq(p.y-cursorPos.y) + sq(p.z-cursorPos.z);
            if(distSq < maxDistSq && p.z > maxZ){
              maxZ = p.z;
              cursorIndex = index;
            }
          }
        }
      }
    }
    return cursorIndex;
  }
  
}

/*
 * ScanConnector class.
 *
 * Usefull class to make the transition between two scans.
 */

public class ScanConnector{
  public ScanContainer iniScan;
  public ScanContainer endScan;
  public ScanContainer intermediateScan;
  public PVector[] separation;
  public float[] deltaRed;
  public float[] deltaGreen;
  public float[] deltaBlue;
  

  //
  // Constructor
  //

  public ScanConnector(ScanContainer tempIniScan, ScanContainer tempEndScan){
    iniScan = new ScanContainer();
    iniScan.create(tempIniScan);
    iniScan.centerAndExtend();
    endScan = new ScanContainer();
    endScan.create(tempEndScan);
    endScan.centerAndExtend();

    // New dimensions of the scans
    int xSize = max(iniScan.xSize,endScan.xSize);
    int ySize = max(iniScan.ySize,endScan.ySize);
    int nPoints = xSize*ySize;
    iniScan.extend(xSize,ySize);
    endScan.extend(xSize,ySize);
    separation = new PVector[nPoints];
    deltaRed = new float[nPoints];
    deltaGreen = new float[nPoints];
    deltaBlue = new float[nPoints];
    
    
    // Populate the scans in the vertical direction
    int firstIni = -1;
    for(int y = 0; y < ySize; y++){
      if(!iniScan.empty[y]){
        firstIni = y;
        break;
      }
    }
    
    int firstEnd = -1;
    for(int y = 0; y < ySize; y++){
      if(!endScan.empty[y]){
        firstEnd = y;
        break;
      }
    }
    
    iniScan.rgbImg.loadPixels();
    endScan.rgbImg.loadPixels();
    for(int y = 0; y < ySize; y++){
      if(!(iniScan.empty[y] && endScan.empty[y])){
        if(iniScan.empty[y] && y < firstIni){
          for(int x = 0; x < xSize; x++){
            int index = x + y*xSize;
            int firstIndex = x + firstIni*xSize;
            iniScan.map3D[index] = iniScan.map3D[firstIndex].get();
            iniScan.rgbImg.pixels[index] = iniScan.rgbImg.pixels[firstIndex];
            iniScan.consImg[index] = iniScan.consImg[firstIndex];
          }
        }
        else if(iniScan.empty[y] && y > firstIni){
          for(int x = 0; x < xSize; x++){
            int index = x + y*xSize;
            int prevIndex = x + (y-1)*xSize;
            iniScan.map3D[index] = iniScan.map3D[prevIndex].get();
            iniScan.rgbImg.pixels[index] = iniScan.rgbImg.pixels[prevIndex];
            iniScan.consImg[index] = iniScan.consImg[prevIndex];
          }
        }
        if(endScan.empty[y] && y < firstEnd){
          for(int x = 0; x < xSize; x++){
            int index = x + y*xSize;
            int firstIndex = x + firstEnd*xSize;
            endScan.map3D[index] = endScan.map3D[firstIndex].get();
            endScan.rgbImg.pixels[index] = endScan.rgbImg.pixels[firstIndex];
            endScan.consImg[index] = endScan.consImg[firstIndex];
          }
        }
        else if(endScan.empty[y] && y > firstEnd){
          for(int x = 0; x < xSize; x++){
            int index = x + y*xSize;
            int prevIndex = x + (y-1)*xSize;
            endScan.map3D[index] = endScan.map3D[prevIndex].get();
            endScan.rgbImg.pixels[index] = endScan.rgbImg.pixels[prevIndex];
            endScan.consImg[index] = endScan.consImg[prevIndex];
          }
        }
      }
    }
    iniScan.rgbImg.updatePixels();
    endScan.rgbImg.updatePixels();
    
    // Update the extremes
    iniScan.findExtremes();
    endScan.findExtremes();


    // Populate the scans in the horizontal direction
    int[] ini = new int[ySize];
    int[] end = new int[ySize];
    boolean[] empty = new boolean[ySize];

    for(int y = 0; y < ySize; y++){
      ini[y] = min(iniScan.ini[y],endScan.ini[y]);
      end[y] = max(iniScan.end[y],endScan.end[y]);
      empty[y] = iniScan.empty[y] && endScan.empty[y];
    }

    iniScan.rgbImg.loadPixels();
    endScan.rgbImg.loadPixels();
    for(int y = 0; y < ySize; y++){
      if(!empty[y]){
        for(int x = ini[y]; x <= end[y]; x++){
          int index = x + y*xSize;
          if(!iniScan.consImg[index] && x < iniScan.ini[y]){
            int indexIni = iniScan.ini[y] + y*xSize;
            iniScan.map3D[index] = iniScan.map3D[indexIni].get();
            iniScan.rgbImg.pixels[index] = iniScan.rgbImg.pixels[indexIni];
            iniScan.consImg[index] = iniScan.consImg[indexIni];
          }
          else if(!iniScan.consImg[index] && x > iniScan.ini[y]){
            int prevIndex = (x-1) + y*xSize;
            iniScan.map3D[index] = iniScan.map3D[prevIndex].get();
            iniScan.rgbImg.pixels[index] = iniScan.rgbImg.pixels[prevIndex];
            iniScan.consImg[index] = iniScan.consImg[prevIndex];
          }
          if(!endScan.consImg[index] && x < endScan.ini[y]){
            int indexIni = endScan.ini[y] + y*xSize;
            endScan.map3D[index] = endScan.map3D[indexIni].get();
            endScan.rgbImg.pixels[index] = endScan.rgbImg.pixels[indexIni];
            endScan.consImg[index] = endScan.consImg[indexIni];
          }
          else if(!endScan.consImg[index] && x > endScan.ini[y]){
            int prevIndex = (x-1) + y*xSize;
            endScan.map3D[index] = endScan.map3D[prevIndex].get();
            endScan.rgbImg.pixels[index] = endScan.rgbImg.pixels[prevIndex];
            endScan.consImg[index] = endScan.consImg[prevIndex];
          }
        }
      }
    }
    iniScan.rgbImg.updatePixels();
    endScan.rgbImg.updatePixels();

    // Update the extremes
    iniScan.findExtremes();
    endScan.findExtremes();


    // Populate the arrays
    iniScan.rgbImg.loadPixels();
    endScan.rgbImg.loadPixels();
    for(int i = 0; i < nPoints; i++){
      separation[i] = PVector.sub(endScan.map3D[i],iniScan.map3D[i]);
      deltaRed[i] = red(endScan.rgbImg.pixels[i]) - red(iniScan.rgbImg.pixels[i]);
      deltaGreen[i] = green(endScan.rgbImg.pixels[i]) - green(iniScan.rgbImg.pixels[i]);
      deltaBlue[i] = blue(endScan.rgbImg.pixels[i]) - blue(iniScan.rgbImg.pixels[i]);
    }

    // Begin from the initial scan
    intermediateScan = new ScanContainer();
    intermediateScan.create(iniScan);
  }
  
  
  //
  // Class Methods 
  //

  // update
  public void update(float t, float steps){
    if(t >=0 && t <= steps){
      int xSize = intermediateScan.xSize;
      int ySize = intermediateScan.ySize;
    
      intermediateScan.rgbImg.loadPixels();
      iniScan.rgbImg.loadPixels();
      for(int y = 0; y < ySize; y++){
        if(!intermediateScan.empty[y]){
          for(int x = intermediateScan.ini[y]; x <= intermediateScan.end[y]; x++){
            int index = x + y*xSize;
            intermediateScan.map3D[index] = PVector.add(iniScan.map3D[index],PVector.mult(separation[index],t/steps));
            intermediateScan.rgbImg.pixels[index] = color(red(iniScan.rgbImg.pixels[index])+deltaRed[index]*(t/steps), 
                                                          green(iniScan.rgbImg.pixels[index])+deltaGreen[index]*(t/steps),
                                                          blue(iniScan.rgbImg.pixels[index])+deltaBlue[index]*(t/steps));
          }
        }
      }
      intermediateScan.rgbImg.updatePixels();

      intermediateScan.calculateNormals();
      intermediateScan.calculateBackSide();
    }
  }


  // drawAsPixels
  public void drawAsPixels(int pSize){
    intermediateScan.drawAsPixels(pSize);
  }


  // drawAsLines
  public void drawAsLines(){
    intermediateScan.drawAsLines();
  }


  // drawAsTriangles
  public void drawAsTriangles(){
    intermediateScan.drawAsTriangles();
  }


  // drawAsPsyTriangles
  public void drawAsPsyTriangles(){
    intermediateScan.drawAsPsyTriangles();
  }


  // drawBackSide
  public void drawBackSide(color c){
    intermediateScan.drawBackSide(c);
  }
  
}

/*
 * Scene controls
 */

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


void mouseReleased(){
  cursor();
}


void keyPressed(){
  switch(keyCode){
    case UP:
      zoom *= 1.02;
      break;
    case DOWN:
      zoom /= 1.02;
      break;
    case LEFT:
      resolution++;
      if(resolution > 5){
        resolution = 5;
      }
      else{
        if(drawMask){
          sMask = createMask(scans[currentScan],maskType);
          sMask.reduceResolution(resolution);
          counter = 0;
        }
        else if(drawModifier){
          sModifier = createModifier(scans[currentScan],modifierType);
          sModifier.reduceResolution(resolution);
          counter = 0;
        }
      }
      break;
    case RIGHT:
      resolution--;
      if(resolution < 1){
        resolution = 1;
      }
      else{
        if(drawMask){
          sMask = createMask(scans[currentScan],maskType);
          sMask.reduceResolution(resolution);
          counter = 0;
        }
        else if(drawModifier){
          sModifier = createModifier(scans[currentScan],modifierType);
          sModifier.reduceResolution(resolution);
          counter = 0;
        }
      }
      break;
  }
  
  if(key == ' '){
    psychedelic = !psychedelic;
  }
  if(key == 'l'){
    asLines = !asLines;
    asPixels = false;
  }
  if(key == 'p'){
    asPixels = !asPixels;
    asLines = false;
  }
  if(key == '+'){
    pixelSize++;
    if(pixelSize > 3) pixelSize = 3;
  }
  if(key == '-'){
    pixelSize--;
    if(pixelSize < 1) pixelSize = 1;
  } 
  if(key == 'c'){
    if(drawMask) sMask.centerOnCursor();
    else sModifier.centerOnCursor();
    counter = 0;
  } 
  if(key == 's'){
    //if(drawMask) sMask.savePoints();
    //else if(drawModifier) sModifier.savePoints();
    //save("pic.jpg");
  }
  if(key == 'i'){
    //if(drawMask) sMask.pointCoordinates();
    //else if(drawModifier) sModifier.pointCoordinates();
  }
  if(key == 'z'){
    //if(drawMask) sMask.scaleFactor(1.1);
    //else if(drawModifier) sModifier.scaleFactor(1.1);
  }
}

/*
 * Returns a ScanMask of the selected type
 */

ScanMask createMask(ScanContainer s, String type){
  if(type == "empty"){
    ScanMask m = new EmptyMask();
    m.create(s,true);
    return m;
  }
  else if(type == "linear"){
    ScanMask m = new LinearMask();
    m.create(s,true);
    return m;
  }
  else if(type == "circular"){
    ScanMask m = new CircularMask();
    m.create(s,true);
    return m;
  }
  else if(type == "random"){
    ScanMask m = new RandomMask();
    m.create(s,false);
    return m;
  }
  else if(type == "balls"){
    ScanMask m = new BallsMask();
    m.create(s,false);
    return m;
  }
  else if(type == "traces"){
    ScanMask m = new TracesMask();
    m.create(s,false);
    return m;
  }
  else if(type == "cursor"){
    ScanMask m = new CursorMask();
    m.create(s,false);
    return m;
  }
  else if(type == "diffusion"){
    ScanMask m = new DLAMask();
    m.create(s,false);
    return m;
  }
  else{
    ScanMask m = new EmptyMask();
    m.create(s,true);
    return m;
  }
}

/* 
 * 3D drawing machines (JGC).
 *
 * Description: Reads and represents scans obtained with the Kinect 3D scanner.
 *   http://www.openprocessing.org/sketch/78606
 * 
 * Controls:
 *   - Use the mouse to rotate the scan
 *   - Use "Up" and "Down" arrows to zoom
 *   - Use "Left" and "Right" arrows to decrease/increase the scan resolution
 *   - 'p' switchs between the mesh view and the pixels view
 *   - 'l' switchs between the mesh view and the lines view
 *   - "+" and "-" increase/decrease the pixel size (only in pixel view)
 *   - 'Space' creates some psychedelic effects (only in mesh view) 
 *   - 'c' centers the scan on the cursor position
 *   - 's' saves the scan 
 */

import controlP5.*;

public String[] files = {"scan1.points","diego1.points","chloe1.points","scan3.points"};
public String[] maskTypes = {"linear","circular","random","balls","traces","cursor","diffusion"};
public String[] modifierTypes = {"convolve","ballMod"};
public int currentScan = 0;
public String maskType = "empty";
public String modifierType = "empty";
public ScanContainer[] scans;
public ScanMask sMask;
public ScanModifier sModifier;
public ScanConnector sConnector;

public boolean drawMask = true;
public boolean drawModifier = false;
public boolean drawMorphing = false;
public boolean asPixels = false;
public boolean asLines = false;
public boolean psychedelic = false;
public int initResolution = 1;
public int resolution = initResolution;
public int pixelSize = 1;
public int counter = 0;
public int morphingIterator = 1;
public int morphingSteps = 30;

public ControlP5 cp5;
public RadioButton scanButtons;
public RadioButton maskButtons;
public RadioButton modifierButtons;
public Toggle morphingToggle;

public PImage bgImg;

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


void setup(){
  size(750,600,P3D);
  
  // Read the scans
  scans = new ScanContainer[files.length];
  for(int i = 0; i < files.length; i++){
    ScanContainer s = new ScanContainer();
    s.createFromFile(files[i]);
    s.reduceResolution(initResolution);
    s.fillHoles(15);
    s.gaussianSmooth(4);
    //s.constrainPoints(-1000,1000,-1000,1000,-1000,1000);
    s.crop();
    scans[i] = s;
  }

  // Create an image to represent the background
  bgImg = createBackgroundImg(color(220));

  // Initialize ControlP5
  controlPanel();
}


void draw(){
  // Draw the background image
  background(bgImg);

  // Draw the control panel
  cp5.draw();

  // Position the scene
  translate(width/2,height/2,0);
  rotateX(rotX);
  rotateY(rotY);
  scale(zoom);
   
  // Draw the ScanMask, the ScanModifier or the ScanConnector
  if(drawMask){
    sMask.update(counter);
    
    if(asPixels){
      sMask.drawAsPixels(pixelSize);
    }
    else if(asLines){
      sMask.drawAsLines();
    }
    else if(psychedelic){
      sMask.drawAsPsyTriangles();
      lights();
      sMask.drawBackSide(color(255));
    }
    else{
      sMask.drawAsTriangles();
      lights();
      sMask.drawBackSide(color(255));
    }
  }
  else if(drawModifier){
    sModifier.update(counter);
    
    if(asPixels){
      sModifier.drawAsPixels(pixelSize);
    }
    else if(asLines){
      sModifier.drawAsLines();
    }
    else if(psychedelic){
      sModifier.drawAsPsyTriangles();
      //lights();
      //sModifier.drawBackSide(color(255));
    }
    else{
      sModifier.drawAsTriangles();
      //lights();
      //sModifier.drawBackSide(color(255));
    }
  }
  else if(drawMorphing && scans.length > 1){
    if(counter > morphingSteps){
      counter = 0;
      morphingIterator++;
      if(morphingIterator == scans.length){
        sConnector = new ScanConnector(scans[morphingIterator-1],scans[0]);
        morphingIterator = 0;
      }
      else{
        sConnector = new ScanConnector(scans[morphingIterator-1],scans[morphingIterator]);
      }
    }
    
    sConnector.update(counter,morphingSteps);
    
    if(asPixels){
      sConnector.drawAsPixels(pixelSize);
    }
    else if(asLines){
      sConnector.drawAsLines();
    }
    else if(psychedelic){
      sConnector.drawAsPsyTriangles();
      lights();
      sConnector.drawBackSide(color(255));
    }
    else{
      sConnector.drawAsTriangles();
      lights();
      sConnector.drawBackSide(color(255));
    }
  }

  counter++;
}

/*
 * Creates a background image with a circular gradient
 */

PImage createBackgroundImg(color col){
  PImage img = createImage(width,height,RGB);

  img.loadPixels();
  for (int y = 0; y < height; y++){
    for (int x = 0; x < width; x++){
      float distance = sqrt(sq(x-width/2)+sq(y-height/2));
      float gradientR = map(distance,0,width/2,red(col),80);
      float gradientG = map(distance,0,width/2,green(col),80);
      float gradientB = map(distance,0,width/2,blue(col),80);
      img.pixels[x + y*width] = color(gradientR,gradientG,gradientB);
    }
  }
  img.updatePixels();
  
  return img;
}

/*
 * EmptyMask class
 *
 * Subclass of the ScanMask class
 */

public class EmptyMask extends ScanMask{
  
  //
  // Constructor
  //

  public EmptyMask(){

  }
  
}



/*
 * LinearMask class
 *
 * Subclass of the ScanMask class
 */

public class LinearMask extends ScanMask{
  private float[] distance;
  private float repetitionInterval;
  public float speed = 4;
  
  
  //
  // Constructor
  //

  public LinearMask(){

  }


  //
  // Class Methods 
  //

  // init
  public void init(){
    float maxDist = 0;
    distance = new float[nPoints];
    for(int i = 0; i < nPoints; i++){
      if(consImgOrig[i]){
        distance[i] = abs(map3D[i].x-centralPos.x);
        if(distance[i] > maxDist){
          maxDist = distance[i];
        }
      }
      else{
        distance[i] = 0;
      } 
    }
    
    repetitionInterval = maxDist;
  }


  // update
  public void update(int t){
    if(t > 0){
      float distLimit = speed*t%repetitionInterval;
      for(int y = 0; y < ySize; y++){
        if(!empty[y]){
          for(int x = ini[y]; x <= end[y]; x++){
            int index = x + y*xSize;
            if(distance[index] < distLimit){
              maskImg[index] = !value;
            }
            else{
              maskImg[index] = value;
            }
          }
        }
      }
      addMask();
      calculateBackSide();
    }
  }

}



/*
 * CircularMask class
 *
 * Subclass of the ScanMask class
 */

public class CircularMask extends ScanMask{
  private float[] distSq;
  private float repetitionInterval;
  public float speed = 5;
  
  
  //
  // Constructor
  //

  public CircularMask(){

  }


  //
  // Class Methods 
  //

  // init
  public void init(){
    float maxDistSq = 0;
    distSq = new float[nPoints];
    for(int i = 0; i < nPoints; i++){
      if(consImgOrig[i]){
        distSq[i] = sq(map3D[i].x-centralPos.x) + sq(map3D[i].y-centralPos.y) + sq(map3D[i].z-centralPos.z);
        if(distSq[i] > maxDistSq){
          maxDistSq = distSq[i];
        }
      }
      else{
        distSq[i] = 0;
      } 
    }
    
    repetitionInterval = sqrt(maxDistSq);
  }


  // update
  public void update(int t){
    if(t > 0){
      float distLimitSq = sq(speed*t%repetitionInterval);
      for(int y = 0; y < ySize; y++){
        if(!empty[y]){
          for(int x = ini[y]; x <= end[y]; x++){
            int index = x + y*xSize;
            if(distSq[index] < distLimitSq){
              maskImg[index] = !value;
            }
            else{
              maskImg[index] = value;
            }
          }
        }
      }
      addMask();
      calculateBackSide();
    }
  }

}



/*
 * RandomMask class
 *
 * Subclass of the ScanMask class
 */

public class RandomMask extends ScanMask{
  private int seeds;
  private int[] xPos;
  private int[] yPos;
  
  
  //
  // Constructor
  //

  public RandomMask(){

  }

 
  //
  // Class Methods 
  //

  // init
  public void init(){
    seeds = int(0.002*nPoints);
    if(seeds < 4) seeds = 4;
    
    xPos = new int[seeds];
    yPos = new int[seeds];

    for(int i = 0; i < seeds; ){
      xPos[i] = int(random(xSize));
      yPos[i] = int(random(ySize));
      if(consImgOrig[xPos[i] + yPos[i]*xSize]){
        i++;
      }
    }
  }


  // update
  public void update(int t){
    for(int i = 0; i < seeds; i++){
      switch(int(random(4))){
        case 0: 
          if(xPos[i] + 1 < xSize) xPos[i]++;
          break;
        case 1:  
          if(yPos[i] + 1 < ySize) yPos[i]++;
          break;
        case 2:  
          if(xPos[i] - 1 >= 0) xPos[i]--;
          break;
        case 3:  
          if(yPos[i] - 1 >= 0) yPos[i]--;
          break;
      }
      maskImg[xPos[i] + yPos[i]*xSize] = !value;
    }
    addMask();
    calculateBackSide();
  }

}



/*
 * BallsMask class
 *
 * Subclass of the ScanMask class
 */

public class BallsMask extends ScanMask{
  private PGraphics pg;
  private int nBalls;
  private float ballSize = 10;
  private color ballColor = color(0);
  private Ball[] balls;

  
  //
  // Constructor
  //

  public BallsMask(){

  }

  
  //
  // Class Methods 
  //

  // init
  public void init(){
    pg = createGraphics(xSize,ySize,P2D);
    pg.beginDraw();
    pg.background(255);
    pg.noStroke();
    pg.noSmooth();
    pg.endDraw();
    
    nBalls = int(0.01*nPoints);
    balls = new Ball[nBalls];
    for(int i = 0; i < nBalls; i++){
      float velMag = 0.015*xSize;
      float velAng = random(TWO_PI);
      balls[i] = new Ball(random(xSize),random(ySize),velMag*cos(velAng),velMag*sin(velAng),ballSize,ballColor);
    }
  }
 

  // update
  public void update(int t){
    pg.beginDraw();
    pg.background(255);
    pg.endDraw();
    
    for(int i = 0; i < nBalls; i++){
      balls[i].update(pg);
      balls[i].paint(pg);
    }
    
    pg.loadPixels();
    for(int i = 0; i < nPoints; i++){
      if(pg.pixels[i] == ballColor){
        maskImg[i] = !value;
      }
      else{
        maskImg[i] = value;
      }
    }
    pg.updatePixels();

    addMask();
    calculateBackSide();
  }

}



/*
 * TracesMask class
 *
 * Subclass of the ScanMask class
 */

public class TracesMask extends ScanMask{
  private PGraphics pg;
  private int nBalls;
  private float ballSize = 5;
  private color ballColor = color(0);
  private Ball[] balls;

  
  //
  // Constructor
  //

  public TracesMask(){

  }
  

  //
  // Class Methods 
  //

  // init
  public void init(){
    pg = createGraphics(xSize,ySize,P2D);
    pg.beginDraw();
    pg.background(255);
    pg.noStroke();
    pg.noSmooth();
    pg.endDraw();

    nBalls = int(0.0003*nPoints);
    if(nBalls < 10) nBalls = 10;
    balls = new Ball[nBalls];
    for(int i = 0; i < nBalls; i++){
      float velMag = 0.004*xSize;
      float velAng = random(TWO_PI);
      balls[i] = new Ball(random(xSize),random(ySize),velMag*cos(velAng),velMag*sin(velAng),ballSize,ballColor);
    }
  }


  // update
  public void update(int t){
    for(int i = 0; i < nBalls; i++){
      balls[i].update(pg);
      balls[i].paint(pg);
    }
    
    pg.loadPixels();
    for(int i = 0; i < nPoints; i++){
      if(pg.pixels[i] == ballColor){
        maskImg[i] = !value;
      }
      else{
        maskImg[i] = value;
      }
    }
    pg.updatePixels();

    addMask();
    calculateBackSide();
  }

}



/*
 * CursorMask class
 *
 * Subclass of the ScanMask class
 */

public class CursorMask extends ScanMask{
  private float cursorSize;
  
  
  //
  // Constructor
  //

  public CursorMask(){

  }
 

  //
  // Class Methods 
  //

  // init
  public void init(){
    cursorSize = 3000/sqrt(nPoints);
  }
  
    
  // update
  public void update(int t){
    int cursorIndex = pointUnderCursor();
    if(cursorIndex >= 0){  
      PVector cursorPos = map3D[cursorIndex].get();
      for(int y = 0; y < ySize; y++){
        if(!empty[y]){
          for(int x = ini[y]; x <= end[y]; x++){
            int index = x + y*xSize; 
            if(consImgOrig[index]){
              float distSq = sq(map3D[index].x-cursorPos.x) + sq(map3D[index].y-cursorPos.y) + sq(map3D[index].z-cursorPos.z);
              if(distSq < sq(cursorSize)){
                maskImg[index] = !value;
              }
            }
          }
        }
      }
    }
    addMask();
    calculateBackSide();
  }
  
  
  // pointUnderCursor
  public int pointUnderCursor(){
    // Perspective parameters
    float fov = PI/3.0;
    float cameraZ = (height/2.0)/tan(fov/2.0);
    
    int cursorIndex = -1;
    float maxZ = -1000;
    float maxDistSq = sq(2*zoom);
    for(int y = 0; y < ySize; y++){
      if(!empty[y]){
        for(int x = ini[y]; x <= end[y]; x++){
          int index = x + y*xSize;
          if(consImgOrig[index]){
            // Scale and rotate the point as is shown in the screen
            PVector p = map3D[index].get();
            p.mult(zoom);
            p = new PVector(p.x*cos(rotY)+p.z*sin(rotY),p.y,-p.x*sin(rotY)+p.z*cos(rotY));
            p = new PVector(p.x,p.y*cos(rotX)-p.z*sin(rotX),p.y*sin(rotX)+p.z*cos(rotX));
            p.add(0,0,0);
            
            // Calculate the cursor position at the same z plane as the point
            float scaling = (cameraZ-p.z)/cameraZ;
            PVector cursorPos = new PVector((mouseX-width/2.0)*scaling,(mouseY-height/2.0)*scaling,p.z);
            
            // Find the closest point to the cursor
            float distSq = sq(p.x-cursorPos.x) + sq(p.y-cursorPos.y) + sq(p.z-cursorPos.z);
            if(distSq < maxDistSq && p.z > maxZ){
              maxZ = p.z;
              cursorIndex = index;
            }
          }
        }
      }
    }
    return cursorIndex;
  }
  
}



/*
 * DLAMask class
 *
 * Subclass of the ScanMask class
 */

public class DLAMask extends ScanMask{
  private int nPar;
  private ArrayList particles;
  private int seeds = 1;
  
  
  //
  // Constructor
  //

  public DLAMask(){

  }
  
  
  //
  // Class Methods 
  //

  // init
  public void init(){
    nPar = int(0.08*nPoints);
    particles = new ArrayList(nPar);
    for(int i = 0; i < nPar; i++){
      particles.add(new DLAparticle(int(random(xSize)),int(random(ySize)),xSize,ySize));
    }
    
    // Introduce the seeds
    for(int i = 0; i < seeds; ){
      if(i == 0){
        maskImg[centralPix[0] + centralPix[1]*xSize] = !value;
        i++;
      }
      else{
        int index = int(random(nPoints));
        if(consImgOrig[index]){
          maskImg[index] = !value;
          i++; 
        }
      }
    }
  }
 

  // update
  public void update(int t){
    int nSteps = 10;  // to speed up the process
    for(int step = 0; step < nSteps; step++){
      for(Iterator i = particles.iterator(); i.hasNext(); ){
        DLAparticle par = (DLAparticle) i.next();
        // Move the particle in a random walk
        par.update();
        // Check if the particle needs to be aggregated
        int[] parIndexes = par.checkForAggregation(maskImg,!value);
        int x = parIndexes[0];
        int y = parIndexes[1];
        // Aggregate if needed
        if(x >= 0 && x < xSize && y >= 0 && y < ySize){
          int index = x + y*xSize;
          maskImg[index] = !value;
          if(x+1 < xSize) maskImg[index+1] = !value;
          if(x-1 >= 0) maskImg[index-1] = !value;
          if(y+1 < ySize) maskImg[index+xSize] = !value;
          if(y-1 >= 0) maskImg[index-xSize] = !value;
          i.remove();
        }
      }
    }
    addMask();
    calculateBackSide();
  }

}

/*
 * ScanMask class
 *
 * Subclass of the ScanContainer class
 * Adds the posibility to use a mask to control the data that is shown
 */

public class ScanMask extends ScanContainer{
  public boolean[] consImgOrig; // Copy of the original consImg array
  public boolean value;         // Value used to initialize the mask
  public boolean[] maskImg;     // Mask with the valid points 
    
  
  //
  // Constructor
  //

  public ScanMask(){

  }
  
  
  //
  // Class Methods 
  //

  // create
  public void create(ScanContainer tempScan, boolean tempValue){
    initialize(tempScan.map3D,tempScan.rgbImg,tempScan.consImg);
    findExtremes();
    findCentralPixel();
    calculateNormals();
    copyOriginal();
    initializeMask(tempValue);
    init();
    addMask();
    calculateBackSide();
  }


  // createFromFile
  public void createFromFile(String tempFile, boolean tempValue){
    initialize(tempFile);
    findExtremes();
    findCentralPixel();
    calculateNormals();
    copyOriginal();
    initializeMask(tempValue);
    init();
    addMask();
    calculateBackSide();
  }


  // copyOriginal
  public void copyOriginal(){
    consImgOrig = new boolean[nPoints];
    for(int i = 0; i < nPoints; i++){
      consImgOrig[i] = consImg[i];
    }
  }


  // restoreOriginal
  public void restoreOriginal(){
    for(int i = 0; i < nPoints; i++){
      consImg[i] = consImgOrig[i];
    }
  }


  // initializeMask
  public void initializeMask(boolean val){
    value = val;
    maskImg = new boolean[nPoints];
    for(int i = 0; i < nPoints; i++){
      maskImg[i] = value;
    }
  }


  // init
  public void init(){

  }


  // addMask
  public void addMask(){
    for(int y = 0; y < ySize; y++){
      if(!empty[y]){
        for(int x = ini[y]; x <= end[y]; x++){
          int index = x + y*xSize;
          consImg[index] = consImgOrig[index] && maskImg[index];
        }
      }
    }
  }


  // update
  public void update(int t){

  }


  // reduceResolution
  public void reduceResolution(int n){
    if(n > 1){
      restoreOriginal();
      super.reduceResolution(n);
      copyOriginal();
      initializeMask(value);
      init();
      addMask();
      calculateBackSide();
    }
  }
  

  // centerAndExtend
  public void centerAndExtend(){
    restoreOriginal();
    super.centerAndExtend();
    copyOriginal();
    initializeMask(value);
    init();
    addMask();
    calculateBackSide();
  }


  // extend
  public void extend(int xSizeExt, int ySizeExt){
    restoreOriginal();
    super.extend(xSizeExt,ySizeExt);
    copyOriginal();
    initializeMask(value);
    init();
    addMask();
    calculateBackSide();
  }


  // scaleFactor
  public void scaleFactor(float f){
    restoreOriginal();
    super.scaleFactor(f);
    copyOriginal();
    initializeMask(value);
    init();
    addMask();
    calculateBackSide();
  }

  
  // fillHoles
  public void fillHoles(int maxHoles){
    restoreOriginal();
    super.fillHoles(maxHoles);
    copyOriginal();
    initializeMask(value);
    init();
    addMask();
    calculateBackSide();
  }


  // gaussianSmooth
  public void gaussianSmooth(int n){
    if(n > 0){
      restoreOriginal();
      super.gaussianSmooth(n);
      copyOriginal();
      initializeMask(value);
      init();
      addMask();
      calculateBackSide();
    }
  }
  
  
  // centerOnCursor
  public void centerOnCursor(){
    restoreOriginal();
    super.centerOnCursor();
    copyOriginal();
    initializeMask(value);
    init();
    addMask();
    calculateBackSide();
  }


  // constrainPoints
  public void constrainPoints(float xMin, float xMax, float yMin, float yMax, float zMin, float zMax){
    restoreOriginal();
    super.constrainPoints(xMin,xMax,yMin,yMax,zMin,zMax);
    copyOriginal();
    initializeMask(value);
    init();
    addMask();
    calculateBackSide();
  }
  
  
  // crop
  public void crop(){
    restoreOriginal();
    super.crop();
    copyOriginal();
    initializeMask(value);
    init();
    addMask();
    calculateBackSide();
  }

}

/*
 * ScanModifier class
 *
 * Subclass of the ScanContainer class
 * Adds the posibility to modify the data from the scan
 */
 
public class ScanModifier extends ScanContainer{
  public PVector[] map3DOrig;   // Copy of the original map3D array
  public PImage rgbImgOrig;     // Copy of the original rgbImg array 
  public boolean[] consImgOrig; // Copy of the original consImg array 
  public PVector[] normalsOrig; // Copy of the original normals array 
  public PVector[] back3DOrig;  // Copy of the original back3D array 


  //
  // Constructor
  //

  public ScanModifier(){

  }
  

  //
  // Class Methods 
  //

  // create
  public void create(ScanContainer tempScan){
    initialize(tempScan.map3D,tempScan.rgbImg,tempScan.consImg);
    findExtremes();
    findCentralPixel();
    calculateNormals();
    calculateBackSide();
    copyOriginal();
    init();
  }


  // createFromFile
  public void createFromFile(String tempFile){
    initialize(tempFile);
    findExtremes();
    findCentralPixel();
    calculateNormals();
    calculateBackSide();
    copyOriginal();
    init();
  }


  // copyOriginal
  public void copyOriginal(){
    map3DOrig = new PVector[nPoints];
    rgbImgOrig = createImage(xSize,ySize,RGB);
    consImgOrig = new boolean[nPoints];
    normalsOrig = new PVector[nPoints];
    back3DOrig = new PVector[nPoints];
    
    rgbImgOrig.loadPixels();
    rgbImg.loadPixels();
    for(int i = 0; i < nPoints; i++){
      map3DOrig[i] = map3D[i].get();
      rgbImgOrig.pixels[i] = rgbImg.pixels[i];
      consImgOrig[i] = consImg[i];
      normalsOrig[i] = normals[i].get();
      back3DOrig[i] = back3D[i].get();
    }
    rgbImgOrig.updatePixels();
  }


  // restoreOriginal
  public void restoreOriginal(){
    rgbImgOrig.loadPixels();
    rgbImg.loadPixels();
    for(int i = 0; i < nPoints; i++){
      map3D[i] = map3DOrig[i].get();
      rgbImg.pixels[i] = rgbImgOrig.pixels[i];
      consImg[i] = consImgOrig[i];
      normals[i] = normalsOrig[i].get();
      back3D[i] = back3DOrig[i].get();
    }
    rgbImg.updatePixels();
  }


  // init
  public void init(){
  
  }


  // update
  public void update(int t){
  
  }


  // reduceResolution
  public void reduceResolution(int n){
    if(n > 1){
      restoreOriginal();
      super.reduceResolution(n);
      copyOriginal();
      init();
      calculateBackSide();
    }
  }

  
  // centerAndExtend
  public void centerAndExtend(){
    restoreOriginal();
    super.centerAndExtend();
    copyOriginal();
    init();
    calculateBackSide();
  }


  // extend
  public void extend(int xSizeExt, int ySizeExt){
    restoreOriginal();
    super.extend(xSizeExt,ySizeExt);
    copyOriginal();
    init();
    calculateBackSide();
  }


  // scaleFactor
  public void scaleFactor(float f){
    restoreOriginal();
    super.scaleFactor(f);
    copyOriginal();
    init();
    calculateBackSide();
  }


  // fillHoles
  public void fillHoles(int maxHoles){
    restoreOriginal();
    super.fillHoles(maxHoles);
    copyOriginal();
    init();
    calculateBackSide();
  }


  // gaussianSmooth
  public void gaussianSmooth(int n){
    if(n > 0){
      restoreOriginal();
      super.gaussianSmooth(n);
      copyOriginal();
      init();
      calculateBackSide();
    }
  }
  
  
  // centerOnCursor
  public void centerOnCursor(){
    restoreOriginal();
    super.centerOnCursor();
    copyOriginal();
    init();
    calculateBackSide();
  }


  // constrainPoints
  public void constrainPoints(float xMin, float xMax, float yMin, float yMax, float zMin, float zMax){
    restoreOriginal();
    super.constrainPoints(xMin,xMax,yMin,yMax,zMin,zMax);
    copyOriginal();
    init();
    calculateBackSide();
  }
  
  
  // crop
  public void crop(){
    restoreOriginal();
    super.crop();
    copyOriginal();
    init();
    calculateBackSide();
  }

}

/*
 * Ball class
 */

public class Ball{
  public float x;     // x position
  public float y;     // y position
  public float vx;    // velocity in x direction
  public float vy;    // velocity in y direction
  public float diam;  // ball diameter
  public color col;   // ball color


  //
  // Constructor
  //
  
  public Ball(float tempX, float tempY, float tempVx, float tempVy, float tempDiam, color tempCol){
    x = tempX;
    y = tempY;
    vx = tempVx;
    vy = tempVy;
    diam = tempDiam;
    col = tempCol;
  }
  
  
  //
  // Class Methods 
  //

  // update
  public void update(PGraphics g){
    x += vx;
    y += vy; 
    
    if(x < 0){
      x = 0;
      vx = -vx;
    }
    else if(x > g.width){
      x = g.width;
      vx = -vx;
    }
    if(y < 0){
      y = 0;
      vy = -vy;
    }
    else if(y > g.height){
      y = g.height;
      vy = -vy;
    }
  }
  
  
  // paint
  public void paint(PGraphics g){
    g.beginDraw();
    g.fill(col);
    g.ellipse(x,y,diam,diam);
    g.endDraw();
  }

}

/*
 * ControlP5 selection panel
 */

void controlPanel(){
  cp5 = new ControlP5(this);
  cp5.setAutoDraw(false);

  // Scan selector
  scanButtons = cp5.addRadioButton("scanSelector")
                   .setPosition(40,40)
                   .setSize(20,20)
                   .setItemsPerRow(10)
                   .setSpacingColumn(10)
                   .setSpacingRow(10)
                   .setNoneSelectedAllowed(false);

  for(int i = 0; i < scans.length; i++){
    scanButtons.addItem("scan"+i,i);
  }
  
  scanButtons.hideLabels();
  scanButtons.activate(currentScan);

  
  // ScanMask selector
  maskButtons = cp5.addRadioButton("maskSelector")
                   .setPosition(2*40+(scans.length)*(20+10),40)
                   .setSize(20,20)
                   .setItemsPerRow(10)
                   .setSpacingColumn(10)
                   .setSpacingRow(10)
                   .setNoneSelectedAllowed(true);
  
  for(int i = 0; i < maskTypes.length; i++){
    maskButtons.addItem(maskTypes[i],i);
  }
  
  maskButtons.hideLabels();
  

  // ScanModifier selector
  modifierButtons = cp5.addRadioButton("modifierSelector")
                       .setPosition(3*40+(scans.length+maskTypes.length)*(20+10),40)
                       .setSize(20,20)
                       .setItemsPerRow(10)
                       .setSpacingColumn(10)
                       .setSpacingRow(10)
                       .setNoneSelectedAllowed(true);
  
  for(int i = 0; i < modifierTypes.length; i++){
    modifierButtons.addItem(modifierTypes[i],i);
  }
  
  modifierButtons.hideLabels();
  
  
  // Morphing toggle
  morphingToggle = cp5.addToggle("morphing",drawMorphing)
                      .setPosition(4*40+(scans.length+maskTypes.length+modifierTypes.length)*(20+10),40)
                      .setSize(20,20)
                      .setLabel("");
}



// Manage events
void controlEvent(ControlEvent event){
  if(event.isFrom(scanButtons)){
    if(event.getValue() >= 0){
      currentScan = int(event.getValue());
      
      if(drawMask || drawMorphing) sMask = createMask(scans[currentScan],maskType);
      else if(drawModifier) sModifier = createModifier(scans[currentScan],modifierType);
      
      resolution = initResolution;
      counter = 0;
      if(drawMorphing){
        morphingToggle.setValue(false);
        drawMask = true;
        drawModifier = false;
        drawMorphing = false;
      }
    }
  }
 
  if(event.isFrom(maskButtons)){
    if(int(event.getValue()) >= 0){
      maskType = maskTypes[int(event.getValue())];
      sMask = createMask(scans[currentScan],maskType);
      resolution = initResolution;
      counter = 0;
      drawMask = true;
      drawModifier = false;
      drawMorphing = false;
      
      modifierButtons.deactivateAll();
      morphingToggle.setState(false);
    }
    else if(drawMask){
      maskType = "empty";
      sMask = createMask(scans[currentScan],maskType);
      resolution = initResolution;
      counter = 0;
      drawMask = true;
      drawModifier = false;
      drawMorphing = false;
    }
  }
  
  if(event.isFrom(modifierButtons)){
    if(int(event.getValue()) >= 0){
      modifierType = modifierTypes[int(event.getValue())];
      sModifier = createModifier(scans[currentScan],modifierType);
      resolution = initResolution;
      counter = 0;
      drawMask = false;
      drawModifier = true;
      drawMorphing = false;
      
      maskButtons.deactivateAll();
      morphingToggle.setState(false);
    }
    else if(drawModifier){
      maskType = "empty";
      sMask = createMask(scans[currentScan],maskType);
      resolution = initResolution;
      counter = 0;
      drawMask = true;
      drawModifier = false;
      drawMorphing = false;
    }
  }

  if(event.isFrom(morphingToggle)){
    if(morphingToggle.getState()){
      if(scans.length > 1){
        morphingIterator = 1;
        sConnector = new ScanConnector(scans[morphingIterator-1],scans[morphingIterator]);
        resolution = initResolution;
        counter = 0;
        drawMask = false;
        drawModifier = false;
        drawMorphing = true;
        
        maskButtons.deactivateAll();
        modifierButtons.deactivateAll();        
      }
    }
    else if(drawMorphing){
      maskType = "empty";
      sMask = createMask(scans[currentScan],maskType);
      resolution = initResolution;
      counter = 0;
      drawMask = true;
      drawModifier = false;
      drawMorphing = false;
    }
  }

}

/*
 * DLAparticle class
 *
 * Difussion-Limited Aggregation particle
 */

public class DLAparticle{
  public int x;               // x position
  public int y;               // y position
  public int w;               // region's width
  public int h;               // region's height
  public int xPrev;           // previous x position
  public int yPrev;           // previous y position
  public boolean aggregated;  // controls if it's already aggregated


  //
  // Constructor
  //

  public DLAparticle(int tempX, int tempY, int tempW, int tempH){
    x = tempX;
    y = tempY;
    w = tempW;
    h = tempH;
    xPrev = x;
    yPrev = y;
    aggregated = false;
  }


  //
  // Class Methods 
  //

  // update
  public void update(){
    switch(int(random(8))){
      case 0: 
        if(x + 1 < w){
          xPrev = x;
          x++;
        }
        break;
      case 1: 
        if(x + 1 < w && y + 1 < h){
          xPrev = x;
          yPrev = y;
          x++;
          y++;
        }
        break;
      case 2:  
        if(y + 1 < h){
          yPrev = y;
          y++;
        }
        break;
      case 3:
        if(x - 1 >= 0 && y + 1 < h){
          xPrev = x;
          yPrev = y;
          x--;
          y++;
        }
        break;
      case 4:  
        if(x - 1 >= 0){
          xPrev = x;
          x--;
        }
        break;
      case 5: 
        if(x - 1 >= 0 && y - 1 >= 0){
          xPrev = x;
          yPrev = y;
          x--;
          y--;
        }
        break;
      case 6:  
        if(y - 1 >= 0){
          yPrev = y;
          y--;
        }
        break;
      case 7: 
        if(x + 1 < w && y - 1 >= 0){
          xPrev = x;
          yPrev = y;
          x++;
          y--;
        }
        break;
    }
  }

  
  // checkForAggregation
  public int[] checkForAggregation(boolean[] m, boolean val){
    int[] aggregationPos = {-1,-1};
    if(!aggregated && m[x + y*w] == val){
      aggregated = true;
      aggregationPos[0] = xPrev;
      aggregationPos[1] = yPrev;
    }
    return aggregationPos;
  }

}

/*
 * ScanContainer class
 *
 * Class to read and manipulate the scans produced with the kinect scanner
 */

public class ScanContainer{
  public int xSize;                // Hozizontal dimension
  public int ySize;                // Vertical dimension
  public int nPoints;              // Number of points
  public PVector[] map3D;          // 3D points
  public PImage rgbImg;            // Point colors 
  public boolean[] consImg;        // Valid points 
  public PVector[] normals;        // Point normals
  public PVector[] back3D;         // 3D points from the back side
  public int[] ini;                // Start of the valid points (in the line)
  public int[] end;                // End of the valid points (in the line)
  public boolean[] empty;          // No valid points in the line
  public int[] centralPix;         // Central pixel (x,y,z) = (0,0,z) in pixel coordinates
  public PVector centralPos;       // Central position of the scan in 3D world coordinates
  public float maxSepSq = sq(120); // Maximum separation allowed between two consecutive points
   
  
  //
  // Constructor
  //
  
  public ScanContainer(){

  }
  
  
  //
  // Class Methods 
  //

  // create
  public void create(ScanContainer tempScan){
    initialize(tempScan.map3D,tempScan.rgbImg,tempScan.consImg);
    findExtremes();
    findCentralPixel();
    calculateNormals();
    calculateBackSide();
  }


  // createFromFile
  public void createFromFile(String tempFile){
    initialize(tempFile);
    findExtremes();
    findCentralPixel();
    calculateNormals();
    calculateBackSide();
  }


  // initialize
  public void initialize(PVector[] points3D, PImage img, boolean[] cons){
    xSize = img.width;
    ySize = img.height;
    nPoints = xSize*ySize;
    map3D = new PVector[nPoints];
    rgbImg = createImage(xSize,ySize,RGB); 
    consImg = new boolean[nPoints];
    normals = new PVector[nPoints];
    back3D = new PVector[nPoints];
    ini = new int[ySize];
    end = new int[ySize];
    empty = new boolean[ySize];
    centralPix = new int[2];
    centralPos = new PVector();
    
    // Populate the arrays
    rgbImg.loadPixels();
    img.loadPixels();
    for(int i = 0; i < nPoints; i++){
      map3D[i] = points3D[i].get();
      rgbImg.pixels[i] = img.pixels[i];
      consImg[i] = cons[i];
      normals[i] = new PVector();
      back3D[i] = new PVector();
    }
    rgbImg.updatePixels();
  
    for(int i = 0; i < ySize; i++){
      ini[i] = 0;
      end[i] = xSize - 1;
      empty[i] = false;
    }
    
    centralPix[0] = int(xSize/2);
    centralPix[1] = int(ySize/2);
    if(consImg[centralPix[0]+centralPix[1]*xSize]){
      centralPos = map3D[centralPix[0]+centralPix[1]*xSize].get();
    }
  }
  
  
  // initialize
  public void initialize(String file){
    String[] pointsAndColors = loadStrings(file);
    String[] header = split(pointsAndColors[0]," ");

    xSize = int(header[0]);
    ySize = int(header[1]);
    nPoints = xSize*ySize;
    map3D = new PVector[nPoints];
    rgbImg = createImage(xSize,ySize,RGB); 
    consImg = new boolean[nPoints]; 
    normals = new PVector[nPoints];
    back3D = new PVector[nPoints];
    ini = new int[ySize];
    end = new int[ySize];
    empty = new boolean[ySize];
    centralPix = new int[2];
    centralPos = new PVector();
  
    // Populate the arrays
    rgbImg.loadPixels();
    for(int i = 0; i < nPoints; i++){
      String[] row = split(pointsAndColors[i+1]," ");
      map3D[i] = new PVector(float(row[0]),float(row[1]),float(row[2]));
      if(float(row[3]) < 0){
        rgbImg.pixels[i] = color(0);
        consImg[i] = false;
      }
      else{
        rgbImg.pixels[i] = color(float(row[3]),float(row[4]),float(row[5]));
        consImg[i] = true;
      }
      normals[i] = new PVector();
      back3D[i] = new PVector();
    }
    rgbImg.updatePixels();
  
    for(int i = 0; i < ySize; i++){
      ini[i] = 0;
      end[i] = xSize -1;
      empty[i] = false;
    }
    
    centralPix[0] = int(xSize/2);
    centralPix[1] = int(ySize/2);
    if(consImg[centralPix[0]+centralPix[1]*xSize]){
      centralPos = map3D[centralPix[0]+centralPix[1]*xSize].get();
    }
  }


  // findExtremes
  public void findExtremes(){
    for(int y = 0; y < ySize; y++){
      boolean started = false;
      for(int x = 0; x < xSize; x++){
        if(consImg[x + y*xSize]){
          if(!started){
            ini[y] = x;
            empty[y] = false;
            started = true;
          }
          end[y] = x;
        }
      }
      if(!started){
        empty[y] = true;
      }
    }
  }  


  // findCentralPixel
  public void findCentralPixel(){
    float minDistSq = 1000;
    for(int y = 0; y < ySize; y++){
      if(!empty[y]){
        for(int x = ini[y]; x <= end[y]; x++){
          int index = x + y*xSize;
          if(consImg[index]){
            float distSq = sq(map3D[index].x)+sq(map3D[index].y);
            if(distSq < minDistSq){
              minDistSq = distSq;
              centralPix[0] = x;
              centralPix[1] = y;
              centralPos = map3D[index].get();
            }
          }       
        }
      }
    }
  }


  // findCentralPixel
  public void findCentralPixel(PVector cen){
    float minDistSq = 1000;
    for(int y = 0; y < ySize; y++){
      if(!empty[y]){
        for(int x = ini[y]; x <= end[y]; x++){
          int index = x + y*xSize;
          if(consImg[index]){
            float distSq = sq(map3D[index].x-cen.x)+sq(map3D[index].y-cen.y)+sq(map3D[index].z-cen.z);
            if(distSq < minDistSq){
              minDistSq = distSq;
              centralPix[0] = x;
              centralPix[1] = y;
              centralPos = map3D[index].get();
            }
          }       
        }
      }
    }
  }


  // calculateNormals
  public void calculateNormals(){
    for(int y = 0; y < ySize; y++){
      if(!empty[y]){
        for(int x = ini[y]; x <= end[y]; x++){
          int index = x + y*xSize;
          if(consImg[index]){
            int n = 0;
            PVector averageNormal = new PVector();
            if(x+1 < xSize && y+1 < ySize && consImg[index+1] && consImg[index+xSize]){
              PVector v1 = PVector.sub(map3D[index+1],map3D[index]);
              PVector v2 = PVector.sub(map3D[index+xSize],map3D[index]);
              PVector perp = v1.cross(v2);
              perp.normalize();
              averageNormal.add(perp);
              n++;
            }
            if(x-1 >= 0 && y+1 < ySize && consImg[index-1] && consImg[index+xSize]){
              PVector v1 = PVector.sub(map3D[index+xSize],map3D[index]);
              PVector v2 = PVector.sub(map3D[index-1],map3D[index]);
              PVector perp = v1.cross(v2);
              perp.normalize();
              averageNormal.add(perp);
              n++;
            }
            if(x-1 >= 0 && y-1 >= 0 && consImg[index-1] && consImg[index-xSize]){
              PVector v1 = PVector.sub(map3D[index-1],map3D[index]);
              PVector v2 = PVector.sub(map3D[index-xSize],map3D[index]);
              PVector perp = v1.cross(v2);
              perp.normalize();
              averageNormal.add(perp);
              n++;
            }
            if(x+1 < xSize && y-1 >= 0 && consImg[index+1] && consImg[index-xSize]){
              PVector v1 = PVector.sub(map3D[index-xSize],map3D[index]);
              PVector v2 = PVector.sub(map3D[index+1],map3D[index]);
              PVector perp = v1.cross(v2);
              perp.normalize();
              averageNormal.add(perp);
              n++;
            }
            if(n > 0){
              normals[index] = PVector.div(averageNormal,n);
            }
            else{
              normals[index] = new PVector();
            }
          }
          else{
            normals[index] = new PVector();
          }
        }
      }
    }
  }
  
  
  // calculateBackSide
  public void calculateBackSide(){
    for(int y = 0; y < ySize; y++){
      if(!empty[y]){
        for(int x = ini[y]; x <= end[y]; x++){
          int index = x + y*xSize;
          if(consImg[index]){
            boolean cond = x-1 >= 0 && x+1 < xSize && y-1 >= 0 && y+1 < ySize &&
                           consImg[index-1] && consImg[index+1] && consImg[index-xSize] && consImg[index+xSize];
            if(cond){
              back3D[index] = PVector.sub(map3D[index],PVector.mult(normals[index],0.1));
            }
            else{
              back3D[index] = PVector.sub(map3D[index],PVector.mult(normals[index],0.01));
            }
          }
          else{
            back3D[index] = map3D[index].get();
          }
        }
      }
    }
  }
  
  
  // reduceResolution
  public void reduceResolution(int n){
    if(n > 1){
      // Dimensions of the reduced scan
      int xSizeRed = xSize/n + 2;
      int ySizeRed = ySize/n + 2;
      int nPointsRed = xSizeRed*ySizeRed;
      PVector[] redMap3D = new PVector[nPointsRed];
      PImage redRGBimg = createImage(xSizeRed,ySizeRed,RGB);
      boolean[] redConsImg = new boolean[nPointsRed]; 

      // Populate the arrays
      redRGBimg.loadPixels();
      rgbImg.loadPixels();
      for(int y = 0; y < ySizeRed; y++){
        for(int x = 0; x < xSizeRed; x++){
          int index = x*n + y*n*xSize;
          int indexRed = x + y*xSizeRed;
          
          // Average between nearby pixels
          PVector p = new PVector();
          float r = 0;
          float g = 0;
          float b = 0;
          float pointsCounter = 0;
          for(int i = -n/2; i <= n/2; i++){
            for(int j = -n/2; j <= n/2; j++){
              int xNearby = (x*n)+i;
              int yNearby = (y*n)+j;
              if(xNearby >=0 && xNearby < xSize && yNearby >= 0 && yNearby < ySize){
                int indexNearby = xNearby + yNearby*xSize;
                if(consImg[indexNearby]){
                  p.add(map3D[indexNearby]);
                  r += red(rgbImg.pixels[indexNearby]);
                  g += green(rgbImg.pixels[indexNearby]);
                  b += blue(rgbImg.pixels[indexNearby]);
                  pointsCounter++;
                }
              }
            }
          }
          if(pointsCounter > 0){
            redMap3D[indexRed] = PVector.div(p,pointsCounter);
            redRGBimg.pixels[indexRed] = color(r/pointsCounter,g/pointsCounter,b/pointsCounter);
            redConsImg[indexRed] = true;
          }
          else{
            redMap3D[indexRed] = new PVector();
            redRGBimg.pixels[indexRed] = color(0);
            redConsImg[indexRed] = false;
          }
        }
      }
      redRGBimg.updatePixels();
    
      // Update the scan to the new resolution
      initialize(redMap3D,redRGBimg,redConsImg);
      findExtremes();
      findCentralPixel();
      calculateNormals();
      calculateBackSide();
    }
  }


  // centerAndExtend
  public void centerAndExtend(){
    // Dimensions of the extended scan
    int xSizeExt = 2*max(centralPix[0]+1,xSize-centralPix[0]) - 1;
    int ySizeExt = 2*max(centralPix[1]+1,ySize-centralPix[1]) - 1;
    int nPointsExt = xSizeExt*ySizeExt;
    PVector[] extMap3D = new PVector[nPointsExt];
    PImage extRGBimg = createImage(xSizeExt,ySizeExt,RGB);
    boolean[] extConsImg = new boolean[nPointsExt]; 

    // Populate the arrays
    extRGBimg.loadPixels();
    for(int i = 0; i < nPointsExt; i++){
      extMap3D[i] = new PVector();
      extRGBimg.pixels[i] = color(0);
      extConsImg[i] = false;
    }
    extRGBimg.updatePixels();
    
    int xStart, yStart;
    if((centralPix[0]+1) > (xSize-centralPix[0])) xStart = 0;
    else xStart = xSizeExt - xSize;
    if((centralPix[1]+1) > (ySize-centralPix[1])) yStart = 0;
    else yStart = ySizeExt - ySize;
        
    extRGBimg.loadPixels();
    rgbImg.loadPixels();
    for(int y = 0; y < ySize; y++){
      if(!empty[y]){
        for(int x = ini[y]; x <= end[y]; x++){
          int index = x + y*xSize;
          int indexExt = (xStart+x) + (yStart+y)*xSizeExt;
          extMap3D[indexExt] = map3D[index].get();
          extRGBimg.pixels[indexExt] = rgbImg.pixels[index];
          extConsImg[indexExt] = consImg[index];
        }
      }
    }
    extRGBimg.updatePixels();

    // Update the scan to the new resolution
    initialize(extMap3D,extRGBimg,extConsImg);
    findExtremes();
    findCentralPixel();
    calculateNormals();
    calculateBackSide();
  }

  
  // extend
  public void extend(int xSizeExt, int ySizeExt){
    if(xSizeExt >= xSize && ySizeExt >= ySize){
      // Dimensions of the extended scan
      int nPointsExt = xSizeExt*ySizeExt;
      PVector[] extMap3D = new PVector[nPointsExt];
      PImage extRGBimg = createImage(xSizeExt,ySizeExt,RGB);
      boolean[] extConsImg = new boolean[nPointsExt];
    
      // Populate the arrays
      extRGBimg.loadPixels();
      for(int i = 0; i < nPointsExt; i++){
        extMap3D[i] = new PVector();
        extRGBimg.pixels[i] = color(0);
        extConsImg[i] = false; 
      }
      extRGBimg.updatePixels();
 
      int xStart = (xSizeExt - xSize)/2;
      int yStart = (ySizeExt - ySize)/2;
 
      extRGBimg.loadPixels();
      rgbImg.loadPixels();
      for(int y = 0; y < ySize; y++){
        if(!empty[y]){
          for(int x = ini[y]; x <= end[y]; x++){
            int index = x + y*xSize;
            int indexExt = (xStart+x) + (yStart+y)*xSizeExt;
            extMap3D[indexExt] = map3D[index].get();
            extRGBimg.pixels[indexExt] = rgbImg.pixels[index];
            extConsImg[indexExt] = consImg[index]; 
          }
        }
      }
      extRGBimg.updatePixels();
    
      // Update the scan to the new resolution
      initialize(extMap3D,extRGBimg,extConsImg);
      findExtremes();
      findCentralPixel();
      calculateNormals();
      calculateBackSide();
    }
  }
 

  // scaleFactor
  public void scaleFactor(float f){
    for(int y = 0; y < ySize; y++){
      if(!empty[y]){
        for(int x = ini[y]; x <= end[y]; x++){
          map3D[x + y*xSize].mult(f);
        }
      }
    }
    calculateBackSide();
  }

 
  // fillHoles
  public void fillHoles(int maxHoles){
    rgbImg.loadPixels();
    for(int y = 0; y < ySize; y++){
      if(!empty[y]){
        // Find holes in the line
        for(int x = ini[y]+1; x < end[y]; x++){
          if(!consImg[x + y*xSize]){
            // Calculate the limits of the hole
            int start = (x-1) + y*xSize;
            int finish = start;
            for(int i = x+1; i <= end[y]; i++){
              if(consImg[i + y*xSize]){
                finish = i + y*xSize;
                x = i; // the x loop will continue from here
                break;
              }
            }
            // Fill the hole if the gap is not too big
            if(((finish-start)-1) <= maxHoles){
              PVector deltaPos = PVector.sub(map3D[finish],map3D[start]);
              float deltaR = red(rgbImg.pixels[finish])-red(rgbImg.pixels[start]);
              float deltaG = green(rgbImg.pixels[finish])-green(rgbImg.pixels[start]);
              float deltaB = blue(rgbImg.pixels[finish])-blue(rgbImg.pixels[start]);
              for(int i = start+1; i < finish; i++){
                float step = float(i-start)/float(finish-start);
                map3D[i] = PVector.add(map3D[start],PVector.mult(deltaPos,step));
                rgbImg.pixels[i] = color(red(rgbImg.pixels[start])+deltaR*step,green(rgbImg.pixels[start])+deltaG*step,blue(rgbImg.pixels[start])+deltaB*step);
                consImg[i] = true;
              }
            }
          } 
        }    
      }
    }
    rgbImg.updatePixels();
    
    findCentralPixel();
    calculateNormals();
    calculateBackSide();
  }


  // gaussianSmooth
  public void gaussianSmooth(int n){
    if(n > 0){
      // Create the gaussian mask
      float[][] m = new float[2*n+1][2*n+1];
      for(int i = -n; i <= n; i++){
        for(int j = -n; j <= n; j++){
          float distSq = sq(i)+sq(j);
          if(distSq <= sq(n)){
            m[i+n][j+n] = pow(2.718,-distSq/(2*sq(n/2)));
          }
          else{
            m[i+n][j+n] = 0;
          }
        }
      }

      // Create the array for the smoothed 3Dpoints
      PVector[] smMap3D = new PVector[nPoints];

      // Populate the array
      for(int y = 0; y < ySize; y++){
        for(int x = 0; x < xSize; x++){
          int index = x + y*xSize;
          if(consImg[index]){
            // Average between nearby pixels
            PVector pCentral = map3D[index].get();
            PVector pAverage = new PVector();
            float multCounter = 0;
            for(int i = -n; i <= n; i++){
              for(int j = -n; j <= n; j++){
                if(x+i >=0 && x+i < xSize && y+j >= 0 && y+j < ySize){
                  int indexNearby = (x+i) + (y+j)*xSize;
                  if(consImg[indexNearby]){
                    PVector pNearby = map3D[indexNearby].get();
                    float distSq = sq(pCentral.x-pNearby.x) + sq(pCentral.y-pNearby.y) + sq(pCentral.z-pNearby.z);
                    if(distSq < maxSepSq){
                      pAverage.add(PVector.mult(pNearby,m[i+n][j+n]));
                      multCounter += m[i+n][j+n];
                    }
                  }
                }
              }
            }
            if(multCounter > 0){
              smMap3D[index] = PVector.div(pAverage,multCounter);
            }
            else{
              smMap3D[index] = map3D[index].get();
            }
          }
          else{
            smMap3D[index] = new PVector();
          }
        }
      }
      
      // Update the scan with the new points
      initialize(smMap3D,rgbImg,consImg);
      findExtremes();
      findCentralPixel();
      calculateNormals();
      calculateBackSide();
    }
  }


  // pointUnderCursor
  public int pointUnderCursor(){
    // Perspective parameters
    float fov = PI/3.0;
    float cameraZ = (height/2.0)/tan(fov/2.0);
    
    int cursorIndex = -1;
    float maxZ = -1000;
    float maxDistSq = sq(2*zoom);
    for(int y = 0; y < ySize; y++){
      if(!empty[y]){
        for(int x = ini[y]; x <= end[y]; x++){
          int index = x + y*xSize;
          if(consImg[index]){
            // Scale and rotate the point as is shown in the screen
            PVector p = map3D[index].get();
            p.mult(zoom);
            p = new PVector(p.x*cos(rotY)+p.z*sin(rotY),p.y,-p.x*sin(rotY)+p.z*cos(rotY));
            p = new PVector(p.x,p.y*cos(rotX)-p.z*sin(rotX),p.y*sin(rotX)+p.z*cos(rotX));
            p.add(0,0,0);
            
            // Calculate the cursor position at the same z plane as the point
            float scaling = (cameraZ-p.z)/cameraZ;
            PVector cursorPos = new PVector((mouseX-width/2.0)*scaling,(mouseY-height/2.0)*scaling,p.z);
            
            // Find the closest point to the cursor
            float distSq = sq(p.x-cursorPos.x) + sq(p.y-cursorPos.y) + sq(p.z-cursorPos.z);
            if(distSq < maxDistSq && p.z > maxZ){
              maxZ = p.z;
              cursorIndex = index;
            }
          }
        }
      }
    }
    return cursorIndex;
  }


  // pointCoordinates
  public void pointCoordinates(){
    int curs = pointUnderCursor();
    if(curs >= 0){
      println(map3D[curs]);
    }
  }


  // centerOnCursor
  public void centerOnCursor(){
    int curs = pointUnderCursor();
    if(curs >= 0){
      PVector newCenter = map3D[curs].get();
      newCenter.add(0,0,100); // small offset
      for(int y = 0; y < ySize; y++){
        if(!empty[y]){
          for(int x = ini[y]; x <= end[y]; x++){
            int index = x + y*xSize;
            if(consImg[index]){
              map3D[index].sub(newCenter);
            }
          }
        }
      }
      findCentralPixel();
      calculateBackSide();
    }
  }
 

  // constrainPoints
  public void constrainPoints(float xMin, float xMax, float yMin, float yMax, float zMin, float zMax){
    for(int y = 0; y < ySize; y++){
      if(!empty[y]){
        for(int x = ini[y]; x <= end[y]; x++){
          int index = x + y*xSize;
          if(consImg[index]){
            PVector p = map3D[index].get();
            if(p.x < xMin || p.x > xMax || p.y < yMin || p.y > yMax || p.z < zMin || p.z > zMax){
              consImg[index] = false;
            }
          }
        }
      }
    }
    findExtremes();
    findCentralPixel();
    calculateNormals();
    calculateBackSide();
  }

  
  // crop
  public void crop(){
    // Calculate the region in the scan with the valid data
    int xIni = xSize;
    int xEnd = -1;
    int yIni = ySize;
    int yEnd = -1;

    for(int y = 0; y < ySize; y++){
      if(!empty[y]){
        if(ini[y] < xIni) xIni = ini[y];
        if(end[y] > xEnd) xEnd = end[y];
        if(y < yIni) yIni = y;
        if(y > yEnd) yEnd = y;
      }
    }

    if(xIni <= xEnd && yIni <= yEnd){
      // 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 indexCropped = x + y*xSizeCropped;
          int index = (xIni+x) + (yIni+y)*xSize;
          croppedMap3D[indexCropped] = map3D[index].get();
          croppedRGBimg.pixels[indexCropped] = rgbImg.pixels[index];
          croppedConsImg[indexCropped] = consImg[index];
        }
      }
      croppedRGBimg.updatePixels();

      // Update the scan to the new size
      initialize(croppedMap3D,croppedRGBimg,croppedConsImg);
      findExtremes();
      findCentralPixel();
      calculateNormals();
      calculateBackSide();
    }
  }


  // savePoints
  public void savePoints(String fileName){
    String[] s = new String[nPoints+1];
    s[0] = xSize+" "+ySize;
    
    rgbImg.loadPixels();
    for(int i = 0; i < s.length-1; i++){
      if(consImg[i]){
        PVector p = map3D[i].get();
        color col = 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");
  }


  // savePoints
  public void savePoints(){
    savePoints("savedScan");
  }


  // drawLine
  private void drawLine(PVector p1, PVector p2, color c){
    float distSq = sq(p1.x-p2.x) + sq(p1.y-p2.y) + sq(p1.z-p2.z);
    if(distSq < maxSepSq){
      stroke(c);
      line(p1.x,p1.y,p1.z,p2.x,p2.y,p2.z);
    }
  }


  // drawTriangle
  private void drawTriangle(PVector p1, PVector p2, PVector p3){
    float distSq1 = sq(p1.x-p2.x) + sq(p1.y-p2.y) + sq(p1.z-p2.z);
    float distSq2 = sq(p1.x-p3.x) + sq(p1.y-p3.y) + sq(p1.z-p3.z);
    float distSq3 = sq(p2.x-p3.x) + sq(p2.y-p3.y) + sq(p2.z-p3.z);
    boolean cond = (distSq1 < maxSepSq) && (distSq2 < maxSepSq) && (distSq3 < maxSepSq) &&
                   (distSq1 != 0) && (distSq2 != 0) && (distSq3 != 0);
    if(cond){
      beginShape(TRIANGLES);
        vertex(p1.x,p1.y,p1.z);
        vertex(p2.x,p2.y,p2.z);
        vertex(p3.x,p3.y,p3.z);
      endShape();
    }
  }

  
  // drawTriangle
  private void drawTriangle(PVector p1, PVector p2, PVector p3, color c1, color c2, color c3){
    float distSq1 = sq(p1.x-p2.x) + sq(p1.y-p2.y) + sq(p1.z-p2.z);
    float distSq2 = sq(p1.x-p3.x) + sq(p1.y-p3.y) + sq(p1.z-p3.z);
    float distSq3 = sq(p2.x-p3.x) + sq(p2.y-p3.y) + sq(p2.z-p3.z);
    boolean cond = (distSq1 < maxSepSq) && (distSq2 < maxSepSq) && (distSq3 < maxSepSq) &&
                   (distSq1 != 0) && (distSq2 != 0) && (distSq3 != 0);
    if(cond){
      beginShape(TRIANGLES);
        fill(c1);
        vertex(p1.x,p1.y,p1.z);
        fill(c2);
        vertex(p2.x,p2.y,p2.z);
        fill(c3);
        vertex(p3.x,p3.y,p3.z);
      endShape();
    }
  }


  // drawPsyTriangle
  private void drawPsyTriangle(PVector p1, PVector p2, PVector p3, color c){
    float distSq1 = sq(p1.x-p2.x) + sq(p1.y-p2.y) + sq(p1.z-p2.z);
    float distSq2 = sq(p1.x-p3.x) + sq(p1.y-p3.y) + sq(p1.z-p3.z);
    float distSq3 = sq(p2.x-p3.x) + sq(p2.y-p3.y) + sq(p2.z-p3.z);
    boolean cond = (distSq1 < maxSepSq) && (distSq2 < maxSepSq) && (distSq3 < maxSepSq) &&
                   (distSq1 != 0) && (distSq2 != 0) && (distSq3 != 0);
    if(cond){
      fill(blendColor(c,color(random(-150,150),random(-150,150),0),ADD));
      beginShape(TRIANGLES);
        vertex(p1.x,p1.y,p1.z);
        vertex(p2.x,p2.y,p2.z);
        vertex(p3.x,p3.y,p3.z);
      endShape();
    }
  }

  
  // drawAsPixels
  public void drawAsPixels(int pSize){
    strokeWeight(pSize);
    rgbImg.loadPixels();
    for(int y = 0; y < ySize; y++){
      if(!empty[y]){
        for(int x = ini[y]; x <= end[y]; x++){
          int index = x + y*xSize;
          if(consImg[index]){
            stroke(rgbImg.pixels[index]);
            point(map3D[index].x,map3D[index].y,map3D[index].z);
          }
        }
      }  
    }
    noStroke();
  }
  

  // drawAsLines
  public void drawAsLines(){
    strokeWeight(1);
    rgbImg.loadPixels();
    for(int y = 0; y < ySize; y++){
      if(!empty[y]){
        for(int x = ini[y]; x <= end[y]; x++){
          int index = x + y*xSize;
          if(consImg[index]){
            if(x+1 < xSize && consImg[index+1]){
              drawLine(map3D[index],map3D[index+1],rgbImg.pixels[index]);
            }
            if(y+1 < ySize && consImg[index+xSize]){
              drawLine(map3D[index],map3D[index+xSize],rgbImg.pixels[index]);
            }
            if(x+1 < xSize && y+1 < ySize && consImg[index+1+xSize]){
              drawLine(map3D[index],map3D[index+1+xSize],rgbImg.pixels[index]);
            }
          }
        }
      }
    }
    noStroke();
  }
  

  // drawAsTriangles
  public void drawAsTriangles(){
    noStroke();
    rgbImg.loadPixels();
    for(int y = 0; y < ySize-1; y++){
      if(!empty[y] && !empty[y+1]){
        int xStart = min(ini[y],ini[y+1]);
        int xEnd = max(end[y],end[y+1]);
        for(int x = xStart; x < xEnd; x++){
          int index = x + y*xSize;
          // First triangle
          if(consImg[index] && consImg[index+1] && consImg[index+xSize]){
            drawTriangle(map3D[index],map3D[index+1],map3D[index+xSize],rgbImg.pixels[index],rgbImg.pixels[index+1],rgbImg.pixels[index+xSize]);
          }
          else if(consImg[index] && consImg[index+1+xSize] && consImg[index+xSize]){
            drawTriangle(map3D[index],map3D[index+1+xSize],map3D[index+xSize],rgbImg.pixels[index],rgbImg.pixels[index+1+xSize],rgbImg.pixels[index+xSize]);
          }
          // Second triangle
          if(consImg[index+1] && consImg[index+1+xSize] && consImg[index+xSize]){
            drawTriangle(map3D[index+1],map3D[index+1+xSize],map3D[index+xSize],rgbImg.pixels[index+1],rgbImg.pixels[index+1+xSize],rgbImg.pixels[index+xSize]);
          }
          else if(consImg[index] && consImg[index+1] && consImg[index+1+xSize]){
            drawTriangle(map3D[index],map3D[index+1],map3D[index+1+xSize],rgbImg.pixels[index],rgbImg.pixels[index+1],rgbImg.pixels[index+1+xSize]);
          }
        }
      }
    }
  }


  // drawAsPsyTriangles
  public void drawAsPsyTriangles(){
    noStroke();
    rgbImg.loadPixels();
    for(int y = 0; y < ySize-1; y++){
      if(!empty[y] && !empty[y+1]){
        int xStart = min(ini[y],ini[y+1]);
        int xEnd = max(end[y],end[y+1]);
        for(int x = xStart; x < xEnd; x++){
          int index = x + y*xSize;
          // First triangle
          if(consImg[index] && consImg[index+1] && consImg[index+xSize]){
            drawPsyTriangle(map3D[index],map3D[index+1],map3D[index+xSize],rgbImg.pixels[index]);
          }
          else if(consImg[index] && consImg[index+1+xSize] && consImg[index+xSize]){
            drawPsyTriangle(map3D[index],map3D[index+1+xSize],map3D[index+xSize],rgbImg.pixels[index]);
          }
          // Second triangle
          if(consImg[index+1] && consImg[index+1+xSize] && consImg[index+xSize]){
            drawPsyTriangle(map3D[index+1],map3D[index+1+xSize],map3D[index+xSize],rgbImg.pixels[index+1]);
          }
          else if(consImg[index] && consImg[index+1] && consImg[index+1+xSize]){
            drawPsyTriangle(map3D[index],map3D[index+1],map3D[index+1+xSize],rgbImg.pixels[index+1]);
          }
        }
      }
    }
  }


  // drawBackSide
  public void drawBackSide(color c){
    noStroke();
    fill(c);
    for(int y = 0; y < ySize-1; y++){
      if(!empty[y] && !empty[y+1]){
        int xStart = min(ini[y],ini[y+1]);
        int xEnd = max(end[y],end[y+1]);
        for(int x = xStart; x < xEnd; x++){
          int index = x + y*xSize;
          // First triangle
          if(consImg[index] && consImg[index+1] && consImg[index+xSize]){
            drawTriangle(back3D[index],back3D[index+1],back3D[index+xSize]);
          }
          else if(consImg[index] && consImg[index+1+xSize] && consImg[index+xSize]){
            drawTriangle(back3D[index],back3D[index+1+xSize],back3D[index+xSize]);
          }
          // Second triangle
          if(consImg[index+1] && consImg[index+1+xSize] && consImg[index+xSize]){
            drawTriangle(back3D[index+1],back3D[index+1+xSize],back3D[index+xSize]);
          }
          else if(consImg[index] && consImg[index+1] && consImg[index+1+xSize]){
            drawTriangle(back3D[index],back3D[index+1],back3D[index+1+xSize]);
          }
        }
      }
    }
  }

}

/*
 * Returns a ScanModifier of the selected type
 */

ScanModifier createModifier(ScanContainer s, String type){
  if(type == "empty"){
    ScanModifier m = new EmptyModifier();
    m.create(s);
    return m;
  }
  else if(type == "convolve"){
    ScanModifier m = new ConvolveModifier();
    m.create(s);
    return m;
  }
  else if(type == "ballMod"){
    ScanModifier m = new BallModifier();
    m.create(s);
    return m;
  }
  else{
    ScanModifier m = new EmptyModifier();
    m.create(s);
    return m;
  }
}