//============================================================================
// Source: avoid.java completed on April 30, 1998
// Last revised on May 10, 1998, Used as an applet
// By: Wei Zhang at UB
//============================================================================
import java.applet.*;
import java.awt.*;

public class avoid extends Applet {

	double d1, a2, a3, x, y, z;
	double od, or;
	double X0, Y0, Z0, Xf, Yf, Zf;
	int vn, mode;
	double[][] t;
	g3d wh = null;

	TextField[] tf = new TextField[12];
	TextArea show = new TextArea();

//	final double err = -Double.MAX_VALUE;
	final double precision = 9.9e-4;
	final double minVal = 1e-10; // define zero value

//	public static void main(String[] argv) {
//		new avoid();
//	}

//	avoid() {
//		super("Obstacle Avoidance for Manipulators");

	public void init() {

		// Create UI layout
		setLayout(new GridLayout(0,1));
		Panel p = new Panel();
		p.setLayout(new GridLayout(0,6,10,10));
		p.setFont(new Font("Helvetica", Font.BOLD, 14));

//		p.add(new Label("Manipulator:", Label.RIGHT));
		p.add(tf[0] = new TextField("1", 10)); // d1
		p.add(new Label("<==d1", Label.LEFT));
		p.add(tf[1] = new TextField("3", 10)); // a2
		p.add(new Label("<==a2", Label.LEFT));
		p.add(tf[2] = new TextField("2", 10)); // a3
		p.add(new Label("<==a3", Label.LEFT));

//		p.add(new Label("Initial Position:", Label.RIGHT));
		p.add(tf[3] = new TextField("0", 10)); // X0
		tf[3].setForeground(Color.blue);
		p.add(new Label("<==X0", Label.LEFT));
		p.add(tf[4] = new TextField("-3", 10));// Y0
		tf[4].setForeground(Color.blue);
		p.add(new Label("<==Y0", Label.LEFT));
		p.add(tf[5] = new TextField("1", 10)); // Z0
		tf[5].setForeground(Color.blue);
		p.add(new Label("<==Z0", Label.LEFT));

//		p.add(new Label("Final Position:", Label.RIGHT));
		p.add(tf[6] = new TextField("0", 10)); // Xf
		p.add(new Label("<==Xf", Label.LEFT));
		p.add(tf[7] = new TextField("3", 10)); // Yf
		p.add(new Label("<==Yf", Label.LEFT));
		p.add(tf[8] = new TextField("1", 10)); // Zf
		p.add(new Label("<==Zf", Label.LEFT));

//		p.add(new Label("Obstacle:", Label.RIGHT));
		p.add(tf[9] = new TextField("4", 10)); // distance
		tf[9].setForeground(Color.blue);
		p.add(new Label("<==Distance", Label.LEFT));
		p.add(tf[10] = new TextField("2", 10)); // radius
		tf[10].setForeground(Color.blue);
		p.add(new Label("<==Radius", Label.LEFT));
		p.add(tf[11] = new TextField("21", 10)); // number of points
		tf[11].setForeground(Color.blue);
		p.add(new Label("<==# of points", Label.LEFT));

		p.add(new Button("Compute!"));
		p.add(new Label("Sequence", Label.LEFT));
		p.add(new Label(" "));
		p.add(new Label("X,Y,Z", Label.CENTER));
		p.add(new Label(" "));
		p.add(new Label("Theta-1,2,3", Label.RIGHT));
//		p.add(new Label(" "));

		add(p);
		show.setFont(new Font("Courier", Font.PLAIN, 12));
		show.setEditable(false);
		add(show);
		resize(700,500);
		show();
	}

	public boolean handleEvent(Event e) {
//		if(e.id == e.WINDOW_DESTROY) {
//			dispose();
//			System.exit(0);
//			return true;
//		}
		if("Compute!".equals(e.arg)) {
			// compute values
			d1=Double.valueOf(tf[0].getText()).doubleValue();
			a2=Double.valueOf(tf[1].getText()).doubleValue();
			a2 = (a2 < 0 ? 0 : a2);
			a3=Double.valueOf(tf[2].getText()).doubleValue();
			a3 = (a3 < 0 ? 0 : a3);

			X0=Double.valueOf(tf[3].getText()).doubleValue();
			Y0=Double.valueOf(tf[4].getText()).doubleValue();
			Z0=Double.valueOf(tf[5].getText()).doubleValue();

			Xf=Double.valueOf(tf[6].getText()).doubleValue();
			Yf=Double.valueOf(tf[7].getText()).doubleValue();
			Zf=Double.valueOf(tf[8].getText()).doubleValue();

			od=Double.valueOf(tf[9].getText()).doubleValue();
			od = (od < 0 ? 0 : od);
			or=Double.valueOf(tf[10].getText()).doubleValue();
			or = (or < 0 ? 0 : or);
			vn=Integer.parseInt(tf[11].getText());
			vn=(vn < 2 ? 2 : vn);
			t = new double[vn][6];

			show.setText("Computing...");
			show.setText(disp());
			return true;
		}
		return false;
	}

