package edu.agh.tunev.model;

import java.awt.geom.Point2D;
import java.util.Map;
import java.util.Map.Entry;
import java.util.NavigableMap;
import java.util.concurrent.ConcurrentHashMap;
import java.util.concurrent.ConcurrentSkipListMap;

public final class Interpolator {

	public static class PersonState {
		public final Point2D.Double position;
	
		public PersonState(AbstractPerson person) {
			position = person.getPosition();
		}
		
		public PersonState(Point2D.Double position) {
			this.position = position;
		}
	}

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

		states.put(t, new Interpolator.PersonState(person));
	}

	public Interpolator.PersonState getState(AbstractPerson person, double t) {
		NavigableMap<Double, Interpolator.PersonState> states = data.get(person);
		if (states == null)
			return null;

		final Entry<Double, Interpolator.PersonState> prev = states.floorEntry(t);
		final Entry<Double, Interpolator.PersonState> next = states.ceilingEntry(t);
		
		if (prev == null && next == null)
			return null;
		else if (prev == null)
			return next.getValue();
		else if (next == null)
			return prev.getValue();
		else if (prev == next)
			return prev.getValue();
		
		// splajny 1-go stopnia? będzie git
		
		final double ratio = (t - prev.getKey()) / (next.getKey() - prev.getKey());

		final Point2D.Double p = prev.getValue().position;
		final Point2D.Double n = next.getValue().position;

		final double px = p.getX();
		final double py = p.getY();
		final double nx = n.getX();
		final double ny = n.getY();
		
		final double x = px + ratio * (nx - px);
		final double y = py + ratio * (ny - py);
		
		return new Interpolator.PersonState(new Point2D.Double(x, y));
	}

	public Interpolator() {
		data = new ConcurrentHashMap<AbstractPerson, NavigableMap<Double, Interpolator.PersonState>>();
	}

	private final Map<AbstractPerson, NavigableMap<Double, Interpolator.PersonState>> data;

}

@@ -343,16 +343,100 @@
 sition;%0A
+%09%09public final double orientation;%0A%09%09public final AbstractPerson.Movement movement;%0A
 %09%0A%09%09publ
@@ -517,88 +517,284 @@
 ;%0A%09%09
-%7D%0A%09%09%0A%09%09public PersonState(Point2D.Double position) %7B%0A%09%09%09this.position = position
+%09orientation = person.getOrientation();%0A%09%09%09movement = person.getMovement();%0A%09%09%7D%0A%09%09%0A%09%09public PersonState(Point2D.Double position, double orientation, AbstractPerson.Movement movement) %7B%0A%09%09%09this.position = position;%0A%09%09%09this.orientation = orientation;%0A%09%09%09this.movement = movement
 ;%0A%09%09
@@ -1510,16 +1510,21 @@
 te%3E prev
+Entry
  = state
@@ -1592,16 +1592,21 @@
 te%3E next
+Entry
  = state
@@ -1629,32 +1629,37 @@
 );%0A%09%09%0A%09%09if (prev
+Entry
  == null && next
@@ -1654,24 +1654,29 @@
 null && next
+Entry
  == null)%0A%09%09
@@ -1700,24 +1700,29 @@
 lse if (prev
+Entry
  == null)%0A%09%09
@@ -1729,24 +1729,29 @@
 %09return next
+Entry
 .getValue();
@@ -1766,16 +1766,21 @@
 if (next
+Entry
  == null
@@ -1787,32 +1787,37 @@
 )%0A%09%09%09return prev
+Entry
 .getValue();%0A%09%09e
@@ -1832,16 +1832,21 @@
 prev
+Entry
  == next
 )%0A%09%09
@@ -1841,16 +1841,21 @@
  == next
+Entry
 )%0A%09%09%09ret
@@ -1858,24 +1858,29 @@
 %09return prev
+Entry
 .getValue();
@@ -1879,16 +1879,204 @@
 Value();
+%0A%0A%09%09final PersonState prev = prevEntry.getValue();%0A%09%09final PersonState next = nextEntry.getValue();%0A%09%09%0A%09%09final double prevT = prevEntry.getKey();%0A%09%09final double nextT = nextEntry.getKey();
 %0A%09%09%0A%09%09//
@@ -2140,24 +2140,29 @@
  = (t - prev
+Entry
 .getKey()) /
@@ -2163,24 +2163,29 @@
 y()) / (next
+Entry
 .getKey() - 
@@ -2184,24 +2184,29 @@
 Key() - prev
+Entry
 .getKey());%0A
@@ -2238,27 +2238,16 @@
  = prev.
-getValue().
 position
@@ -2284,19 +2284,8 @@
 ext.
-getValue().
 posi
@@ -2565,16 +2565,49 @@
 le(x, y)
+, prev.orientation, prev.movement
 );%0A%09%7D%0A%0A%09

Add these properties to PersonState