import lejos.nxt.LCD;
import lejos.nxt.Motor;

public class BraitenbergNavigator {

	private float wheel_distance;
	private float wheel_diameter;
	private Motor motorLeft;
	private Motor motorRight;
	private float x;
	private float y;
	private float angle;
	private float definitiveX;
	private float definitiveY;
	private float definitiveAngle;

	public BraitenbergNavigator(float wheel_distance, float wheel_diameter,
			Motor motorLeft, Motor motorRight) {
		this.wheel_distance = wheel_distance;
		this.wheel_diameter = wheel_diameter;
		this.motorLeft = motorLeft;
		this.motorLeft.resetTachoCount();
		this.motorRight = motorRight;
		this.motorRight.resetTachoCount();
	}

	public void stop() {
		motorLeft.stop();
		motorRight.stop();
		definitiveUpdatePosition();
	}

	public void driveBackward(int speed) {
		driveForward(-speed);
	}

	public void driveForward(int speed) {
		driveArc(speed, speed);
	}

	public void driveArc(int speedL, int speedR) {
		definitiveUpdatePosition();

		if (speedL > 0) {
			motorLeft.setSpeed(speedL);
			motorLeft.forward();
		} else {
			motorLeft.setSpeed(-speedL);
			motorLeft.backward();
		}

		if (speedR > 0) {
			motorRight.setSpeed(speedR);
			motorRight.forward();
		} else {
			motorRight.setSpeed(-speedR);
			motorRight.backward();
		}
	}

	private void definitiveUpdatePosition() {
		updatePosition();
		motorLeft.resetTachoCount();
		motorRight.resetTachoCount();
		definitiveX = x;
		definitiveY = y;
		definitiveAngle = angle;
	}

	private void updatePosition() {
		float t1 = (float) (Math.toRadians(motorLeft.getTachoCount())
				* wheel_diameter / 2);
		float t2 = (float) (Math.toRadians(motorRight.getTachoCount())
				* wheel_diameter / 2);
		if (((int) t1 == 0) && ((int) t2 == 0))
			return;

		float alpha = (t1 - t2) / wheel_distance;
		float r = 0;
		if (alpha != 0)
			r = (t1 + t2) / (2 * alpha);
		float relative_y = (float) (r * Math.sin(alpha));
		float relative_x = (float) (r * (1 - Math.cos(alpha)));
		x = (float) (definitiveX + Math.cos(-angle) * relative_x - Math
				.sin(-angle)
				* relative_y);
		y = (float) (definitiveY + Math.sin(-angle) * relative_x + Math
				.cos(-angle)
				* relative_y);
		angle = (float) Math.toRadians((int) Math.toDegrees(definitiveAngle
				+ alpha) % (360));
		LCD.drawInt((int) x, 3, 6, Utils.ROW_X);
		LCD.drawInt((int) y, 3, 6, Utils.ROW_Y);
		LCD.drawInt((int) Math.toDegrees(angle), 3, 6, Utils.ROW_ANGLE);
		LCD.refresh();
	}

	public float getX() {
		updatePosition();
		return x;
	}

	public float getY() {
		updatePosition();
		return y;
	}

	public float getAngle() {
		updatePosition();
		return angle;
	}

	public void goTo(float newX, float newY) throws InterruptedException {
		definitiveUpdatePosition();
		rotate(angleTo(getX(), getY(), newX, newY) - getAngle());
		float distance = distanceTo(getX(), getY(), newX, newY); // cm
		int speed = 500;
		driveForward(speed); // degrees per second
		Thread.sleep((int) (1000 * cmToDegrees(distance) / speed)); // milliseconds
		stop();

	}

	/**
	 * @param angle
	 *            in radians
	 */
	public void rotate(float angle) throws InterruptedException {
		definitiveUpdatePosition();
		float angledr = angle * wheel_distance / wheel_diameter;
		int speed = 500;
		driveArc(speed, -speed);
		Thread.sleep((int) (1000 * Math.abs(Math.toDegrees(angledr)) / speed));
		stop();
	}

	/**
	 * @return radians, absolute from N
	 */
	public static float angleTo(float fromX, float fromY, float toX, float toY) {
		float dx = toX - fromX;
		float dy = toY - fromY;
		return (float) (Math.PI / 2 - Math.atan2(dy, dx));
	}

	public static float distanceTo(float fromX, float fromY, float toX,
			float toY) {
		float dx = toX - fromX;
		float dy = toY - fromY;
		return (float) Math.sqrt(dx * dx + dy * dy);
	}

	private float cmToRadians(float cm) {
		return cm * 2 / wheel_diameter;
	}

	private float cmToDegrees(float cm) {
		return (float) Math.toDegrees(cmToRadians(cm));
	}
}