	double c(double x) {return Math.cos(x);}
	double s(double x) {return Math.sin(x);}
	double q(double x) {return Math.sqrt(x);}
	double a(double x, double y) {return Math.atan2(y, x);}

	double roundoff(double d) {
		if(Math.ceil(d)-d <= precision) return Math.ceil(d);
		if(d-Math.floor(d) <= precision) return Math.floor(d);
		return d;
	}

	String x(double num, int len, int dec) {
		String ret = "", tmp = Double.toString(num);
		int decPos = tmp.indexOf('.');
		decPos = (decPos == -1 ? tmp.length() : Math.min(decPos+dec+1, tmp.length()));
		if (decPos > len) {
			for(int i=0; i < len; i++) ret += "*";
			return ret;
		} else {
			for(int i=0; i < len - decPos; i++) ret += " ";
			return (ret + tmp.substring(0, decPos));
		}
	}

	String disp() {
		double dt1, dt2, dt3, t1, t2, t3, tmp;
		int c = 0;  // point C

		if(od < or) return "\n\tObstacle too big!\n";

		if((X0 == 0 & Y0 == 0) || q(X0*X0+Y0*Y0+(Z0-d1)*(Z0-d1)) >= a2+a3 ||
			q(X0*X0+Y0*Y0+(Z0-d1)*(Z0-d1)) <= a2-a3) {
			return "\n\tInvalid initial position.\n";

		}

		if((Xf == 0 & Yf == 0) || q(Xf*Xf+Yf*Yf+(Zf-d1)*(Zf-d1)) >= a2+a3 ||
			q(Xf*Xf+Yf*Yf+(Zf-d1)*(Zf-d1)) <= a2-a3) {
			return "\n\tInvalid final position.\n";
		}

		mode = 0; // verify initial and final points with mode 0

		// verify Initial position
		tmp = (X0*X0+Y0*Y0+(Z0-d1)*(Z0-d1)-a2*a2-a3*a3)/(2*a2*a3);
		t1 = a(X0, Y0);
		t3 = a(tmp, q(1-tmp*tmp));
		t2 = a(q(X0*X0+Y0*Y0), Z0-d1) - a(a2+a3*c(t3), a3*s(t3));

		planning(0,t1,t2,t3);

		if(t[0][3] != X0 || t[0][4] != Y0 || t[0][5] != Z0) {
			t3 = -t3;
			t2 = a(q(X0*X0+Y0*Y0), Z0-d1) - a(a2+a3*c(t3), a3*s(t3));
			planning(0,t1,t2,t3);
			if(t[0][3] != X0 || t[0][4] != Y0 || t[0][5] != Z0)
				return "\n\tInvalid initial position.\n";
		}

		// verify Final position
		tmp = (Xf*Xf+Yf*Yf+(Zf-d1)*(Zf-d1)-a2*a2-a3*a3)/(2*a2*a3);
		t1 = a(Xf, Yf);
		t3 = a(tmp, q(1-tmp*tmp));
		t2 = a(q(Xf*Xf+Yf*Yf), Zf-d1) - a(a2+a3*c(t3), a3*s(t3));

		planning(vn-1,t1,t2,t3);

		if(t[vn-1][3] != Xf || t[vn-1][4] != Yf || t[vn-1][5] != Zf) {
			t3 = -t3;
			t2 = a(q(Xf*Xf+Yf*Yf), Zf-d1) - a(a2+a3*c(t3), a3*s(t3));
			planning(vn-1,t1,t2,t3);
			if(t[vn-1][3] != Xf || t[vn-1][4] != Yf || t[vn-1][5] != Zf)
				return "\n\tInvalid final position.\n";
		}

		String txt = "The points sequence:\n\n";
		tmp = a(q(od*od-or*or), or);
		mode = (Zf*Z0 < 0 & (Math.abs(a(X0, Y0)) < tmp | Math.abs(a(Xf, Yf)) < tmp)) ? 1 : 0;

		// Plan the path
		t1 = t[0][0];
		t2 = t[0][1];
		t3 = t[0][2];
		tmp = Math.abs(t[0][0]);
		for(int i=0; i < vn-1; i++) {
			planning(i,t1,t2,t3);
			if (mode == 0) {
				if (Math.abs(t[i][0]) < tmp) {
					tmp = Math.abs(t[i][0]);
					c = i;
				}
			} else {
				if (Math.abs(t[i][0]) > tmp) {
					tmp = Math.abs(t[i][0]);
					c = i;
				}
			}
			t1 = t[i][0] + (t[vn-1][0]-t[i][0])/(vn-1-i);
			t2 = t[i][1] + (t[vn-1][1]-t[i][1])/(vn-1-i);
			t3 = t[i][2] + (t[vn-1][2]-t[i][2])/(vn-1-i);
		}

		// replan the path merely for smooth purpose
		t1 = t[c][0];
		t2 = t[c][1];
		t3 = t[c][2];
		for(int i=c; i > 0; i--) {
			planning(i,t1,t2,t3);
			t1 = t[i][0] + (t[0][0]-t[i][0])/i;
			t2 = t[i][1] + (t[0][1]-t[i][1])/i;
			t3 = t[i][2] + (t[0][2]-t[i][2])/i;
		}
			
		for(int i=0; i < vn; i++) {
			txt += "  " + Integer.toString(i);
			txt += "\t\t" + x(t[i][3], 8, 3) + x(t[i][4], 8, 3) + x(t[i][5], 8, 3);
			txt += (q((t[i][3]-od)*(t[i][3]-od)+t[i][4]*t[i][4]+t[i][5]*t[i][5]) < or) ? " *" : "";
			txt += "\t\t" + x(roundoff(t[i][0]*180/Math.PI), 8, 3);
			txt += x(roundoff(t[i][1]*180/Math.PI), 8, 3) + x(roundoff(t[i][2]*180/Math.PI), 8, 3)+"\n";
		}

		if(wh != null) wh.dispose();
		wh = new g3d(t, or, od, d1, a2);

		return txt;
	}

