import java.awt.*;
import java.text.*;
import java.awt.event.*;
import java.util.Random;
import javax.swing.Timer;
import javax.media.j3d.*;
import javax.vecmath.*;
import com.sun.j3d.utils.universe.*; 
import com.sun.j3d.utils.geometry.Box;
import com.sun.j3d.utils.geometry.Cone;
import com.sun.j3d.utils.geometry.Cylinder;
import com.sun.j3d.utils.geometry.Sphere;
import com.sun.j3d.utils.behaviors.mouse.*;


public class CoalCart3D extends Frame implements KeyListener, ActionListener {

    static final long serialVersionUID = 1;


    //! True velocity of the cart [m/s]  (controlled by keyboard input)
    Vector3d vel = new Vector3d();
    //! True orientation velocity of the carts forward direction with respect
    //! to world X frame [rad/s] 		 (controlled by keyboard input)
    Vector3d velRot = new Vector3d();
    
    //! Objects to be drawn in j3D scene
    TransformGroup coalCart = new TransformGroup();
    TransformGroup planeCross = new TransformGroup();
    TransformGroup coalCartEst = new TransformGroup();
    
    //! Coal cart parameter
    CoalCartParameter parameter = new CoalCartParameter();    

    //! Scale used when starting the system.
    double defaultScale = 8;    
    
    //! Scale [pixel/m] used for rendering all graphics
    double scale = defaultScale;

    //! Margin around the workspace in pixel
    int margin = 10;    
    
    //! The random number generator used for simulation
    Random rnd = new Random ();

    UnscentedKalmanFilter3D ukf = new UnscentedKalmanFilter3D (parameter);


    //! Last gyro measurements
    Vector3d wBody = new Vector3d();

    //! Last accelerometer measurements
    Vector3d aBody = new Vector3d();
    

    //! Labels for showing text messages
    Label statusText, timeText;  
   
    //! Coal cart transform matrix and velocity [m/s] stored during last measurement
    Transform3D body2World = new Transform3D();
    Vector3d vWorldOld = new Vector3d();
    
    //! Time [s] since last measurement
    double timeSinceLastMeasurement;
    
    //! Time since last scene initialization
    double time;

    //************* Options initial settings

    //! Whether to make a measurement after each update step
    boolean autoMeasure = false;

    //! Whether to corrupt the simulated measurements
    boolean doUseNoise = false; 

    //! Whether to show the posterior distribution by simulated noise
    boolean doShowPosterior = false;    

    //! Whether to use a timer for continuous motion
    /*! If false the system works in single steps. */
    boolean doStepByStep = false;    

    //***********************************************
    
    //! Timer for animation of 3D scene
    private Timer timer;

    void printState ()
    {
	System.out.println ("autoMeasure:"+autoMeasure+ "; noise:"+doUseNoise+
                            "; doShowPosterior:"+doShowPosterior+"; doStepByStep:"+doStepByStep);	
    }    

