/*
  Use the wrapped phase information,  and propagate it across the boundaries.
  This implementation uses a flood-fill propagation algorithm.
  
  Because the algorithm starts in the center and propagates outwards,
  so if you have noise (e.g.: a black background, a shadow) in
  the center, then it may not reconstruct your image.
*/

LinkedList toProcess;

void phaseUnwrap() {
  int startX = inputWidth / 2;
  int startY = inputHeight / 2;

  toProcess = new LinkedList();
  toProcess.add(new int[]{startX, startY});
  process[startX][startY] = false;

  while (!toProcess.isEmpty()) {
    int[] xy = (int[]) toProcess.remove();
    int x = xy[0];
    int y = xy[1];
    float r = phase[y][x];
    
    if (y > 0)
      phaseUnwrap(r, x, y-1);
    if (y < inputHeight-1)
      phaseUnwrap(r, x, y+1);
    if (x > 0)
      phaseUnwrap(r, x-1, y);
    if (x < inputWidth-1)
      phaseUnwrap(r, x+1, y);
  }
}

void phaseUnwrap(float basePhase, int x, int y) {
  if(process[y][x]) {
    float diff = phase[y][x] - (basePhase - (int) basePhase);
    if (diff > .5)
      diff--;
    if (diff < -.5)
      diff++;
    phase[y][x] = basePhase + diff;
    process[y][x] = false;
    toProcess.add(new int[]{x, y});
  }
}

  /*
  Assumes you're using grayscale images.
  Go through all the pixels in the out of phase images,
  and determine their angle (theta). Throw out noisy pixels.
*/

void phaseWrap() { 
  PImage phase1Image = loadImage("phase1.jpg");
  PImage phase2Image = loadImage("phase2.jpg");
  PImage phase3Image = loadImage("phase3.jpg");
  
  float sqrt3 = sqrt(3);
  for (int y = 0; y < inputHeight; y++) {
    for (int x = 0; x < inputWidth; x++) {     
      int i = x + y * inputWidth;  
      
      float phase1 = (phase1Image.pixels[i] & 255) / 255.;
      float phase2 = (phase2Image.pixels[i] & 255) / 255.;
      float phase3 = (phase3Image.pixels[i] & 255) / 255.;
      
      float phaseSum = phase1 + phase2 + phase3;
      float phaseRange = max(phase1, phase2, phase3) - min(phase1, phase2, phase3);
      
      // avoid the noise floor
      float gamma = phaseRange / phaseSum;
      mask[y][x] = gamma < noiseTolerance;
      process[y][x] = !mask[y][x];

      // this equation can be found in Song Zhang's
      // "Recent progresses on real-time 3D shape measurement..."
      // and it is the "bottleneck" of the algorithm
      // it can be sped up with a LUT, which has the benefit
      // of allowing for simultaneous gamma correction.
      phase[y][x] = atan2(sqrt3 * (phase1 - phase3), 2 * phase2 - phase1 - phase3) / TWO_PI;
    }
  }
}

  import peasy.*;

/*
 These three variables are the main "settings".
 
 zscale corresponds to how much "depth" the image has,
 zskew is how "skewed" the imaging plane is.
 
 These two variables are dependent on both the angle
 between the projector and camera, and the number of stripes.
 The sign on both is based on the direction of the stripes
 (whether they're moving up vs down)
 as well as the orientation of the camera and projector
 (which one is above the other).
 
 noiseTolerance can significantly change whether an image
 can be reconstructed or not. Start with it small, and work
 up until you start losing important parts of the image.
*/
float zscale = 140;
float zskew = 23;
float noiseTolerance = 0.15;

int inputWidth = 480;
int inputHeight = 640;

PeasyCam cam;

float[][] phase = new float[inputHeight][inputWidth];
boolean[][] mask = new boolean[inputHeight][inputWidth];
boolean[][] process = new boolean[inputHeight][inputWidth];

void setup() {
  size(inputWidth, inputHeight, P3D);
  cam = new PeasyCam(this, width);
  
  stroke(255);
  
  phaseWrap();
  phaseUnwrap();
}

void draw () {
  background(0);
  translate(-inputWidth / 2, -inputHeight / 2);  
  int step = 2;
  for (int y = step; y < inputHeight; y += step) {
    float planephase = 0.5 - (y - (inputHeight / 2)) / zskew;
    for (int x = step; x < inputWidth; x += step)
      if (!mask[y][x])
        point(x, y, (phase[y][x] - planephase) * zscale);
  }
}