	// Standard obstacle avoidance criteria for a spheric obstacle
	void planning(int n, double t1, double t2, double t3) {
		double tmp = a(q(od*od-or*or), or);
		if (t1 > -tmp & t1 < tmp) {
			double r = q(or*or - Math.min(or*or, od*od*s(t1)*s(t1)));
			double d = od*c(t1);
			double D = q(d1*d1+d*d);
			double a = a(d, d1);
			double L2 = Math.abs(D*s(t2+a));
			if (L2 < r && q(D*D-L2*L2)-q(r*r-L2*L2) <= a2) {
				if (mode == 1) {
					tmp = a(q(od*od-or*or), or);
					for(; L2 < r && Math.abs(t1) <= tmp; 
						t1 += (t1 < 0 ? -Math.PI/180 : Math.PI/180)) {
						r = q(or*or - Math.min(or*or, od*od*s(t1)*s(t1)));
						d = od*c(t1);
						D = q(d1*d1+d*d);
						a = a(d, d1);
						L2 = Math.abs(D*s(t2+a));
					}
					t1 = t1 < 0 ? Math.max(-tmp, t1) : Math.min(tmp, t1);
				} else {
					tmp = a(q(D*D-r*r), r);
					t2 = t2 < -a ? -tmp-a : tmp-a;
					L2 = r;
				}
			}
			double P = q(a2*a2-2*a2*D*c(t2+a)+D*D);
			double b = a(L2, a2-D*c(t2+a));
			double L3 = P*c(Math.abs(t3)-b);
			if (L3 < r && q(P*P-L3*L3)-q(r*r-L3*L3) <= a3) {
				if (mode == 1) {
					tmp = a(q(od*od-or*or), or);
					for(; L3 < r && Math.abs(t1) <= tmp; 
						t1 += (t1 < 0 ? -Math.PI/180 : Math.PI/180)) {
						r = q(or*or - Math.min(or*or, od*od*s(t1)*s(t1)));
						d = od*c(t1);
						D = q(d1*d1+d*d);
						a = a(d, d1);
						L2 = Math.abs(D*s(t2+a));
						P = q(a2*a2-2*a2*D*c(t2+a)+D*D);
						b = a(L2, a2-D*c(t2+a));
						L3 = P*c(Math.abs(t3)-b);
					}
					t1 = t1 < 0 ? Math.max(-tmp, t1) : Math.min(tmp, t1);
				} else {
					tmp = a(r, q(P*P-r*r));
					t3 = t2 < -a ? tmp+b : -tmp-b;
					L3 = r;
				}
			}
		}
		t[n][0] = t1;
		t[n][1] = t2;
		t[n][2] = t3;
		t[n][3] = roundoff((a2*c(t2)+a3*c(t2+t3))*c(t1));
		t[n][4] = roundoff((a2*c(t2)+a3*c(t2+t3))*s(t1));
		t[n][5] = roundoff(a2*s(t2)+a3*s(t2+t3)+d1);
	}
}

class g3d extends Frame {

	double a = Math.PI/9, b = Math.PI/18;
	double[][] points;
	double r, d, d1, a2, scaler=30;

	Image buff;
	Graphics G;

