import java.awt.*;
import javax.swing.*;
import java.util.Vector;
import javax.media.j3d.*;
import com.sun.j3d.utils.geometry.*;
import javax.vecmath.*;
import javax.vecmath.Vector4d;
import java.util.*;


/**
 *
 *
 */
public class DHJoint
{
	public double theta = 0;
	public double d = 0;
	public double a = 0;
	public double alpha = 0;
	public String type = "Rotational";
	public double var = 0;
	public double min = 0;
	public double max = 0;
//	public BranchGroup bg;
	public TransformGroup tgt = new TransformGroup();

	public DHPoint point; /// location
	public Vector childs;
	public DHJoint parent;

	/**
	 * DHJoint
	 *
	 * @param d
	 * @param theta
	 * @param alpha
	 * @param a
	 */
	public DHJoint (double d, double theta, double alpha, double a)
	{
		this.d  = d;
		this.theta = theta;
		this.alpha = alpha;
		this.a = a;
		this.tgt.setCapability(TransformGroup.ALLOW_TRANSFORM_WRITE);
	}

	public DHJoint(String line)
	{
		String[] cols = line.split(" ");
		for (int i=0;i<cols.length;i++) {
			switch (i) {
				case 0:
					this.theta = this.parseDouble(cols[i]);
					break;
				case 1:
					this.d = this.parseDouble(cols[i]);
					break;
				case 2:
					this.a = this.parseDouble(cols[i]);
					break;
				case 3:
					this.alpha = this.parseDouble(cols[i]);
					break;
				case 4:
					this.type = this.parseType(cols[i]);
					break;
				case 5:
					this.min = this.parseDouble(cols[i]);
					break;
				case 6:
					this.max = this.parseDouble(cols[i]);
					break;
				case 7:
					this.var = this.constrain(this.parseDouble(cols[i]));
					break;
			}
		}
		this.tgt.setCapability(TransformGroup.ALLOW_TRANSFORM_WRITE);
		//setTransformGroup();
//		System.out.println(this);
	}
	/*
	public void setTransformGroup()
	{
        this.tg.setCapability(TransformGroup.ALLOW_TRANSFORM_WRITE);
        Transform3D t3d = new Transform3D();
        t3d.setTranslation(new Vector3d(0.2f,0.3f,0.5f));
        this.tg.setTransform(t3d);
        this.tg.addChild(new ColorCube(0.1f));
	}
	*/


	public double sin(double a)
	{
		return Math.sin(Math.toRadians(a));
	}

	public double cos(double a)
	{
		return Math.cos(Math.toRadians(a));
	}


	public Transform3D getTransForm3D()
	{
		double d = this.d;
		double theta = this.theta;

		if (type.equals("Rotational")) {
			theta = this.theta + var;
		} else
		if (type.equals("Prismatic")) {
			d = this.d + var;
		}

		Matrix3d m3d = new Matrix3d(
			cos(theta), -cos(alpha) * sin(theta), sin(alpha) * sin(theta),
			sin(theta), cos(alpha) * cos(theta), -sin(alpha) * cos(theta),
			0, sin(alpha), cos(alpha)
		);

		Vector3d v3d = new Vector3d(
			a * cos(theta),
			a * sin(theta),
			d
		);

		Transform3D t3d = new Transform3D(m3d, v3d, 1);
		return t3d;
	}

	public BranchGroup getBranchGraph()
	{
		/*
		//set all transformations
		Matrix3d rotTheta = new Matrix3d(
				Math.cos(Math.toRadians(this.theta)),-Math.sin(Math.toRadians(this.theta)),0,
				Math.sin(Math.toRadians(this.theta)),Math.cos(Math.toRadians(this.theta)),0,
				0,0,1);
		Matrix3d rotAlpha = new Matrix3d(
				1,0,0,
				0,Math.cos(Math.toRadians(this.alpha)),-Math.sin(Math.toRadians(this.alpha)),
				0,Math.sin(Math.toRadians(this.alpha)),Math.cos(Math.toRadians(this.alpha)));
		Vector3d transD = new Vector3d(0,0,this.d);
		Vector3d transA = new Vector3d(this.a,0,0);

		//execute transformations
		Transform3D tr = new Transform3D();
		tr.setRotation(rotTheta);	//theta == rot om z(n-1)
		tr.setTranslation(transD);  //d 	== trans over z(n-1)
		tr.setTranslation(transA);  //a     == trans over z(n)
		tr.setRotation(rotAlpha);   //alpha == rot over z(n)
		*/

		Transform3D t3d = this.getTransForm3D();

		//update transGroup, enzo

		tgt.setTransform(t3d);
		tgt.addChild(new ColorCube(0.5f));

		//update branchGroup
		BranchGroup bg = new BranchGroup();
		bg.addChild(tgt);
		DHJoint next;
		if (this.childs != null) {
			next = (DHJoint) this.childs.get(0);
			tgt.addChild(next.getBranchGraph());
		}
		bg.compile();
		return bg;
	}

