// http://local.wasp.uwa.edu.au/~pbourke/geometry/lineline3d/

double eps = 100 * Double.MIN_VALUE;

Vec3D rayRayAverage(Ray3D a, Ray3D b) {
  Vec3D p1 = a.getPointAtDistance(0);
  Vec3D p2 = a.getPointAtDistance(1);
  Vec3D p3 = b.getPointAtDistance(0);
  Vec3D p4 = b.getPointAtDistance(1);
  Vec3D p13 = new Vec3D();
  Vec3D p43 = new Vec3D();
  Vec3D p21 = new Vec3D();
  double d1343, d4321, d1321, d4343, d2121;
  double numer, denom;

  p13.x = p1.x - p3.x;
  p13.y = p1.y - p3.y;
  p13.z = p1.z - p3.z;
  p43.x = p4.x - p3.x;
  p43.y = p4.y - p3.y;
  p43.z = p4.z - p3.z;
  if (abs(p43.x) < eps && abs(p43.y) < eps && abs(p43.z) < eps)
    return null;
  p21.x = p2.x - p1.x;
  p21.y = p2.y - p1.y;
  p21.z = p2.z - p1.z;
  if (abs(p21.x) < eps && abs(p21.y) < eps && abs(p21.z) < eps)
    return null;

  d1343 = p13.x * p43.x + p13.y * p43.y + p13.z * p43.z;
  d4321 = p43.x * p21.x + p43.y * p21.y + p43.z * p21.z;
  d1321 = p13.x * p21.x + p13.y * p21.y + p13.z * p21.z;
  d4343 = p43.x * p43.x + p43.y * p43.y + p43.z * p43.z;
  d2121 = p21.x * p21.x + p21.y * p21.y + p21.z * p21.z;

  denom = d2121 * d4343 - d4321 * d4321;
  if (Math.abs(denom) < eps)
    return null;
  numer = d1343 * d4321 - d1321 * d4343;

  double mua = numer / denom;
  double mub = (d1343 + d4321 * (mua)) / d4343;

  float x = (float) (p1.x + mua * p21.x + p3.x + mub * p43.x) / 2;
  float y = (float) (p1.y + mua * p21.y + p3.y + mub * p43.y) / 2;
  float z = (float) (p1.z + mua * p21.z + p3.z + mub * p43.z) / 2;
  
  return new Vec3D(x, y, z);
}

  import toxi.geom.*;
import peasy.*;

PeasyCam cam;

Camera capture, projector;
Vec3D[][] points;

int wi = 640;
int hi = 480;
int n = 18;

float cpDist = 250;
float screenDist = 400;
float pAov = HALF_PI / 8;
float cAov = HALF_PI / 5;

float[][][] mapping;
void setup() {
  size(640, 480, P3D);
  cam = new PeasyCam(this, 512);
  
  projector = new Camera(new Vec3D(0, 0, -screenDist), new Vec3D(0, 0, screenDist), 4 * pAov, 3 * pAov);
  capture = new Camera(new Vec3D(0, -cpDist, -screenDist), new Vec3D(0, cpDist, screenDist), 4 * cAov, 3 * cAov);
  
  mapping = loadGrayPositions();
  
  points = new Vec3D[hi][wi];
  print("Building intersections... ");
  for(int i = 0; i < hi; i++) {
    for(int j = 0; j < wi; j++) {
      if(!(mapping[i][j][0] == 0 && mapping[i][j][1] == 0)) {
        Ray3D pLine = projector.getLine(mapping[i][j][1], mapping[i][j][0]);
        Ray3D cLine = capture.getLine((float) j / wi, (float) i / hi);
        points[i][j] = rayRayAverage(pLine, cLine);
      }
    }
  }
  println("done.");
}

void draw() {
  background(0);
  
  stroke(255, 0, 0, 200);
  projector.draw(512);
  
  stroke(0, 0, 255, 200);
  capture.draw(512);
  
  stroke(255, 150);
  noFill();
  for(int i = 0; i < hi; i ++)
    for(int j = 0; j < wi; j ++)
      if(points[i][j] != null)
        point(points[i][j]);
}

void keyPressed() {
  if(key == ' ')
    setup();
}

  void line(Ray3D r, float dist) {
  Vec3D p = r.getPointAtDistance(dist);
  line(r.x, r.y, r.z, p.x, p.y, p.z);
}

void line(Vec3D v) {
  line(0, 0, 0, v.x, v.y, v.z);
}

void point(Vec3D v) {
  point(v.x, v.y, v.z);
}

void vertex(Vec3D v) {
  vertex(v.x, v.y, v.z);
}

void translate(Vec3D v) {
  translate(v.x, v.y, v.z);
}

