import java.awt.*;
import java.util.*;
import java.awt.event.*;
import java.awt.image.*;
import java.applet.Applet;

public class proj2 extends java.applet.Applet 
{
	Image limage,rimage;
	ShowPanel showp;
	DrawPanel lpic,rpic;
    Controls control;
	Label status;

    public void init() {		
		limage = getImage(getCodeBase(), "cubw_l.gif");
		rimage = getImage(getCodeBase(), "cubw_r.gif");
		setFont(new Font("Helvetica", Font.PLAIN, 14));
		status=new Label ("                                       ");
		lpic=new DrawPanel();
		rpic=new DrawPanel();
		showp=new ShowPanel(status,lpic,rpic);
		control = new Controls(showp,status,this);
		GridBagLayout gridbag = new GridBagLayout();
		GridBagConstraints c = new GridBagConstraints();
		setLayout(gridbag);
		c.fill = GridBagConstraints.BOTH;
		c.insets=new Insets (2,2,2,2);
        c.weightx = 1.0;
        c.weighty = 1.0;
 		c.gridheight = 1;
		gridbag.setConstraints(showp, c);
		add(showp);
 		c.gridwidth = GridBagConstraints.RELATIVE;	   	 
        c.weightx = 1.0;
		gridbag.setConstraints(lpic, c);
		add(lpic);
        c.weightx = 1.0;
 		c.gridwidth = GridBagConstraints.REMAINDER;	   	 
		gridbag.setConstraints(rpic, c);
		add(rpic);
        c.weighty = 0.0;
 		c.gridwidth = GridBagConstraints.RELATIVE;	   	 
		gridbag.setConstraints(control, c);
		add(status);
 		c.gridwidth = GridBagConstraints.REMAINDER;	   	 
		gridbag.setConstraints(control, c);
		add(control);
    }

    public void destroy() {
        remove(status);
        remove(showp);
        remove(control);
    }

    public static void main(String args[]) {
        Frame f = new Frame("Proj2");
        proj2 proj = new proj2();
        proj.init();
        f.add("Center", proj);
        f.setSize(700, 300);
        f.show();
    }
    public String getAppletInfo() {
        return " 3D reconstruction by stereo vision.By Adrian Barbu.";
    }
}

class ShowPanel extends Panel implements MouseListener, MouseMotionListener,Runnable {
	DrawPanel lpic,rpic;
	int[][] Fl,Fr,Fc,z;
	double[][] dx;
	int w,h,mesh=3,h1,w1,view=1;
	double minen=10000,div=2.;
	int L=(int)(25*div);
	double ub=(L-1)/div;
	Point pi=new Point (0,0),pf=new Point(0,0);
	Vector V=new Vector (); //
	Vector Vp=new Vector ();  //coords of the rotated points
    Matrix m=new Matrix ();
	Label status;
	Thread t=new Thread (this);
	public ShowPanel(Label status,DrawPanel lpic,DrawPanel rpic) {
		int i,j;
		this.status =status;
		this.lpic =lpic;
		this.rpic =rpic;		
        setBackground(Color.white);
        addMouseMotionListener(this);
        addMouseListener(this);
		w=h=64;h1=h/mesh;w1=w/mesh;
		Fl=new int[h][w];
		Fr=new int[h][w];	
		dx=new double[h][w];
		lpic.w=w;rpic.w=w;lpic.h=h;rpic.h=h;
		lpic.F=Fl;rpic.F=Fr;
		m.resetview ();
		V.removeAllElements ();
		Vp.removeAllElements ();
		for (j=0;j<h1;j++)
			for(i=0;i<w1;i++){
				V.addElement (new Point3d (mesh*(2*i-w1),mesh*(2*j-h1),100));
				Vp.addElement(new Point3d (mesh*(2*i-w1),mesh*(2*j-h1),100));
			}
		recenter(V);
   }

	public double fi(double x,double tr){
		double r=Math.abs(x);
		return tr*(1-1/(1+r*r));
	}
	public double fi1(double x,double tr){
		if((0<=x)&&(x<tr)) {return x;}
		else {
			if((x<=0)&&(-tr<x)) return -x;
			else return tr;
		}
	}
	public double fi2(double x,double tr){
		double r=x*x;
		if (r<tr) return r;
		else return tr;
	}
	public double fi3(double x,double tr){
		double r;
		if(0<=x) r=Math.sqrt(x);
		else r=Math.sqrt(-x);
		if(r<tr) return r;
		else return tr;
	}
	
