/*
 *    http://www.mediamolecule.com/blog/2007/12/10/homebrew-3d-scanner/
 *    http://www.david-laserscanner.com/forum/viewtopic.php?p=1534&sid=84455abad746563ec57604c8d8a14bd7
 *
 *    slidegen.zip (cpp) mm_alex (see above)
 *    fjenett 20071218 (speed-port to processing)
 *    redfish group (Stephen, Ben G, Ben L, Shawn) 20080501 (integration with camera code)
 */

//import processing.opengl.*;

PImage p1, p2, p3, o, o2, gr;
PImage[] imgs;

float P1 = PI*2.0/3.0;
float P2 = PI*4.0/3.0;

int AVERAGE_ITER = 1; // blurs the image dumbly this many times (3x3 kernel)
int HOLE_FILL_ITER = 4; // fills in holes and expands boundaries up to this size in pixels 
int MEDIAN_ITER = 4; // median blurs the image to remove outliers. also has a side effect of hole filling (more dumb than hole filler)
int XRES = 640;
int YRES = 480;

float[][] _wrapphase = new float[YRES][XRES];
float[][] _gamma = new float[YRES][XRES];
float[][] _owrap = new float[YRES][XRES];
float[][] _var = new float[YRES][XRES];
float[][] _ovar = new float[YRES][XRES];
float[][] _dx = new float[YRES][XRES];
float[][] _dy = new float[YRES][XRES];

int[][] _isconf = new int[YRES][XRES];
int[][] _unwraporder = new int[YRES][XRES];

java.util.Vector pix;

int idxx=0;
float scalefac = (2.0 / 3.0) * cos( 1.0 / 6.0 * PI );

// zscale is related to 1/tan(angle-between-projector-and-camera), so for small angles you need a big zscale and vice-versa
float zscale = 75.0; //75.0 * 2.0;

int bestx=0;
int besty=0;
float best=0;
float avgd=0,avgdw=0;