// due to a bug in toxiclibs
// http://code.google.com/p/toxiclibs/issues/detail?id=7
Vec3D swapYZ(Vec3D v) {
  float y = v.y;
  v.y = v.z;
  v.z = y;
  return v;
}

  class Camera extends Ray3D {
  float hAov, vAov;
  Camera(Vec3D o, Vec3D d, float hAov, float vAov) {
    super(o, d);
    this.hAov = hAov;
    this.vAov = vAov;
  }
  // not really, but close enough.
  Ray3D getLine(float xi, float yi) {
    Vec3D dir = getDirection();
    float theta = map(xi, 0, 1, -hAov, hAov) / 2;
    float phi = map(yi, 0, 1, -vAov, vAov) / 2;
    float x = sin(theta);
    float y = sin(phi);
    float z = sin(HALF_PI - theta);
    Vec3D lineDir = new Vec3D(x, y, z);
    lineDir.rotateX(getDirection().angleBetween(new Vec3D(0, 0, 1)));
    swapYZ(lineDir);
    return new Ray3D(this, lineDir);
  }
  void draw(float imageDist) {    
    // center line
    line(this, imageDist);
    
    // fov
    Ray3D topLeft = getLine(0, 0);
    Ray3D topRight = getLine(1, 0);
    Ray3D bottomLeft = getLine(0, 1);
    Ray3D bottomRight = getLine(1, 1);
    
    line(topLeft, imageDist);
    line(topRight, imageDist);
    line(bottomLeft, imageDist);
    line(bottomRight, imageDist);
    
    beginShape();
    vertex(topLeft.getPointAtDistance(imageDist));
    vertex(topRight.getPointAtDistance(imageDist));
    vertex(bottomRight.getPointAtDistance(imageDist));
    vertex(bottomLeft.getPointAtDistance(imageDist));
    endShape(CLOSE);
    
    pushMatrix();
    translate(this);
    noFill();
    box(10);
    popMatrix();
  }
}

  int cutoff = 128;

float[][][] loadGrayPositions() {
  println("Acquiring encoded position data."); 
  boolean[][][] grayMapping = new boolean[hi][wi][n];
  boolean[][] binImg = new boolean[hi][wi];
  for(int level = 0; level < n; level++) {
    print(level + ": loading... ");
    PImage cur = loadImage((level + 1) + ".jpg");
    
    print("binary... ");
    makeBinary(cur, cutoff, binImg);
    
    print("noting... ");
    for(int i = 0; i < hi; i++) {
      for(int j = 0; j < wi; j++) {
        grayMapping[i][j][level] = binImg[i][j];
      }
    }
    
    println("done.");
  }
  
  print("Converting gray code to binary... ");
  boolean[][][] binMapping = new boolean[hi][wi][n];
  for(int i = 0; i < hi; i++) {
    for(int j = 0; j < wi; j++) {
      boolean lasty = false;
      boolean lastx = false;
      for(int level = 0; level < n; level += 2) {
        binMapping[i][j][level] = grayMapping[i][j][level] ^ lasty;
        lasty = binMapping[i][j][level];
        binMapping[i][j][level + 1] = grayMapping[i][j][level + 1] ^ lastx;
        lastx = binMapping[i][j][level + 1];
      }
    }
  }
  println("done.");
  
  float[] levelOffset = new float[n];
  for(int i = 0; i < n; i++) {
    levelOffset[i] = 1. / pow(2, (i / 2) + 1);
  }
  
  print("Decoding position mapping... ");
  float[][][] mapping = new float[hi][wi][2];
  for(int i = 0; i < hi; i++) {
    for(int j = 0; j < wi; j++) {
      for(int level = 0; level < n; level += 2) {
        mapping[i][j][0] += binMapping[i][j][level] ? levelOffset[level] : 0;
        mapping[i][j][1] += binMapping[i][j][level + 1] ? levelOffset[level + 1] : 0;
      }
      mapping[i][j][0] *= 4./3.; // the entire 1024x1024 isn't squished into 1024x768
    }
  }
  println("done.");
  
  return mapping;
}

boolean[][] makeBinary(PImage img, int cutoff, boolean[][] binImg) {
  img.loadPixels();
  for(int y = 0; y < img.height; y++) {
    int row = y * img.width;
    for(int x = 0; x < img.width; x++) {
      binImg[y][x] = brightness(img.pixels[row + x]) > cutoff;
    }
  }
  return binImg;
}

PImage makeImage(boolean[][] binImg) {
  int wi = binImg[0].length;
  int hi = binImg.length;
  PImage img = new PImage(wi, hi);
  img.loadPixels();
  for(int y = 0; y < hi; y++) {
    int row = y * wi;
    for(int x = 0; x < wi; x++) {
      img.pixels[row + x] = binImg[y][x] ? 0xffffff : 0x000000;
    }
  }
  img.updatePixels();
  return img;
}