	public double val(int[] F, double x){
		int xi;double a;
		xi=(int)x;a=x-xi;
		if ((x>w-1)||(x<0)) return 0;
		if (a==0) return F[xi];
		else return (1-a)*F[xi]+a*F[xi+1];
	}
	public double Ep(double[] d,int i){
		if((d[i]>ub)||(d[i+1]>ub))
			return 0.5;
		else  return fi2(d[i+1]-d[i],30);
	}
	public double Ep1(double[] d,int i){
		return fi2(d[i+1]+d[i-1]-2*d[i],3);
	}
	public double Ed(double[] d,int i,int[] Fl, int[] Fr){
		if (d[i]>ub) return 0.5;
		return fi2(val(Fl,i+d[i])-val(Fr,i-d[i]),500);
	}
	public double Dynamic(double[] d,double c1,int[] Fl, int[] Fr){
		int i,j,k,minat=0;
		double[][][] D=new double [w][L+1][w];
		//  memorizes the best path: D[k][v][] is the best d[] such that d[k]=v/div  
		double[][] E=new double [w][L+1]; 
		// E[k][v] = the minimal energy of chains d such that d[k]=v/div 
		// v=L means ocluded point,
		double min,r;
		for (j=0;j<L+1;j++){
			d[1]=j/div;D[1][j][1]=j/div;	
			min=10000;
			for (i=0;i<L+1;i++){ 
				d[0]=i/div;r=Ed(d,0,Fl,Fr)+c1*Ep(d,0);
				if (r<min) {min=r;minat=i;}
			}
			E[1][j]=min+Ed(d,1,Fl,Fr);
			D[1][j][0]=minat/div;
		} //done case k=1=initial condition
		for (k=2;k<w;k++){
			for (j=0;j<L+1;j++){
				d[k]=j/div;D[k][j][k]=j/div;		
				min=10000;
				for (i=0;i<L+1;i++){ 
					d[k-1]=i/div;
					r=E[k-1][i]+c1*Ep(d,k-1);
					if (r<min) {min=r;minat=i;}
				}
				for (i=0;i<k;i++) 
					d[i]=D[k][j][i]=D[k-1][minat][i];
				E[k][j]=min+Ed(d,k,Fl,Fr);
			}
		}
		min=10000;
		for (j=0;j<L+1;j++){
			r=E[w-1][j];	
			if (r<min) {min=r;minat=j;}
		}
		for (i=0;i<w;i++) 
			d[i]=D[w-1][minat][i];
		return E[w-1][minat];
	}
	public void run(){
		int i,j;
		double r,ht;
		status.setText ("Performing minimization...");
		m.resetview ();
		for (j=0;j<h;j++){
			double x=Dynamic(dx[j],1,Fl[j],Fr[j]);
			if (j%mesh==0) {
				updatez(V);repaint();}
		}
		status.setText ("Done.");		
	}
 	public void setDrawMode(int mode) {
    }
	public void recenter(Vector V){
		double	m=0;
		Point3d p;
		for (int k=0;k<V.size ();k++){
			p=(Point3d)V.elementAt (k);
			m=m+p.z;
		}
		m=m/V.size ();
		for (int k=0;k<V.size ();k++){
			p=(Point3d)V.elementAt (k);
			p.z=p.z-m;
		}					
	}
	public void mouseClicked(MouseEvent e) {
    }
    public void mousePressed(MouseEvent e) {
		pi=e.getPoint ();
   }
    public void mouseDragged(MouseEvent e) {
        e.consume();
		pf=e.getPoint ();
		m.xrot ((pi.y-pf.y) * 7./getSize().height);
		m.yrot ((pi.x-pf.x )* 7./getSize().width);
		pi=pf;
        repaint();	
    }
    public void mouseReleased(MouseEvent e) {
		pf=e.getPoint ();
		repaint();
    }
    public void mouseMoved(MouseEvent e) {
    }
    public void mouseEntered(MouseEvent e) {
    }
    public void mouseExited(MouseEvent e) {
    }
	public boolean ocluded(int i,int j){
		return (dx[j][i]>ub);
	}
    public void paint(Graphics g) {
		int i,j;
		int sizex=getSize().width /2,sizey=(int)((getSize().height /2)*1.2);
		Point3d p,q,r;
        g.setColor(Color.black);
        g.setPaintMode();
		if(view==1){
			m.update (V,Vp);
			for(j=0;j<h1-1;j++)
				for (i=0;i<w1-1;i++){
					p=(Point3d)Vp.elementAt (j*w1+i);
					q=(Point3d)Vp.elementAt ((j+1)*w1+i);
					r=(Point3d)Vp.elementAt (j*w1+i+1);
					if ((!ocluded(i*mesh,j*mesh))&&(!ocluded(i*mesh,(j+1)*mesh)))
						g.drawLine (p.x+sizex,p.y+sizey,q.x+sizex,q.y+sizey);
					if ((!ocluded(i*mesh,j*mesh))&&(!ocluded((i+1)*mesh,j*mesh)))
						g.drawLine (p.x+sizex,p.y+sizey,r.x+sizex,r.y+sizey);
				}
			for(j=0;j<h1-1;j++){
				p=(Point3d)Vp.elementAt (j*w1+w1-1);
				q=(Point3d)Vp.elementAt ((j+1)*w1+w1-1);
				if ((!ocluded((w1-1)*mesh,j*mesh))&&(!ocluded((w1-1)*mesh,(j+1)*mesh)))
					g.drawLine (p.x+sizex,p.y+sizey,q.x+sizex,q.y+sizey);
			}
			for (i=0;i<w1-1;i++){
				p=(Point3d)Vp.elementAt ((h1-1)*w1+i);
				q=(Point3d)Vp.elementAt ((h1-1)*w1+i+1);
				if ((!ocluded(i*mesh,(h1-1)*mesh))&&(!ocluded((i+1)*mesh,(h1-1)*mesh)))
					g.drawLine (p.x+sizex,p.y+sizey,q.x+sizex,q.y+sizey);
			}
		}
		if(view==2){
			int c;
			m.update (V,Vp);
			for(i=0;i<w1;i++)
				for (j=0;j<h1;j++)
					if (!ocluded(i*mesh,j*mesh)){
						p=(Point3d)Vp.elementAt (i+j*w1);
						c=(int) val(Fl[j*mesh],i*mesh+dx[i*mesh][j*mesh]);
						g.setColor (new Color (c,c,c));
						g.fillOval (p.x+sizex-mesh,p.y+sizey-mesh,2*mesh,2*mesh);
					}
		}
	}
	public void updatez(Vector V){
		int i,j;
		double r=0,ht;
		Point3d p;
		for (j=0;j<h1;j++)
			for(i=0;i<w1;i++){
				r=Math.abs(dx[j*mesh][i*mesh]);
				if(r>1) ht=100/r;
				else ht=100;
				p=(Point3d)V.elementAt (j*w1+i);
				p.z=ht;
			}
		recenter(V);
	}
}