	Scrollbar sbVer = new Scrollbar(Scrollbar.VERTICAL, (int)(b*180/Math.PI), 10, 0, 360);
	Scrollbar sbHor = new Scrollbar(Scrollbar.HORIZONTAL, (int)(a*180/Math.PI), 10, 0, 360);

	int w, h;

	g3d(double[][] data, double r, double d, double d1, double a2) {
		super("Obstacle Avoidance for Manipulators");

		add("East", sbVer);
		add("South", sbHor);
		resize(600, 500);
		setResizable(false);
		show();

		w = size().width-7-sbVer.size().width;
		h = size().height-26-sbHor.size().height;

		buff = createImage(w, h);
		G = buff.getGraphics();
		G.setFont(new Font("Courial", Font.PLAIN, 10));

		points = new double[data.length][6];

		for(int i=0; i<data.length; i++) {
			points[i][0] = data[i][0];
			points[i][1] = data[i][1];
			points[i][2] = data[i][2];
			points[i][3] = data[i][3];
			points[i][4] = data[i][4];
			points[i][5] = data[i][5];
		}

		this.r = r;
		this.d = d;
		this.d1=d1;
		this.a2=a2;
	}

	double s(double t) {return Math.sin(t);}
	double c(double t) {return Math.cos(t);}

	public boolean handleEvent(Event e) {
		if (e.id == e.WINDOW_DESTROY) {
			dispose();
			G.dispose();
			return true;
		}
		if (e.target == sbVer) {
			b = sbVer.getValue()*Math.PI/180;
			repaint();
			return true;
		}
		if (e.target == sbHor) {
			a = sbHor.getValue()*Math.PI/180;
			repaint();
			return true;
		}
		return false;
	}

	public void paint(Graphics g) {

		G.setColor(Color.white);
		G.fillRect(0, 0, w, h);
		// draw descriptive chars
		G.setColor(Color.red);
		G.drawString("H-angle: "+Integer.toString((int)(a*180/Math.PI)), 0, size().height-80);
		G.drawString("V-angle: "+Integer.toString((int)(b*180/Math.PI)), 0, 18);
		G.drawString("Z", w/2-2-(int)(d*scaler*s(b)*s(a)), h/2-(int)((d+1)*scaler*c(b)));
		G.drawString("Y", w/2+(int)((d+1)*scaler*c(a)), h/2+2+(int)(d*scaler*s(a)*s(b)));
		G.drawString("X", w/2-2-(int)((d+1)*scaler*s(a)), h/2+2+(int)((d+1)*scaler*s(b)));
		// draw frame axes
		G.setColor(Color.blue);
		G.drawLine(w/2, h/2, w/2-(int)(d*scaler*s(a)), h/2+(int)(d*scaler*s(b)));
		G.drawLine(w/2, h/2, w/2+(int)(d*scaler*c(a)), h/2+(int)(d*scaler*s(a)*s(b)));
		G.drawLine(w/2, h/2, w/2-(int)(d*scaler*s(b)*s(a)), h/2-(int)(d*scaler*c(b)));
		// draw obstacle
		G.setColor(Color.gray);
		G.drawOval(w/2-(int)((d*s(a)+r)*scaler), h/2+(int)((d*s(b)-r)*scaler), 
			(int)(2*r*scaler), (int)(2*r*scaler));
		// calculate projection
		int u = w/2+(int)((points[0][4]*c(a)-points[0][3]*s(a))*scaler);
		int v = h/2-(int)((points[0][5]*c(b)-points[0][3]*s(b))*scaler);
		int x, y, u1, v1, u2=0, v2=0;

		for(int i=0; i < points.length; i++) {

			x = w/2+(int)((points[i][4]*c(a)-points[i][3]*s(a))*scaler);
			y = h/2-(int)((points[i][5]*c(b)-points[i][3]*s(b))*scaler);
			// draw the pos of the end-effector
			G.setColor(Color.red);
			G.drawOval(x-1, y-1, 2, 2);
			// calculate the pos of the link joint
			u1 = w/2-(int)(d1*scaler*s(b)*s(a));
			v1 = h/2-(int)(d1*scaler*c(b));
			u2 = w/2+(int)(c(points[i][1])*s(points[i][0]-a)*a2*scaler);
			v2 = h/2-(int)(((a2*s(points[i][1])+d1)*c(b)-a2*c(points[i][1])*c(points[i][0])*s(b))*scaler);
			// draw link every other pos
			if(points.length <= 15 || (points.length > 15 && (i % 2) == 0)) {
	                G.setColor(Color.pink);
				G.drawLine(u1, v1, u2, v2);
				G.drawLine(u2, v2, x, y);
			}
			// draw trajectory
			G.setColor(Color.black);
			G.drawLine(u, v, x, y);
			u = x;
			v = y;
		}
		g.drawImage(buff, 0, 0, null);	
		buff.flush();
	}

	public void update(Graphics g) {
		paint(g);
	}
}