package cz.cuni.amis.pogamut.defcon.base3d.worldview.object;
   
   import cz.cuni.amis.pogamut.base3d.worldview.object.Location;
   
   /**
    * Checks and corrects coordinates for the ingame locations and provides useful
    * methods.
    * 
    * @author Radek 'Black_Hand' Pibil
   * 
   */
  public class DefConLocation extends Location {
  
  	public DefConLocation() {
  		super();
  	}
  
  	public DefConLocation(double x, double y) {
  		super(x, y);
  
  		if (y > 100d || y < -100d) {
  			throw new IllegalArgumentException(
  					"DefConLocation Y coord must be inside <-100, 100>");
  		}
  
  		correct();
  	}
  
  	public DefConLocation(float f[]) {
  		this(f[0], f[1]);
  	}
  
  	public DefConLocation(double d[]) {
  		this(d[0], d[1]);
  	}
  
  	public DefConLocation(Location location) {
  		this(location.getX(), location.getY());
  	}
  
  	@Override
  	public double getDistance2D(Location location) {
  		return getDistance(location);
  	}
  
  	@Override
  	public double getDistance(Location location) {
  
  		Location tmp = new Location();
  
  		double diff = this.x - location.x;
  
  		// diff > 180 => l1.x > l2.x
  		if (diff > 180d) {
  			tmp.x = (this.x - 360d) - location.x;
  		} else if (diff < -180d) { // diff < -180 => l1.x < l2.x
  			tmp.x = (this.x + 360d) - location.x;
  		} else {
  			tmp.x = this.x - location.x;
  		}
  
  		tmp.y = this.y - location.y;
  
  		// PogamutJBotSupport.writeToConsole("distancedfee: " + tmp);
  
  		return tmp.getLength();
  	}
  
  	public static Location sub(DefConLocation l1, DefConLocation l2) {
  		Location out = new Location();
  
  		double diff = l1.x - l2.x;
  
  		// diff > 180 => l1.x > l2.x
  		if (diff > 180d) {
  			out.x = (l1.x - 360d) - l2.x;
  		} else if (diff < -180d) { // diff < -180 => l1.x < l2.x
  			out.x = (l1.x + 360d) - l2.x;
  		} else {
  			out.x = l1.x - l2.x;
  		}
  
  		out.y = (l1.y - l2.y);
  
  		return out;
  	}
  
  
  	protected void correct() {
  		if (y > 100d || y < -100d) {
  			throw new IllegalArgumentException(
  					"DefConLocation Y coord must be inside <-100, 100>. " + y);
  		}
  
  		x = mathModulus(x + 180d, 360d) - 180d;
  	}
  
  	protected final static double mathModulus(double a, double b) {
  		return (a % b + b) % b;
 	}
 
 	@Override
 	public DefConLocation scale(double d) {
 		return new DefConLocation(super.scale(d));
 	}
 
 
 	/**
 	 * Retrieves sum of this location and given location.
 	 * 
 	 * @param l
 	 *            Location to be added to this location.
 	 * @return Sum of the two locations.
 	 */
 	@Override
 	public DefConLocation add(Location l) {
 
 		DefConLocation out = new DefConLocation();
 
 		double diff = x - l.x;
 
 		// diff > 180 => l.x > x
 		if (diff > 180d) {
 			out.x = (x + 360d) + l.x;
 		} else if (diff < -180d) { // diff < -180 => x > l.x
 			out.x = (x - 360d) + l.x;
 		} else {
 			out.x = x + l.x;
 		}
 
 		out.y = y + l.y;
 
 		out.correct();
 
 		return out;
 	}
 
 	/**
 	 * Retrieves sum of this location and given location.
 	 * 
 	 * @param l
 	 *            Location to be added to this location.
 	 * @return Sum of the two locations.
 	 */
 	public Location sub(DefConLocation l) {
 		return sub(this, l);
 	}
 
 	@Override
 	public DefConLocation getNormalized() {
 		return new DefConLocation(super.getNormalized());
 	}
 
 	/**
 	 * Calculates inverse Location
 	 * 
 	 * @return new inverted Location
 	 */
 	@Override
 	public DefConLocation invert() {
 		return new DefConLocation(-x, -y);
 	}
 
 	/**
 	 * Set content of this location from passed location.
 	 * 
 	 * @return this with newly set value
 	 */
 	@Override
 	public DefConLocation setTo(Location l) {
 		this.x = l.x;
 		this.y = l.y;
 		correct();
 
 		return this;
 	}
 
 	/**
 	 * Set content of this location from passed data.
 	 * 
 	 * @return this with newly set value
 	 */
 	@Override
 	public DefConLocation setTo(double x, double y, double z) {
 		return setTo(x, y);
 	}
 
 	/**
 	 * Set content of this location from passed data.
 	 * 
 	 * @return this with newly set value
 	 */
 	public DefConLocation setTo(double x, double y) {
 		this.x = x;
 		this.y = y;
 		correct();
 
 		return this;
 	}
 
 	/**
 	 * Calculates the square of the distance between this and given location.
 	 * 
 	 * @param l
 	 *            Location to be calculated the distance to.
 	 * @return Square of the euclidean distance between the two locations.
 	 */
 	@Override
 	public double getDistanceSquare(Location l) {
 
 		l = this.sub(l);
 
 		double dx = l.x;
 		double dy = l.y;
 		double dz = l.z;
 
 
 		return dx * dx + dy * dy + dz * dz;
 	}
 
 
 }