import java.awt.BorderLayout;
import java.awt.Color;
import java.awt.Dimension;
import java.awt.Graphics;
import java.awt.Graphics2D;
import java.awt.RenderingHints;
import java.awt.event.ComponentEvent;
import java.awt.event.ComponentListener;
import java.awt.font.TextLayout;
import java.awt.geom.AffineTransform;
import java.awt.geom.GeneralPath;
import java.awt.geom.Rectangle2D;
import java.text.DecimalFormat;
import java.util.Random;

import javax.swing.JFrame;
import javax.swing.JPanel;
import javax.swing.JSlider;
import javax.swing.event.ChangeEvent;
import javax.swing.event.ChangeListener;

import Jama.Matrix;

/**
 * @author knorton
 */
public class Kalman extends JPanel implements ComponentListener, Runnable, ChangeListener {
	
	private boolean			_active = true;
	private Random			_random;
	private int				_xscale = 1000;
	
	private float			_t = 0f;
	private float			_dt = 1f;
	
	private float[]			_ymeas = new float[_xscale];
	private GeneralPath		_pmeas = new GeneralPath();
	private float[]			_ypred = new float[_xscale];
	private GeneralPath		_ppred = new GeneralPath();
	
	private int				_yix = 0;
	private int				_yln	 = 0;
	
	private Matrix			_E;
	private Matrix			_I;
	private Matrix			_A;
	private Matrix			_X;
	
	private float			_dx;
	private AffineTransform	_tx;
	
	private double			_stdy = 0.1;
	private double			_stdx = 1e-3;
	
	public Kalman() {
		this._random = new Random();
		this.setBackground(Color.WHITE);
		this.addComponentListener(this);

		
		_E = new Matrix(new double[][] {{1., 0.},{0., 1.}});
		_I = Matrix.identity(2,2);
		_A = new Matrix(new double[][] {{2.,-1.},{1., 0.}});
		_X = new Matrix(new double[][] {{.1}, {0.}});
		
		new Thread(this).start();		
	}
	
	private float yn(float t) {
		return (float)(Math.sin(0.1*t + 4*Math.sin(0.01*t)) + _stdy*_random.nextGaussian());
	}
	
	private void printMatrix(Matrix m) {
		System.out.print("[");
		for (int i=0;i<m.getRowDimension();i++) {
			System.out.print("[");
			for (int j=0;j<m.getColumnDimension();j++) {
				if (j != 0)
					System.out.print(",");
				System.out.print(m.get(i,j));
			}
			System.out.print("]");
		}
		System.out.println("]");
	}
	
	public void run() {
		while (_active) {
			
			float x = (float)_X.get(0,0);
			
			float ymeas = yn(_t);
			float ypred = (float)Math.sin(x);
			
			Matrix B = new Matrix(new double[][] {{Math.cos(x), 0.}});
			
			Matrix K = _E.times(B.transpose()).times(
					1. / (B.times(_E).times(B.transpose()).get(0,0) + _stdy*_stdy));
			
			_X = _X.plus(K.times(ymeas-ypred));
			_E = (_I.minus(K.times(B))).times(_E);
			_X = _A.times(_X);
			_E = _A.times(_E).times(_A.transpose()).plus(_I.times(_stdx*_stdx));

			//update values
			if (_yln < _ymeas.length) {
				_ymeas[_yln] = ymeas;
				_ypred[_yln++] = ypred;
			}
			else {
				_ymeas[_yix] = ymeas;
				_ypred[_yix++] = ypred;
				_yix %= _ymeas.length;
			}
			_t += _dt;
			
			//create paths (little bit of a race condition here, but fuq it)
			GeneralPath p;
			p = new GeneralPath();
			if (_yln > 0) {
				p.moveTo(0f,_ymeas[_yix]);
				for (int i=1;i<this._yln;i++)
					p.lineTo(i*_dx,_ymeas[(i+_yix)%_ymeas.length]);
				_pmeas = p;
			}
			
			p = new GeneralPath();
			if (_yln > 0) {
				p.moveTo(0f,_ypred[_yix]);
				for (int i=1;i<this._yln;i++)
					p.lineTo(i*_dx,_ypred[(i+_yix)%_ypred.length]);
				_ppred = p;
			}
			
			//_active = false;
			//post & yield for paint
			this.repaint();
			try {
				Thread.sleep(15);
			}
			catch (InterruptedException e) {	
			}
		}
	}
	
	public void paintComponent(Graphics g) {
		super.paintComponent(g);
		Graphics2D gg = (Graphics2D)g;
		gg.setRenderingHint(
				RenderingHints.KEY_ANTIALIASING,
				RenderingHints.VALUE_ANTIALIAS_ON);

		TextLayout		tl = new TextLayout(
				"stdx = " + new DecimalFormat("0.00E0").format(_stdx),this.getFont(),gg.getFontRenderContext());
		Rectangle2D		tr = tl.getBounds();
		tl.draw(gg,10f,(float)tr.getHeight()+10f);
		
		_pmeas.transform(_tx);
		g.setColor(Color.RED);
		gg.draw(_pmeas);

		_ppred.transform(_tx);		
		g.setColor(Color.BLUE);
		gg.draw(_ppred);
	}
	
	public void componentResized(ComponentEvent arg0) {
		_dx = (float)this.getWidth()/(float)this._xscale;
		_tx = AffineTransform.getTranslateInstance(0,this.getHeight()>>1);
		_tx.scale(1,(float)this.getHeight()/4f);
	}

	public void stateChanged(ChangeEvent e) {
		JSlider 	s = (JSlider)e.getSource();
		_stdx = (float)Math.pow(10f,-(float)s.getValue()/10f);
	}
	
	public void componentHidden(ComponentEvent arg0) {
	}
	
	public void componentMoved(ComponentEvent arg0) {
	}
		
	public void componentShown(ComponentEvent arg0) {
	}
	
	static class KalmanNoiseSlider extends JSlider {
		public KalmanNoiseSlider() {
			super(JSlider.VERTICAL,10,50,30);
			this.setBackground(Color.WHITE);
			this.setMajorTickSpacing(10);
			this.setMinorTickSpacing(5);
			this.setPaintTicks(true);
		}
		
		public void paintComponent(Graphics g) {
			g.setColor(Color.WHITE);
			g.fillRect(0,0,this.getWidth(),this.getHeight());
			super.paintComponent(g);
		}
	}
	
	public static void main(String[] args) {
		JFrame		fr = new JFrame();
		Kalman		kp = new Kalman();
		
		KalmanNoiseSlider
					js = new KalmanNoiseSlider();
		js.addChangeListener(kp);
		
		kp.setPreferredSize(new Dimension(800,300));
		fr.getContentPane().setLayout(new BorderLayout());
		fr.getContentPane().add(kp,BorderLayout.CENTER);
		fr.getContentPane().add(js,BorderLayout.WEST);
		fr.pack();
		fr.show();
	}
}