    CoalCart3D ()
    {
        super ("Tracking a Coal Cart with an Extended Kalman Filter in 3D");
        setSize(780, 700);
		
        MenuItem mi;
        
	MenuBar menuBar = new MenuBar();
	setMenuBar (menuBar);
	
	Menu fileMenu = new Menu("File");
	menuBar.add (fileMenu);
	
	mi = new MenuItem ("Restart", new MenuShortcut(KeyEvent.VK_R));
	mi.addActionListener (new ActionListener() {
		public void actionPerformed(ActionEvent e) {
		    init();		    
		}		
	    });	
	fileMenu.add (mi);
	
	mi = new MenuItem ("Quit", new MenuShortcut(KeyEvent.VK_Q));
	mi.addActionListener (new ActionListener() {
		public void actionPerformed(ActionEvent e) {
		    System.exit  (0);		    
		}		
	    });	
	fileMenu.add (mi);
	
	Menu optionsMenu = new Menu("Options");
	menuBar.add (optionsMenu);
	
        CheckboxMenuItem cmi = new CheckboxMenuItem("Auto measure", autoMeasure);
	cmi.setShortcut (new MenuShortcut (KeyEvent.VK_A));	
        cmi.addItemListener (new ItemListener() {
        	public void itemStateChanged(ItemEvent e) {
        		CheckboxMenuItem tcmi = (CheckboxMenuItem) e.getSource();
        		autoMeasure = tcmi.getState();
			printState ();			
        	}
        });
        optionsMenu.add (cmi);

        cmi = new CheckboxMenuItem("Activate measurement noise", doUseNoise);
	cmi.setShortcut (new MenuShortcut (KeyEvent.VK_N));	
        cmi.addItemListener (new ItemListener() {
        	public void itemStateChanged(ItemEvent e) {
        		CheckboxMenuItem tcmi = (CheckboxMenuItem) e.getSource();
        		doUseNoise = tcmi.getState();
			printState ();			
        	}
        });
        optionsMenu.add (cmi);

        cmi = new CheckboxMenuItem("Show sampled posterior uncertainty", doShowPosterior);
	cmi.setShortcut (new MenuShortcut (KeyEvent.VK_U));	
        cmi.addItemListener (new ItemListener() {
        	public void itemStateChanged(ItemEvent e) {
        		CheckboxMenuItem tcmi = (CheckboxMenuItem) e.getSource();
        		doShowPosterior = tcmi.getState();
			printState ();			
        	}
        });
        optionsMenu.add (cmi);

        cmi = new CheckboxMenuItem("run step by step", doStepByStep);
	cmi.setShortcut (new MenuShortcut (KeyEvent.VK_P));	
        cmi.addItemListener (new ItemListener() {
        	public void itemStateChanged(ItemEvent e) {
        		CheckboxMenuItem tcmi = (CheckboxMenuItem) e.getSource();
			doStepByStep = tcmi.getState();			
			printState ();			
        	}
        });
        optionsMenu.add (cmi);
        
        
        Menu  checkMenu= new Menu("Debug");
        menuBar.add(checkMenu);
	
        mi = new MenuItem ("One timestep", new MenuShortcut(KeyEvent.VK_ENTER));
        mi.addActionListener (new ActionListener() {
            public void actionPerformed(ActionEvent e) {
		timeStep ();
            }		
	    });	
        checkMenu.add (mi);

        mi = new MenuItem ("Check scaledAxis <--> matrix conversion");
        mi.addActionListener (new ActionListener() {
            public void actionPerformed(ActionEvent e) {
            	if (ukf!=null) ukf.checkMatrixExponential ();
            }		
	    });	
        checkMenu.add (mi);

        mi = new MenuItem ("Check State.plus, State.minus");
        mi.addActionListener (new ActionListener() {
            public void actionPerformed(ActionEvent e) {
            	if (ukf!=null) ukf.checkStatePlusMinus ();
            }		
	    });	
        checkMenu.add (mi);

        mi = new MenuItem ("mean/covariance <--> sigma points conversion ");
        mi.addActionListener (new ActionListener() {
            public void actionPerformed(ActionEvent e) {
            	if (ukf!=null) ukf.checkSigmaPoints ();
            }		
	    });	
        checkMenu.add (mi);
        
        checkMenu.addSeparator();
        
        setLayout(new BorderLayout());
        
    	statusText = new Label();
	    statusText.setFont(new Font("Courier",Font.PLAIN,14));	
    	add (statusText, "South");
    	
    	timeText = new Label("", Label.RIGHT);
    	timeText.setFont(new Font("Courier", Font.PLAIN, 14));
    	timeText.setBackground(new Color(0.0f, 0.0f, 0.0f, 0.0f));
    	timeText.setForeground(Color.WHITE);
    	add(timeText, "North");

    	addKeyListener(this);
        
    	// set up 3D canvas for drawing the 3d scene
        GraphicsConfiguration config = SimpleUniverse.getPreferredConfiguration();
        Canvas3D canvas3D = new Canvas3D(config);
        add("Center", canvas3D);
        canvas3D.addKeyListener(this);
        // create 3D world
        SimpleUniverse simpleU = new SimpleUniverse(canvas3D);
        BranchGroup scene = createSceneGraph(simpleU);
        simpleU.addBranchGraph(scene);
        // set viewpoint
        ViewingPlatform vp = simpleU.getViewingPlatform();
        Transform3D tf = new Transform3D();
        tf.rotX(Math.PI/2.0);
        tf.setTranslation(new Vector3f(0.0f, -12.0f, 1.0f));
        vp.getViewPlatformTransform().setTransform(tf);
        
        // init all object properties
        init();
        
        //start timer
        timer = new Timer((int) (parameter.deltaT*1000), this);
        timer.start();
        
        setVisible(true);
        
    } // end of CoalCart3D constructor
    