	public void adjust(double d)
	{
		this.var = this.constrain(d);
		tgt.setTransform(this.getTransForm3D());
	}

	public void propagate(String p)
	{
//		String t[] = p.trim().split(" ");
	}

	public double parseDouble(String val)
	{
		double d;
		try {
			d = Double.parseDouble(val);
		} catch(NumberFormatException e) {
			d = 0;
		}
		return d;
	}

	public String parseType(String val)
	{
		String v = val.toLowerCase();
		if (v.startsWith("r")) {
			v = "Rotational";
		} else
		if (v.startsWith("t") || v.startsWith("p")) {
			v = "Prismatic";
		} else
		if (v.startsWith("c")) {
			v = "Complex";
		} else
		if (v.startsWith("s")) {
			v = "Spherical";
		} else
		if (v.startsWith("w")) {
			v = "Weld";
		}
		return v;
	}

	/**
	 * Add a joint as a child
	 *
	 * @param j
	 */
	public void add(DHJoint j)
	{
		j.parent = this;
		if (childs == null) {
			childs = new Vector();
		}
		this.childs.add(j);
	}

	/**
	 * Add a joint as a limb
	 *
	 * @param j
	 */
	public void addToEnd(DHJoint j)
	{
		DHJoint c = this;
		while (c.childs != null) {
			c = (DHJoint) c.childs.get(0);
		}
		c.add(j);
	}

	/**
	 * get
	 */
	public Vector getPath()
	{
		Vector v = new Vector();
		DHJoint p = this;
		while (p.parent != null) {
			p = p.parent;
			v.add(p);
		}
		return v;
	}

	public double constrain(double v)
	{
		if (v > this.max) {
			return max;
		} else
		if (v < this.min) {
			return min;
		}
		return v;
	}

	/// interface

	public JPanel getPanel()
	{
		JTextField f = new JTextField();
		f.setText(Double.toString(this.var));
		f.setSize(100,20);
		f.addActionListener(
			new CommandAdjust("f", f, this)
		);

		JButton bp2 = new JButton("++");
		bp2.addActionListener(
			new CommandAdjust("++", f, this)
		);

		JButton bp = new JButton("+");
		bp.addActionListener(
			new CommandAdjust("+", f, this)
		);

		JButton bm = new JButton("-");
		bm.addActionListener(
			new CommandAdjust("-", f, this)
		);

		JButton bm2 = new JButton("--");
		bm2.addActionListener(
			new CommandAdjust("--", f, this)
		);

		JPanel p = new JPanel();
		p.setLayout(new GridLayout(1,9));
		p.add(new JLabel(this.type + " "));
		p.add(new JLabel("Min: " + this.min));
		p.add(new JLabel("Max: " + this.max));
		p.add(new JLabel("Var: "));
		p.add(f);
		p.add(bp2);
		p.add(bp);
		p.add(bm);
		p.add(bm2);
		return p;
	}

	public JPanel getPanels()
	{
		JPanel p = new JPanel();
		p.setLayout(new FlowLayout());
		DHJoint c = this;
		p.add(c.getPanel());
		while (c.childs != null) {
			c = (DHJoint) c.childs.get(0);
			p.add(c.getPanel());
		}
		return p;
	}

	/**
	 * toString()
	 */
	public String toString()
	{
		String t = "";
		t += "theta: " + this.theta + " ";
		t += "d: " + this.d + " ";
		t += "a: " + this.a+ " ";
		t += "alpha: " + this.alpha + " ";
		t += "type : " + this.type+ " ";
		t += "min : " + this.min+ " ";
		t += "max : " + this.max+ " ";
		t += "var : " + this.var+ " ";
		return t;
	}
}