class DrawPanel extends Panel{
	int[][] F;
	int w,h;
    public DrawPanel() {
        setBackground(Color.white);
    }
	public void setDrawMode(int mode) {
    }
    public void paint(Graphics g) {
		for(int j=0;j<h;j++)
			for(int i=0;i<w;i++){
				g.setColor (new Color (F[j][i],F[j][i],F[j][i]));
				g.fillRect (3*i,3*j,3,3);	
			}
	}
}

class Controls extends Panel implements ActionListener {
	ShowPanel showp;
	proj2 proj;
	Button bRandom,bFig1,bStart,bStripes,bPixels,bMesh;
	Label status;
    public Controls( ShowPanel showp, Label status,proj2 proj) {
		this.status =status;
		this.showp=showp;
		this.proj=proj;
        setLayout(new GridLayout(1,3));
		setBackground(Color.lightGray);
		bRandom = new Button("Random");
		add(bRandom);
		bRandom.addActionListener (this);
		bRandom.setForeground(Color.black);
		bStripes = new Button("Stripes");
		add(bStripes);
		bStripes.addActionListener (this);
		bStripes.setForeground(Color.black);
		bFig1 = new Button("Fig.1");
		add(bFig1);
		bFig1.addActionListener (this);
		bFig1.setForeground(Color.black);
		bMesh = new Button("Mesh");
		add(bMesh);
		bMesh.addActionListener (this);
		bMesh.setForeground(Color.black);
		bPixels = new Button("Pixels");
		add(bPixels);
		bPixels.addActionListener (this);
		bPixels.setForeground(Color.black);
		bStart = new Button("Start");
		add(bStart);
		bStart.addActionListener (this);
		bStart.setForeground(Color.black);
			int i,j,h=showp.h,w=showp.w;
			int[][] F=new int [h][w+2];
			for( i=0;i<w+2;i++)
				for(j=0;j<h;j++){
					double r;
					r=29*(i%9);
					F[j][i]=(int)r;
				}
			for(i=0;i<w;i++)
				for(j=0;j<h;j++) showp.Fl[j][i]=F[j][i];
			for(i=0;i<w;i++)
				for(j=0;j<h;j++) showp.Fr[j][i]=F[j][i+2];
			for(i=20;i<40;i++)
				for(j=20;j<40;j++) showp.Fl[j][i+4]=150;
			for(i=20;i<40;i++)
				for(j=20;j<40;j++) showp.Fr[j][i-4]=150;			
			showp.lpic.repaint();
			showp.rpic.repaint();
			for (j=0;j<h;j++)
				for (i=0;i<w;i++) showp.dx[j][i]=0;
			showp.updatez(showp.V);
			showp.repaint();
    }