    public TransformGroup build3DAxes(float size) {	

		float axisSize = size;
		//X
		ColoringAttributes xAxisColor = new ColoringAttributes(0.85f, 0.0f, 0.0f, ColoringAttributes.SHADE_FLAT);
		Appearance xAxisAp = new Appearance();
		xAxisAp.setColoringAttributes(xAxisColor);
		TransformGroup xAxis = new TransformGroup();
		xAxis.addChild(new Cylinder(0.0025f * axisSize + 0.003f, axisSize, xAxisAp));
		TransformGroup xAxisCap = new TransformGroup();
		xAxisCap.addChild(new Cone(0.015f * axisSize, 0.05f * axisSize, xAxisAp));
		Transform3D tf = new Transform3D();
		tf.setTranslation(new Vector3d(0.0, axisSize/2.0f, 0.0));
		xAxisCap.setTransform(tf);
		xAxis.addChild(xAxisCap);
		tf.setIdentity();
		tf.setRotation(new AxisAngle4d(0.0, 0.0, 1.0, -Math.PI/2.0));
		xAxis.setTransform(tf);
		//Y
		ColoringAttributes yAxisColor = new ColoringAttributes(0.0f, 0.78f, 0.0f, ColoringAttributes.SHADE_FLAT);
		Appearance yAxisAp = new Appearance();
		yAxisAp.setColoringAttributes(yAxisColor);
		TransformGroup yAxis = new TransformGroup();
		yAxis.addChild(new Cylinder(0.0025f * axisSize + 0.003f, axisSize, yAxisAp));
		TransformGroup yAxisCap = new TransformGroup();
		yAxisCap.addChild(new Cone(0.015f * axisSize, 0.05f * axisSize, yAxisAp));
		yAxis.addChild(yAxisCap);
		tf.setIdentity();
		tf.setTranslation(new Vector3d(0.0, axisSize/2.0f, 0.0));
		yAxisCap.setTransform(tf);
		//Z
		ColoringAttributes zAxisColor = new ColoringAttributes(0.0f, 0.0f, 0.8f, ColoringAttributes.SHADE_FLAT);
		Appearance zAxisAp = new Appearance();
		zAxisAp.setColoringAttributes(zAxisColor);
		TransformGroup zAxis = new TransformGroup();
		zAxis.addChild(new Cylinder(0.0025f * axisSize + 0.003f, axisSize, zAxisAp));
		TransformGroup zAxisCap = new TransformGroup();
		zAxisCap.addChild(new Cone(0.015f * axisSize, 0.05f * axisSize, zAxisAp));
		zAxis.addChild(zAxisCap);
		tf.setIdentity();
		tf.setTranslation(new Vector3d(0.0, axisSize/2.0f, 0.0));
		zAxisCap.setTransform(tf);
		tf.setIdentity();
		tf.setRotation(new AxisAngle4d(1.0, 0.0, 0.0, Math.PI/2.0));
		zAxis.setTransform(tf);
		
		TransformGroup axes = new TransformGroup();
		axes.addChild(xAxis);
		axes.addChild(yAxis);
		axes.addChild(zAxis);
		
    	return axes;
    }
    
