package edu.agh.tunev.interpolation;

import java.util.Map;
import java.util.NavigableMap;
import java.util.concurrent.ConcurrentHashMap;
import java.util.concurrent.ConcurrentSkipListMap;

import edu.agh.tunev.model.AbstractMovable;

public final class Interpolator {

	/**
	 * Zapisuje dyskretny stan w interpolatorze. -- m.
	 * 
	 * @param t
	 *            Dana chwila czasu dla jakiej zapisujemy stan.
	 */
	public void saveState(AbstractMovable movable, double t) {
		NavigableMap<Double, State> states = data.get(movable);
		if (states == null) {
			states = new ConcurrentSkipListMap<Double, State>();
			data.put(movable, states);
		}

		states.put(t, new State(movable));
	}

	public State getState(AbstractMovable movable, double t) {
		// TODO Auto-generated method stub
		return null;
	}

	public class State {
		public final double x;
		public final double y;

		public State(AbstractMovable movable) {
			this.x = movable.getX();
			this.y = movable.getY();
		}
	}

	public Interpolator() {
		data = new ConcurrentHashMap<AbstractMovable, NavigableMap<Double, State>>();
	}

	private final Map<AbstractMovable, NavigableMap<Double, State>> data;

}

@@ -49,24 +49,52 @@
 a.util.Map;%0A
+import java.util.Map.Entry;%0A
 import java.
@@ -108,24 +108,24 @@
 vigableMap;%0A
-
 import java.
@@ -772,56 +772,729 @@
 %7B%0A%09%09
-// TODO Auto-generated method stub%0A%09%09return null
+NavigableMap%3CDouble, State%3E states = data.get(movable);%0A%09%09if (states == null)%0A%09%09%09return null;%0A%0A%09%09Entry%3CDouble, State%3E prev = states.floorEntry(t);%0A%09%09Entry%3CDouble, State%3E next = states.ceilingEntry(t);%0A%09%09%0A%09%09if (prev == null && next == null)%0A%09%09%09return null;%0A%09%09else if (prev == null)%0A%09%09%09return next.getValue();%0A%09%09else if (next == null)%0A%09%09%09return prev.getValue();%0A%09%09else if (prev == next)%0A%09%09%09return prev.getValue();%0A%09%09%0A%09%09// splajny 1-go stopnia? bdzie git%0A%09%09%0A%09%09double ratio = (t - prev.getKey()) / (next.getKey() - prev.getKey());%0A%09%09%0A%09%09double x = prev.getValue().x + ratio * (next.getValue().x - prev.getValue().x);%0A%09%09double y = prev.getValue().y + ratio * (next.getValue().y - prev.getValue().y);%0A%09%09%0A%09%09return new State(x, y)
 ;%0A%09%7D
@@ -1638,16 +1638,16 @@
 getX();%0A
-
 %09%09%09this.
@@ -1670,16 +1670,90 @@
 ();%0A%09%09%7D%0A
+%09%09%0A%09%09public State(double x, double y) %7B%0A%09%09%09this.x = x;%0A%09%09%09this.y = y;%0A%09%09%7D%0A
 %09%7D%0A%0A%09pub

Interpolate positions