    public void paint(Graphics g) {
        Rectangle r = getBounds();
		g.setColor(Color.lightGray);
        g.draw3DRect(0, 0, r.width-1, r.height-1, true);
    }

	public void actionPerformed(ActionEvent e) {
		Object src = e.getSource();
		if (src == bRandom) {
			if (showp.t.isAlive ()){showp.t.stop() ;}
			int i,j,h=showp.h,w=showp.w;
			int[][] F=new int [h][w+2];
			for( i=0;i<w+2;i++)
				for(j=0;j<h;j++){
					double r;
					r=Math.random ()*255;
					F[j][i]=(int)r;
				}
			for(i=0;i<w;i++)
				for(j=0;j<h;j++) showp.Fl[j][i]=F[j][i];
			for(i=0;i<w;i++)
				for(j=0;j<h;j++) showp.Fr[j][i]=F[j][i+2];
			for(i=20;i<40;i++)
				for(j=20;j<40;j++) showp.Fl[j][i+4]=F[j][i+1];
			for(i=20;i<40;i++)
				for(j=20;j<40;j++) showp.Fr[j][i-4]=F[j][i+1];			
			showp.lpic.repaint();
			showp.rpic.repaint();
			for (j=0;j<h;j++)
				for (i=0;i<w;i++) showp.dx[j][i]=0;
			showp.updatez(showp.V);
			showp.m.resetview ();
			status.setText ("");
			showp.repaint();
		}
		if (src == bStripes) {
			if (showp.t.isAlive ()){showp.t.stop() ;}
			int i,j,h=showp.h,w=showp.w;
			int[][] F=new int [h][w+2];
			for( i=0;i<w+2;i++)
				for(j=0;j<h;j++){
					double r;
					r=29*(i%9);
					F[j][i]=(int)r;
				}
			for(i=0;i<w;i++)
				for(j=0;j<h;j++) showp.Fl[j][i]=F[j][i];
			for(i=0;i<w;i++)
				for(j=0;j<h;j++) showp.Fr[j][i]=F[j][i+2];
			for(i=20;i<40;i++)
				for(j=20;j<40;j++) showp.Fl[j][i+4]=150;
			for(i=20;i<40;i++)
				for(j=20;j<40;j++) showp.Fr[j][i-4]=150;			
			showp.lpic.repaint();
			showp.rpic.repaint();
			for (j=0;j<h;j++)
				for (i=0;i<w;i++) showp.dx[j][i]=0;
			showp.updatez(showp.V);
			showp.m.resetview ();
			status.setText ("");
			showp.repaint();
		}  
		if (src == bFig1) {
			if (showp.t.isAlive ()){showp.t.stop() ;}
			/*w=limage.getWidth (this);
			if (rimage.getWidth (this)<w) w=rimage.getWidth (this);
			h=limage.getHeight (this);
			if (rimage.getHeight (this)<h) h=rimage.getHeight (this);*/
			getpixels(proj.limage,showp.Fl,showp.w,showp.h);
			getpixels(proj.rimage,showp.Fr,showp.w,showp.h);							
			showp.lpic.repaint();
			showp.rpic.repaint();
			for (int j=0;j<showp.h;j++)
				for (int i=0;i<showp.w;i++) showp.dx[j][i]=0;
			showp.updatez(showp.V);
			showp.m.resetview ();
			status.setText ("");
			showp.repaint();
		}
		if (src == bMesh) {
			showp.view =1;
			showp.repaint ();
		}  
		if (src == bPixels) {
			showp.view =2;
			showp.repaint ();
		}  
		if (src == bStart) {
			if (showp.t.isAlive ()){showp.t.stop() ;}
			showp.t=new Thread (showp);
			for (int j=0;j<showp.h;j++)
				for (int i=0;i<showp.w;i++) showp.dx[j][i]=0;
			showp.t.start();
		}  
	}
	public void getpixels(Image img,int[][] pix ,int w,int h) {
		int[] pixels=new int [w*h];
		PixelGrabber pg = new PixelGrabber(img, 0, 0, w, h, pixels, 0, w);
		try {
		    pg.grabPixels();
		} catch (InterruptedException e) {
		    System.err.println("interrupted waiting for pixels!");
		    return;
		}
		if ((pg.getStatus() & ImageObserver.ABORT) != 0) {
		    System.err.println("image fetch aborted or errored");
		    return;
		}
		for (int j=0;j<h;j++)
			for (int i=0;i<w;i++) pix[j][i]=pixels[j*w+i]& 0xff;
	}
}