    public TransformGroup createChequerBoard(int nx, int ny, float quadSize) {
    	
    	TransformGroup cb = new TransformGroup();
    	float quadSizeHalf = quadSize/2.0f;
    	double xOff = -(nx-1) * quadSizeHalf;
    	double yOff = -(ny-1) * quadSizeHalf;
    	
    	Appearance apA = new Appearance();
    	Appearance apB = new Appearance();
    	ColoringAttributes colorA = new ColoringAttributes(0.7f, 0.76f, 0.45f, ColoringAttributes.SHADE_FLAT);
    	//ColoringAttributes colorA = new ColoringAttributes(0.7539f, 0.8984f, 0.4258f, ColoringAttributes.SHADE_FLAT);
    	apA.setColoringAttributes(colorA);
    	ColoringAttributes colorB = new ColoringAttributes(0.825f, 0.875f, 0.586f, ColoringAttributes.SHADE_FLAT);
    	//ColoringAttributes colorB = new ColoringAttributes(0.7539f, 0.8789f, 0.4258f, ColoringAttributes.SHADE_FLAT);
    	apB.setColoringAttributes(colorB);
    	Transform3D tf = new Transform3D();
    	
    	for (int y = 0; y < ny; y++) {
    		for(int x = 0; x < nx; x++) {
    			double centerX = x * quadSize + xOff;
    			double centerY = y * quadSize + yOff;
    			TransformGroup quad = new TransformGroup();
    			//alternating colors
    			if (x % 2 == y % 2) {
    				quad.addChild(new Box(quadSizeHalf, quadSizeHalf, 0.001f, apA));
    			} else {
    				quad.addChild(new Box(quadSizeHalf, quadSizeHalf, 0.001f, apB));
    			}
    			tf.setIdentity();
    			tf.setTranslation(new Vector3d(centerX, centerY, 0.0));
    			quad.setTransform(tf);
    			cb.addChild(quad);		
    		}
    	}
    	
    	return cb;
    	
    }
	
