package classes;

import java.awt.Color;
import java.awt.Graphics2D;

public class HolonBody implements Comparable<HolonBody> {

	public Vector2d velocity;
	public Vector2d position;
	private float mass;
	private float radius;
	private float angularVel;
	private float orientation = 45f;
	private Color color;

	public HolonBody(float x, float y, float radius, float mass, Color color) {
		this.velocity = new Vector2d(0, 0);
		this.position = new Vector2d(x, y);
		this.setMass(mass);
		this.setRadius(radius);
		this.color = color;
	}

	
	public void draw(Graphics2D g2) {

		g2.setColor(color);
		g2.fillOval((int) (position.getX() - getRadius()), (int) (position.getY() - getRadius()),
				(int) (2 * getRadius()), (int) (2 * getRadius()));

	}

	public void setRadius(float radius) {
		this.radius = radius;
	}

	public float getRadius() {
		return radius;
	}

	public boolean colliding(HolonBody body) {
		float xd = position.getX() - body.position.getX();
		float yd = position.getY() - body.position.getY();

		float sumRadius = getRadius() + body.getRadius();
		float sqrRadius = sumRadius * sumRadius;

		float distSqr = (xd * xd) + (yd * yd);

		if (distSqr <= sqrRadius) {
			return true;
		}

		return false;

	}

	public void resolveCollision(HolonBody body) {

		// get the mtd
		Vector2d delta = (position.subtract(body.position));
		float r = getRadius() + body.getRadius();
		float dist2 = delta.dot(delta);

		if (dist2 > r * r)
			return; // they aren't colliding

		float d = delta.getLength();

		Vector2d mtd;
		if (d != 0.0f) {
			// minimum translation distance to push bodies apart after
			// intersecting
			mtd = delta.multiply(((getRadius() + body.getRadius()) - d) / d);

		} else {
			// Special case. Bodies are exactly on top of eachother. Don't want
			// to divide by zero.
			d = body.getRadius() + getRadius() - 1.0f;
			delta = new Vector2d(body.getRadius() + getRadius(), 0.0f);

			mtd = delta.multiply(((getRadius() + body.getRadius()) - d) / d);
		}

		// resolve intersection
		float im1 = 1 / getMass(); // inverse mass quantities
		float im2 = 1 / body.getMass();

		// push-pull them apart
		position = position.add(mtd.multiply(im1 / (im1 + im2)));
		body.position = body.position.subtract(mtd.multiply(im2 / (im1 + im2)));

		// impact speed
		Vector2d v = (this.velocity.subtract(body.velocity));
		float vn = v.dot(mtd.normalize());

		// sphere intersecting but moving away from each other already
		if (vn > 0.0f)
			return;

		// collision impulse
		float i = (-(1.0f + Constants.restitution) * vn) / (im1 + im2);
		Vector2d impulse = mtd.multiply(i);

		// change in momentum
		this.velocity = this.velocity.add(impulse.multiply(im1));
		body.velocity = body.velocity.subtract(impulse.multiply(im2));

	}

	private void setMass(float mass) {
		this.mass = mass;
	}

	private float getMass() {
		return mass;
	}

	public int compareTo(HolonBody body) {
		if (this.position.getX() - this.getRadius() > body.position.getX() - body.getRadius()) {
			return 1;
		} else if (this.position.getX() - this.getRadius() < body.position.getX() - body.getRadius()) {
			return -1;
		} else {
			return 0;
		}
	}

}