class Point3d extends Point {
	double z;
	Point3d(Point3d p){
		super(p.x,p.y);
		this.z =p.z;
	}
	Point3d(Point p,int z){
		super(p);
		this.z =z;
	}
	Point3d(int x,int y,double z){
		super(x,y);
		this.z =z;
	}
	public void setLocation(int x, int y, double z){
		this.setLocation (x,y);
		this.z=z;
	}
	public void setLocation(Point3d p){
		int x=p.x,y=p.y; double z=p.z;
		this.x=x;
		this.y=y;
		this.z=z;
	}
}
class Matrix {
	//Matrix for the rotation of the figure
	//memorizes all rotations that have been done.
    double xx, xy, xz;
    double yx, yy, yz;
    double zx, zy, zz;
	Matrix(){
		xx=yy=zz=1;
		xy=xz=yx=yz=zx=zy=0;
	}
	public void reset(){
		xx=yy=zz=1;
		xy=xz=yx=yz=zx=zy=0;
	}
	public void resetview(){
		reset ();
		xrot (Math.PI /2);yrot (0.5);xrot (-0.5);
	}
	public void xrot(double theta) {
		//rotate teta rad arround x axis
		double ct = Math.cos(theta);
		double st = Math.sin(theta);
		double Nyx = yx * ct + zx * st;
		double Nyy = yy * ct + zy * st;
		double Nyz = yz * ct + zz * st;
		double Nzx = zx * ct - yx * st;
		double Nzy = zy * ct - yy * st;
		double Nzz = zz * ct - yz * st;
		yx = Nyx;
		yy = Nyy;
		yz = Nyz;
		zx = Nzx;
		zy = Nzy;
		zz = Nzz;
    }
    public void yrot(double theta) {
		//rotate teta rad arround y axis
		double ct = Math.cos(theta);
		double st = Math.sin(theta);
		double Nxx = xx * ct + zx * st;
		double Nxy = xy * ct + zy * st;
		double Nxz = xz * ct + zz * st;
		float Nzx = (float) (zx * ct - xx * st);
		float Nzy = (float) (zy * ct - xy * st);
		float Nzz = (float) (zz * ct - xz * st);
		xx = Nxx;
		xy = Nxy;
		xz = Nxz;
		zx = Nzx;
		zy = Nzy;
		zz = Nzz;
    }
	public void rotatept(Point3d p){
		double x=xx*p.x+xy*p.y+xz*p.z;
		double y=yx*p.x+yy*p.y+yz*p.z;
		double z=zx*p.x+zy*p.y+zz*p.z;
		p.setLocation ((int)x,(int)y,z);		
	}
	public void update (Vector V,Vector Vp){
		int i,n=V.size ();
		Point3d p,q;
		for (i=0;i<n;i++){
			p=(Point3d) V.elementAt (i);
			q=(Point3d) Vp.elementAt (i);
			q.setLocation (p);
			rotatept(q);
		}
	}
}