void setup()
{
    size( 800, 600, P3D );

    // input images must be XRES,YRES res color RGB png files
    p2 = loadImage("image1.png");
    p1 = loadImage("image2.png");
    p3 = loadImage("image3.png");

    o = new PImage(p1.width, p1.height);
    o2 = new PImage(p1.width, p1.height);
    gr = new PImage(p1.width, p1.height);

    for (int y=0;y<YRES;++y)
    {
        for (int x=0;x<XRES;++x)
        {
            float fix=0;
            float fixhi=0;

            float ph1=((p1.get(x,y) & 255)/255.0-fix)/(1.0-fix-fixhi);
            float ph2=((p2.get(x,y) & 255)/255.0-fix)/(1.0-fix-fixhi);
            float ph3=((p3.get(x,y) & 255)/255.0-fix)/(1.0-fix-fixhi);
            if (ph1<0) ph1=0;
            if (ph2<0) ph2=0;
            if (ph3<0) ph3=0;
            /*ph1=ph1*ph1;		ph2=ph2*ph2;		ph3=ph3*ph3;
             ph1=sqrtf(ph1);		ph2=sqrtf(ph2);		ph3=sqrtf(ph3);*/
            //float colo;
            float idash = (ph1+ph2+ph3) + 0.5f/255.0;
            float iddash = max( max(ph1,ph2), ph3 ) - min( min(ph1,ph2), ph3 );
            float gamma = iddash/(idash+0.4)*2;

            if (gamma<0.2) gamma = 0;

            float r = unwrapphase( ph1, ph2, ph3, x+y*p1.width );

            _wrapphase[y][x] = r;
            _gamma[y][x]=gamma;

           // o.set( x, y, v4_to_u32( new v4( r, gamma, idash*0.5, 1 ) ) );
        }
    }

    //gr.save(sketchPath("image_grey.tif"));

    //image( o, 0,0 );
    //o.save(sketchPath("image_phased.tif"));

    for (int y=0;y<YRES;++y)
    {
        for (int x=0;x<XRES;++x)
        {
            int k=0; // this sets the kernel size for the variance map
            int x1=x-k;
            int y1=y-k;
            int x2=x+k+1;
            int y2=y+k+1;

            if (x1<0) x1=0;
            if (y1<0) y1=0;
            if (x2>XRES) x2=XRES;
            if (y2>YRES) y2=YRES;
            float dx=0,dy=0,dx2=0,dy2=0,w=0;

            for (int yy=y1;yy<y2;++yy)
            {
                for (int xx=x1;xx<x2;++xx)
                {
                    //if (_gamma[yy][xx]>0) {
                    float _w=1;//_gamma[yy][xx];
                    w+=_w;
                    dx+=_w* _dx[yy][xx];
                    dx2+=_w* _dx[yy][xx]*_dx[yy][xx];
                    dy+=_w* _dy[yy][xx];
                    dy2+=_w* _dy[yy][xx]*_dy[yy][xx];
                    // }
                }
            }

            if (w == 1)
            {
                dx/=w;
                dy/=w;
                dx2/=w;
                dy2/=w;
            }

            dx=dx*dx;
            dy=dy*dy;

            if (dx2<dx)
            {
                int i=1;
            }

            float var = 1.0 / ( 1+ 200*( (dy2-dy)+(dx2-dx) ) ); //_gamma[y][x]
            // this seems to allways be 1.0 ..? 

            if (_gamma[y][x]==0) var=0;

            _var[y][x]=var;
            _ovar[y][x]=var;

            float d = sqrt(dx+dy);

            avgdw += var;
            avgd += var*d;

            float cdx=(x/float(XRES))-0.5f;
            float cdy=(y/float(YRES))-0.5f;

            float cen = var*(1-sqrt(cdx*cdx+cdy*cdy));

            if (cen>best)
            {
                best=cen;
                bestx=x;
                besty=y;
            }
            //o.set( x, y, v4_to_u32( new v4( var, _owrap[y][x], _dy[y][x]+0.5, 1 ) ) );
        }
    }

    //image( o, 0,0 );
   // o.save(sketchPath("image_masked.tif"));

    //bestx=XRES/2; besty=YRES/2;
    //println( avgd );
    //println( avgdw );
    avgd = avgdw / avgd; // fudge the 2.1 if the output mesh appears to be skewed along z

    // trial'n'error value .. not sure why it's not set correctly
    avgd = 20.0;

    // avgd is now the average size (in pixels) of one complete 'fringe' wrap.

    pix = new Vector();
    pix.add(  new int[]{  //new Float(-_var[besty][bestx]),
        bestx,besty                } 
    );
    _var[besty][bestx] = -1;

    while ( !pix.isEmpty() )
    {
        // unwrap & insert n/e/s/w if their _var is >0; set their _var to -1 as we do so to avoid revisiting.

        int[] xy = (int[])pix.remove(0);
        int x = xy[0];
        int y = xy[1];
        float r = _wrapphase[y][x];
        if (y>0 && _var[y-1][x]>0)
        {
            unwrap(r,x,y-1);
        }
        if (y<YRES-1 && _var[y+1][x]>0)
        {
            unwrap(r,x,y+1);
        }
        if (x>0 && _var[y][x-1]>0)
        {
            unwrap(r,x-1,y);
        }
        if (x<XRES-1 && _var[y][x+1]>0)
        {
            unwrap(r,x+1,y);
        }
        //if (idxx>10000) { println("break! ("+pix.size()+")"); break; }
    }


    // hole fill
    for (int iter=0;iter<HOLE_FILL_ITER;++iter)
    {
        for (int y=1;y<YRES-1;++y)
            for (int x=1;x<XRES-1;++x)
            {
                if (_var[y][x]>=0)
                {
                    // is this a hole?
                    boolean gotx=false,goty=false;
                    float rx=0,ry=0;
                    if (_var[y][x-1]==-1 && _var[y][x+1]==-1)
                    {
                        gotx=true;
                        rx=(_wrapphase[y][x-1]+_wrapphase[y][x+1])*0.5;					
                    }
                    else if (_var[y][x-1]==-1 && x>=2 && _var[y][x-2]==-1)
                    {
                        rx=_wrapphase[y][x-1]*2-_wrapphase[y][x-2];
                        gotx=true;
                    }
                    else if (_var[y][x+1]==-1 && x<XRES-2 && _var[y][x+2]==-1)
                    {
                        rx=_wrapphase[y][x+1]*2-_wrapphase[y][x+2];
                        gotx=true;
                    }

                    if (_var[y-1][x]==-1 && _var[y+1][x]==-1)
                    {
                        goty=true;
                        ry=(_wrapphase[y-1][x]+_wrapphase[y+1][x])*0.5;
                    }
                    else if (_var[y-1][x]==-1 && y>=2 && _var[y-2][x]==-1)
                    {
                        ry=_wrapphase[y-1][x]*2-_wrapphase[y-2][x];
                        goty=true;
                    }
                    else if (_var[y+1][x]==-1 && y<YRES-2 && _var[y+2][x]==-1)
                    {
                        ry=_wrapphase[y+1][x]*2-_wrapphase[y+2][x];
                        goty=true;
                    }
                    float r=0;
                    if (gotx) r+=rx; 
                    if (goty) r+=ry;
                    if (gotx && goty) r*=0.5;
                    if (gotx || goty)
                    {
                        _var[y][x]=-2;
                        _wrapphase[y][x]=r;
                    }
                }
            }
        for (int y=1;y<YRES-1;++y)
            for (int x=1;x<XRES-1;++x)
            {
                if (_var[y][x]==-2) _var[y][x]=-1;
            }
    }

    /*
    // average
    for (int iter=0;iter<AVERAGE_ITER;++iter)
    {
        for (int y=1;y<YRES-1;++y)
        {
            for (int x=1;x<XRES-1;++x) if (_var[y][x]==-1)
            {
                int w=0;
                float r=0;
                int[] weights = new int[]{ 
                    1,2,1,2,4,2,1,2,1                                                                                                                                                                                 };
                int ww = 0;
                for (int yy=y-1;yy<=y+1;++yy)
                {
                    for (int xx=x-1;xx<=x+1;++xx)
                    { 
                        if (_var[yy][xx] == -1)
                        {				
                            r += _wrapphase[yy][xx] * weights[ww];
                            w += weights[ww];
                            ww++;
                        }
                    }
                }
                if (w == 1)
                {
                    _wrapphase[y][x] = r/w;
                }
            }
        }
    }
    */

    /*
    // median
    for (int iter=0;iter<MEDIAN_ITER;++iter)
    {
        for (int y=1;y<YRES-1;++y)
            for (int x=1;x<XRES-1;++x) //if (_var[y][x]==-1)
            {

                float[] vals = new float[9];

                int n=0;

                for (int yy=y-1;yy<=y+1;++yy)
                {
                    for (int xx=x-1;xx<=x+1;++xx) if (_var[yy][xx]==-1)
                    {
                        vals[n++]=_wrapphase[yy][xx];
                    }
                }
                if (n == 1)
                {
                    //std::sort(&vals[0],&vals[n]);
                    float r;
                    if ( (n&1) == 1 )
                    {
                        r=vals[n/2];
                    }
                    else
                    {
                        r=(vals[n/2]+vals[n/2-1])*0.5;
                    }
                    _wrapphase[y][x]=r;
                    _var[y][x]=-2;
                }
            }
        for (int y=1;y<YRES-1;++y)
        {
            for (int x=1;x<XRES-1;++x)
            {
                if (_var[y][x]==-2) _var[y][x]=-1;
            }
        }
    }
    */
    
    int idx=0;
    String fout = "";
    for (int y=0;y<YRES;++y)
    {
        for (int x=0;x<XRES;++x)
        {
            float planephase = 0.5-(y-besty)/avgd;

            //if (_var[y][x]>=0) _wrapphase[y][x]=-1000;

           // o2.set( x, y, v4_to_u32( new v4( ((_wrapphase[y][x]-planephase)*0.1+0.2) , _owrap[y][x] , frac(_unwraporder[y][x]/float(idxx)) , 1 ) ) );

            /* if (_var[y][x]==-1)
             {
             _isconf[y][x] = ++idx;
             fout += "v " + float(x) + " " + -float(y) + " " + ((_wrapphase[y][x]-planephase)*zscale)+"\n";
             }
             */
        }
    }

   // o2.save(sketchPath( "image_O2.jpg" ));

    /*for (int y=0;y<YRES;++y)
     {
     for (int x=0;x<XRES;++x)
     {
     
     if ( y > -1 && y+1 < _isconf.length && x > -1 && x+1 < _isconf[y].length && x+1 < _isconf[y+1].length )
     {
     
     fout += "f " + IDX(x+1,y)[0]   + "/" + IDX(x+1,y)[1]   + " "
     + IDX(x,y)[0]     + "/" + IDX(x,y)[1]     + " "
     + IDX(x,y+1)[0]   + "/" + IDX(x,y+1)[1]   + " " 
     + IDX(x+1,y+1)[0] + "/" + IDX(x+1,y+1)[1] + "\n";
     
     }	
     }
     }*/

    //println( fout );
    //saveStrings("image.obj", new String[]{fout});

    //image( o, 0,0 );
}