	public BranchGroup createSceneGraph(SimpleUniverse su) {
		
		// Create the root of branch graph
		BranchGroup objRoot = new BranchGroup();
        TransformGroup obj = new TransformGroup();
		objRoot.addChild(obj);
		
		/* set up mouse interaction */
		
        TransformGroup vpTrans = su.getViewingPlatform().getViewPlatformTransform();
        BoundingSphere mouseBounds = new BoundingSphere(new Point3d(), 1000.0);

        MouseRotate myMouseRotate = new MouseRotate(MouseBehavior.INVERT_INPUT);
        myMouseRotate.setTransformGroup(vpTrans);
        myMouseRotate.setSchedulingBounds(mouseBounds);
        myMouseRotate.setFactor(0.001);
        objRoot.addChild(myMouseRotate);

        MouseTranslate myMouseTranslate = new MouseTranslate(MouseBehavior.INVERT_INPUT);
        myMouseTranslate.setTransformGroup(vpTrans);
        myMouseTranslate.setSchedulingBounds(mouseBounds);
        objRoot.addChild(myMouseTranslate);

        MouseZoom myMouseZoom = new MouseZoom(MouseBehavior.INVERT_INPUT);
        myMouseZoom.setTransformGroup(vpTrans);
        myMouseZoom.setSchedulingBounds(mouseBounds);
        objRoot.addChild(myMouseZoom);
	
        /* build 3D scene */
        
        double planeSize = 12.0;
		//global scale is a double, use a float scale for local methods
		scale = planeSize / parameter.workspaceLength;

		//chequerboard plane with 100m quads
		TransformGroup chequerBoardPlane = createChequerBoard(20, 20, (float)(100 * scale));
		obj.addChild(chequerBoardPlane);
		
		//world axes
		TransformGroup worldAxes = build3DAxes((float)planeSize / 4.0f);
		obj.addChild(worldAxes);
		
		//lights
		DirectionalLight dl = new DirectionalLight();
        dl.setDirection(0.0f, 0.0f, -1.0f);
        AmbientLight al = new AmbientLight();
        BoundingBox bb = new BoundingBox(new Point3d(-planeSize*4, -planeSize*4, -planeSize*4),
        								 new Point3d( planeSize*4,  planeSize*4,  planeSize*4));
        dl.setInfluencingBounds(bb);
        al.setInfluencingBounds(bb);
        //move lights upwards
        TransformGroup lights = new TransformGroup();
        lights.addChild(dl);
        lights.addChild(al);
        Transform3D ltf = new Transform3D();
        ltf.setTranslation(new Vector3d(0.0, 0.0, 40.0));
        lights.setTransform(ltf);
        objRoot.addChild(lights);
		
	//coal cart
        Appearance coalCartAp = new Appearance();
	coalCartAp.setMaterial(new Material());
	//short part ("wing")
	TransformGroup wing = new TransformGroup();
	wing.addChild(new Sphere(1.0f, Sphere.GENERATE_NORMALS, coalCartAp));
	Transform3D tfWing = new Transform3D();
	tfWing.setScale(new Vector3d(parameter.cartWidth * scale, 
				     parameter.cartLength * scale * 0.3, 
				     parameter.cartHeight * scale * 0.5));
	wing.setTransform(tfWing);
	coalCart.addChild(wing);
	//long part ("fuselage") shifted along y-axis
	TransformGroup fuselage = new TransformGroup();
	fuselage.addChild(new Sphere(1.0f, Sphere.GENERATE_NORMALS, coalCartAp));
	Transform3D tfFuselage = new Transform3D();
	tfFuselage.setScale(new Vector3d(parameter.cartWidth  * scale * 0.4, 
					 parameter.cartLength * scale, 
					 parameter.cartHeight * scale));
	tfFuselage.setTranslation(new Vector3d(0.0, -parameter.cartLength * scale * 0.25, 0.0));
	fuselage.setTransform(tfFuselage);
	coalCart.addChild(fuselage);
	//tail ("rudder")
	TransformGroup rudder = new TransformGroup();
	rudder.addChild(new Sphere(1.0f, Sphere.GENERATE_NORMALS, coalCartAp));
	Transform3D tfRudder = new Transform3D();
	tfRudder.rotX(-0.2);
	tfRudder.setScale(new Vector3d(parameter.cartWidth  * scale * 0.05, 
				       parameter.cartLength * scale * 0.1, 
				       parameter.cartHeight * scale * 0.8));
	tfRudder.setTranslation(new Vector3d( 0.0, 
					      -parameter.cartLength * scale * 1.1, 
					      parameter.cartHeight * scale));
	rudder.setTransform(tfRudder);
	coalCart.addChild(rudder);
	coalCart.setCapability(TransformGroup.ALLOW_TRANSFORM_WRITE);
	coalCart.setCapability(TransformGroup.ALLOW_TRANSFORM_READ); 
	obj.addChild(coalCart);
	
	// cross object on plane indicating coalCart's position
	Appearance crossAp = new Appearance();
	crossAp.setColoringAttributes(new ColoringAttributes(0.1f, 0.1f, 0.1f, ColoringAttributes.SHADE_FLAT));
	planeCross.addChild(new Box((float) (parameter.cartWidth * scale * 0.4),
				    (float) (parameter.cartWidth * scale * 0.1),
				    (float) (parameter.cartWidth * scale * 0.05),
				    crossAp));
	planeCross.addChild(new Box((float) (parameter.cartWidth * scale * 0.1),
				    (float) (parameter.cartWidth * scale * 0.4),
				    (float) (parameter.cartWidth * scale * 0.05),
				    crossAp));
	planeCross.setCapability(TransformGroup.ALLOW_TRANSFORM_WRITE);
	planeCross.setCapability(TransformGroup.ALLOW_TRANSFORM_READ); 
	obj.addChild(planeCross);
	
	// estimated coal cart
        Appearance coalCartEstAp = new Appearance();
	coalCartAp.setMaterial(new Material());
	TransparencyAttributes ta = new TransparencyAttributes(TransparencyAttributes.NICEST, parameter.estCoalCartTransparency);
	coalCartEstAp.setTransparencyAttributes(ta);
	//short part ("wing")
	TransformGroup wingEst = new TransformGroup();
	wingEst.addChild(new Sphere(1.0f, Sphere.GENERATE_NORMALS, coalCartEstAp));
	wingEst.setTransform(tfWing);
	coalCartEst.addChild(wingEst);
	//long part ("fuselage") shifted along z-axis
	TransformGroup fuselageEst = new TransformGroup();
	fuselageEst.addChild(new Sphere(1.0f, Sphere.GENERATE_NORMALS, coalCartEstAp));
	fuselageEst.setTransform(tfFuselage);
	coalCartEst.addChild(fuselageEst);
	//tail ("rudder")
	TransformGroup rudderEst = new TransformGroup();
	rudderEst.addChild(new Sphere(1.0f, Sphere.GENERATE_NORMALS, coalCartEstAp));
	rudderEst.setTransform(tfRudder);
	coalCartEst.addChild(rudderEst);
	//estimate axes
	TransformGroup estAxes = build3DAxes((float) (parameter.cartWidth * scale * 3));
	coalCartEst.addChild(estAxes);
	coalCartEst.setCapability(TransformGroup.ALLOW_TRANSFORM_WRITE);
	coalCartEst.setCapability(TransformGroup.ALLOW_TRANSFORM_READ); 
	obj.addChild(coalCartEst);
	
	/* end of 3D scene build */
	
        // Let Java 3D perform optimizations on this scene graph.
        objRoot.compile();
	
        return objRoot;
	} // end of createSceneGraph method


    public void init ()
    {
    	//coal cart does not move at the beginning
		vel    = new Vector3d(0.0, 0.0, 0.0);
		velRot = new Vector3d(0.0, 0.0, 0.0);
    	
	   	// calculate initial coalCart position
		body2World.setIdentity();
    	body2World.setTranslation(new Vector3d(parameter.initialCartX * scale, 
    										   parameter.initialCartY * scale, 
    										   parameter.initialCartZ * scale));
    	coalCart.setTransform(body2World);
    	coalCartEst.setTransform(body2World);
    	vWorldOld = new Vector3d(0.0, 0.0, 0.0);
    	timeSinceLastMeasurement = 0.0;
    	time = 0.0;
    	updateTimeText();
    	
    	ukf.initialize(parameter);
		
		repaint ();
    }

    //! Move the transparent coal cart showing the (sampled) estimate to the new position
    public void updateEstimate ()
    {
	//draw estimated position/orientation
	Transform3D tmpEst = new Transform3D();
	if (ukf.muX != null) {
	    Matrix3d m3d;
	    if (doShowPosterior) m3d = new Matrix3d(ukf.drawFromPosterior().body2World.getRowPackedCopy());	    
	    else m3d = new Matrix3d(ukf.muX.body2World.getRowPackedCopy());
	    tmpEst.setRotation(m3d);
	}
    	Transform3D new2World = new Transform3D();
    	coalCart.getTransform(new2World);
	Vector3d newPos = new Vector3d();
	new2World.get(newPos);
	tmpEst.setTranslation (newPos);
	coalCartEst.setTransform(tmpEst);
	
	repaint ();	
    }
    
    
    //! Move coal cart by \c t seconds
    /*! Does not update the inertial measurements nor call the UKF nor update the estimate. */
    public void move(double t)
    {
	// Update the true position according to exact motion
    	Transform3D new2World = new Transform3D();
    	coalCart.getTransform(new2World);
    	
    	//calc current angle for simulation of coal cart y-rotation
    	Matrix3d rotation = new Matrix3d();
    	new2World.get(rotation);
    	AxisAngle4d oldRot = new AxisAngle4d();
    	oldRot.set(rotation);
    	
    	//calc new transform matrix
    	Transform3D tf = new Transform3D();
    	//simulate rotation around world-Z by the given body-y-orientation (aircraft-like)
	if (vel.y > 0.0) {
	    Matrix3d tf3d = new Matrix3d();
	    tf3d.setIdentity();		    
	    Vector3d down = new Vector3d(0,0,-1);
	    Matrix3d rotationInv = new Matrix3d (rotation);
	    rotationInv.invert();
	    rotationInv.transform(down); 
	    // down is in body coordinates
	    tf3d.rotZ(-0.03 * down.x);	
	    tf3d.mul(rotation);
	    new2World.setRotation(tf3d);		
    	}
		
	//add rotX and rotZ
    	tf.rotX(velRot.x * t);
    	new2World.mul(tf);
    	tf.rotY(velRot.y * t);
    	new2World.mul(tf);
    	//add translation
    	tf.setIdentity();
    	tf.setTranslation(new Vector3d(0.0, vel.y * t * scale, 0.0));    	
    	new2World.mul(tf);
    	//apply new transform matrix to coal cart
    	coalCart.setTransform(new2World);
    	
    	//set cross on plane exactly below coalCart
	tf.setIdentity();
	Vector3d newPos = new Vector3d();
	new2World.get(newPos);
	tf.setTranslation(new Vector3d(newPos.x, newPos.y, 0.0));
	planeCross.setTransform(tf);
	
	//update time for measure()
	timeSinceLastMeasurement += t;
    }