void draw ()
{
    background(0);

    translate( width/2, height/2, -zscale );
    rotX = -HALF_PI + (mouseY / float(height)) * PI;
    rotY = -HALF_PI + (mouseX / float(width)) * PI;
    rotateY(rotY);
    rotateX(-rotX);


    fill( 255 );
    noStroke();

    if ( pmouseX==mouseX & pmouseY==mouseY ) {
        cnt++;
    }
    else
        cnt = 0;
    
    step = cnt > 5 ? 1 : 5;

    int idx=0;
   // PImage txt = loadImage("redWhite.png");
    for (int y=step;y<YRES;y+=step)
    {
        beginShape(TRIANGLE_STRIP);
        texture(gr);
       // texture(txt);

        for (int x=step;x<XRES;x+=step)
        {

            float planephase = 0.5-(y-besty)/avgd;

            if ( _var[y][x] == -1 )
            {
                vertex( x-bestx, y-besty, (_wrapphase[y][x]-planephase) * zscale, x, y );
            }

            planephase = 0.5-(y-step-besty)/avgd;

            if ( _var[y-step][x-step] == -1 )
            {
                vertex( x-step-bestx, y-step-besty, (_wrapphase[y-step][x-step]-planephase) * zscale, x, y );
            }
        }

        endShape(CLOSE);
    }

    if ( showCenter )
    {
        noFill();

        // red == x
        stroke(255,0,0);
        beginShape();
        vertex(0,0,0);
        vertex(100,0,0);
        endShape();

        // green == y
        stroke(0,255,0);
        beginShape();
        vertex(0,0,0);
        vertex(0,100,0);
        endShape();

        // blue == z
        stroke(0,0,255);
        beginShape();
        vertex(0,0,0);
        vertex(0,0,100);
        endShape();
    }
    if ( saveAFrame )
    {
        saveFrame("image3d/image-"+step+"-####.jpg");
        saveAFrame = false;
    }
}

int step = 5;
int cnt = 0;
boolean saveAFrame = false;
boolean showCenter = false;
float rotX = 0.0;
float rotY = 0.0;

void keyPressed ()
{
    if ( key == '1' )
    {
        zscale += 0.25;
        println(zscale);
    }
    else if ( key == '2' )
    {
        zscale -= 0.25;
        println(zscale);
    }

    if ( key == '3' )
    {
        avgd += 2;
        println(avgd);
    }
    else if ( key == '4' )
    {
        avgd -= 2;
        println(avgd);
    }

    if ( key == 'c' )
    {
        showCenter = !showCenter;
    }
    
    if ( key == 's' )
    {
        saveAFrame = true;   
    }
}