    // result += trans^{-1}*(v,0)
    void addInverseTransformedFreeVector (Vector3d result, Transform3D trans, Vector3d v)
    {
	Matrix3d qInv = new Matrix3d ();
	trans.get(qInv);
	qInv.invert ();
	Vector3d hv = new Vector3d(v);
	qInv.transform (hv);
	result.add (hv);	
    }
    

    //! Computes the measurements of the inertial sensor form old and new pose/velocity
    /*! The angular velocity and translational acceleration are computed and stored in
      \c wBody and \c aBody but not (yet) passed to the UKF.
    */
    public void computeInertialSensorMeasurements ()
    {
	double deltaT = timeSinceLastMeasurement;
    	// avoid division by zero 
	//(usually measure() would not be called if timeSinceLastMeasurement == 0)
	if (deltaT == 0.0) deltaT = 0.00001;
	
    	//current transform matrix
	Transform3D new2World = new Transform3D();
	coalCart.getTransform(new2World);
	
	//get transform matrix from last measurement
	Transform3D old2World = new Transform3D(body2World);
	//Inv(old2world) * new2world -> new2Old
	Transform3D new2Old = new Transform3D(old2World);
	new2Old.invert();
	new2Old.mul(new2World);	
	
	Matrix3d rotation = new Matrix3d();
	AxisAngle4d new2OldRot = new AxisAngle4d();
	new2Old.get(rotation);
	new2OldRot.set(rotation);
	
	wBody.x = new2OldRot.x * new2OldRot.angle / deltaT;
	wBody.y = new2OldRot.y * new2OldRot.angle / deltaT;
	wBody.z = new2OldRot.z * new2OldRot.angle / deltaT;
	
	
	//calculate new velocity
	Vector3d transNew = new Vector3d();
	new2World.get(transNew);
	transNew.scale(1.0/scale);	//invert drawing scale
	
	Vector3d transOld = new Vector3d();
	old2World.get(transOld);
	transOld.scale(1.0/scale);	//invert drawing scale
	
	Vector3d vWorldNew = new Vector3d();
	vWorldNew.x = (transNew.x - transOld.x) / deltaT;
	vWorldNew.y = (transNew.y - transOld.y) / deltaT;
	vWorldNew.z = (transNew.z - transOld.z) / deltaT;
	
	//calc acceleration in world coordinates
	Vector3d aWorld = new Vector3d();
	aWorld.x = (vWorldNew.x - vWorldOld.x) / deltaT - parameter.gravity[0];
	aWorld.y = (vWorldNew.y - vWorldOld.y) / deltaT - parameter.gravity[1];
	aWorld.z = (vWorldNew.z - vWorldOld.z) / deltaT - parameter.gravity[2];
	//transform to (old) body coordinate system 
	aBody = new Vector3d();
	addInverseTransformedFreeVector (aBody, old2World, aWorld);
	addInverseTransformedFreeVector (aBody, new2World, aWorld);
	aBody.scale (0.5);
	
	//update transform matrix and velocity used for the last measurement
	body2World = new Transform3D(new2World);
	vWorldOld = new Vector3d(vWorldNew);
	
	if (doUseNoise) {	    
	    // Rotational velocity
	    wBody.x += parameter.dynamicNoiseSigma*rnd.nextGaussian();
	    wBody.y += parameter.dynamicNoiseSigma*rnd.nextGaussian();
	    wBody.z += parameter.dynamicNoiseSigma*rnd.nextGaussian();
	    // Acceleration
	    aBody.x += parameter.measurementNoiseSigma*rnd.nextGaussian();
	    aBody.y += parameter.measurementNoiseSigma*rnd.nextGaussian();
	    aBody.z += parameter.measurementNoiseSigma*rnd.nextGaussian();	
	}

	//debug
	System.out.println("Time since last measurement: " + deltaT + " s");		
	System.out.println("aBody: " + aBody.toString());
	System.out.println("wBody: " + wBody.toString());
	System.out.println("");	
    	
	//reset time since last measurement
	timeSinceLastMeasurement = 0.0;
    }
    
    void clearMeasurement ()
    {
	aBody = new Vector3d();
	wBody = new Vector3d();	
    }

    //! Let one timestep pass
    /*! Executes the simulation dynamics, computes inertial
	measurements, calls the UKFs dynamic step with the angular
	velocity, calls the UKFs measurement step with the
	acceleration (if \c autoMeasure==true) and updates
	the estimate for visualization.
    */
    void timeStep ()
    {
	move(parameter.deltaT);
	computeInertialSensorMeasurements ();
	if (ukf!=null) {
	    ukf.dynamic (wBody.x, wBody.y, wBody.z);
	    if (autoMeasure) ukf.measurement (aBody.x, aBody.y, aBody.z);
	}
	time += parameter.deltaT;
	updateEstimate ();	
	updateStatusText ();
	updateTimeText();
    }
    
    void updateStatusText ()
    {
	String txt;
	DecimalFormat format = new DecimalFormat("+00.00;-00.00");
	txt = "w:(" + format.format(wBody.x) +" ," + format.format(wBody.y) +" ,"+format.format(wBody.z)+"); ";
	txt = txt+"a:(" + format.format(aBody.x) +" ," + format.format(aBody.y) +" ,"+format.format(aBody.z)+"); ";
	if (ukf!=null && ukf.sigmaX!=null) {
	    double ux = 180/Math.PI*Math.sqrt(ukf.sigmaX.get(0,0));
	    double uy = 180/Math.PI*Math.sqrt(ukf.sigmaX.get(1,1));
	    double uz = 180/Math.PI*Math.sqrt(ukf.sigmaX.get(2,2));	    
	    txt = txt+"uncertainty: "
		+format.format(ux)+"° /"
		+format.format(uy)+"° /"
		+format.format(uz)+"°";
	}       
	statusText.setText (txt);	
    }
    
    void updateTimeText()
    {
    	String txt;
    	DecimalFormat hm = new DecimalFormat("00");
    	DecimalFormat s = new DecimalFormat("00.000");
    	int hours = (int) (time / 3600);
    	int minutes = (int) (time / 60);
    	double seconds = time - hours - minutes;
    	txt = hm.format(hours) + ":" + hm.format(minutes) + ":" + s.format(seconds) + " ";
    	timeText.setText (txt);	
    }
    
    
    
	public void keyTyped(KeyEvent arg0) {
		// TODO Auto-generated method stub

	}

	public void keyPressed(KeyEvent evt) {
		int key = evt.getKeyCode();  // keyboard code for the key that was pressed
	    double rotationStep = 0.25;
	    if (key == KeyEvent.VK_LEFT) {
	    	velRot.y -= rotationStep;  
	    }      
	    else if (key == KeyEvent.VK_RIGHT) {
	    	velRot.y += rotationStep;	  
	    }
	    else if (key == KeyEvent.VK_UP) {
	    	velRot.x += rotationStep;  
	    }
	    else if (key == KeyEvent.VK_DOWN) {
	    	velRot.x -= rotationStep;  
	    }
	    else if (key == KeyEvent.VK_S) {
	    	vel.y -= 10.0;  
	    }
	    else if (key == KeyEvent.VK_W) {
	    	vel.y += 10.0;  
	    }
	    else if (key == KeyEvent.VK_A) {
		autoMeasure = !autoMeasure;
		printState ();		
	    }	    
	    else if (key == KeyEvent.VK_N) {
		doUseNoise = !doUseNoise;
		printState ();		
	    }	    
	    else if (key == KeyEvent.VK_U) {
		doShowPosterior = !doShowPosterior;
		printState ();		
	    }	    
	    else if (key == KeyEvent.VK_P) {
		doStepByStep = !doStepByStep;
		printState ();
	    }	    
	    else if (key == KeyEvent.VK_ENTER) {
		timeStep ();
	    }	    
	    else if (key == KeyEvent.VK_SPACE) {
	    	if (ukf!=null) ukf.measurement (aBody.x, aBody.y, aBody.z);
	    }
	}

	public void keyReleased(KeyEvent arg0) {
		// TODO Auto-generated method stub

	}
	
	public void actionPerformed(ActionEvent evt) {
	    if (!doStepByStep) timeStep ();
	    updateEstimate ();	    
	    updateStatusText ();		
	}
	
	public static void main(String[] args) 
    {
        
    	CoalCart3D cc3d = new CoalCart3D();
    	cc3d.addWindowListener(new java.awt.event.WindowAdapter() {
            public void windowClosing(WindowEvent winEvt) {
                System.exit(0); 
            }
        });
    }

}
