/**
    * This source code file is part of a direct port of Stan Birchfield's implementation
    * of a Kanade-Lucas-Tomasi feature tracker. The original implementation can be found
    * here: http://www.ces.clemson.edu/~stb/klt/
    *
    * As per the original code, the source code is in the public domain, available
    * for both commercial and non-commercial use.
    */
   package org.openimaj.video.tracking.klt;
  
  import java.io.DataInput;
  import java.io.DataOutput;
  import java.io.DataOutputStream;
  import java.io.IOException;
  import java.io.PrintWriter;
  import java.util.Scanner;
  
  import org.openimaj.math.geometry.point.Point2d;
  
  import Jama.Matrix;
  
  /**
   * A tracked feature
   *
   * @author Stan Birchfield
   * @author Jonathon Hare (jsh2@ecs.soton.ac.uk)
   */
  public class Feature implements Point2d, Cloneable {
  	/**
  	 * x ordinate of feature
  	 */
  	public float x;
  
  	/**
  	 * y ordinate of feature
  	 */
  	public float y;
  
  	/**
  	 * value of feature
  	 */
  	public int val;
  
  	/* for affine mapping */
  	// public FImage aff_img;
  	// public FImage aff_img_gradx;
  	// public FImage aff_img_grady;
  	// public float aff_x;
  	// public float aff_y;
  	// public float aff_Axx;
  	// public float aff_Ayx;
  	// public float aff_Axy;
  	// public float aff_Ayy;
  
  	/**
  	 * Convert to string representation with the given format
  	 * 
  	 * @param format
  	 * @param type
  	 * @return formatted string
  	 */
  	public String toString(String format, String type) {
  		assert (type.equals("f") || type.equals("d"));
  		String s = "";
  
  		if (type.equals("f"))
  			s += String.format(format, x, y, val);
  		else if (type.equals("d")) {
  			/* Round x & y to nearest integer, unless negative */
  			float _x = x;
  			float _y = y;
  			if (_x >= 0.0)
  				_x += 0.5;
  			if (_y >= 0.0)
  				_y += 0.5;
  			s += String.format(format, (int) x, (int) y, val);
  		}
  
  		return s;
  	}
  
  	/**
  	 * Write feature as binary data
  	 * 
  	 * @param os
  	 * @throws IOException
  	 */
  	public void writeFeatureBin(DataOutputStream os) throws IOException {
  		os.writeFloat(x);
  		os.writeFloat(y);
  		os.writeInt(val);
  	}
  
  	@Override
  	public float getX() {
  		return x;
  	}
  
  	@Override
 	public float getY() {
 		return y;
 	}
 
 	@Override
 	public void setX(float x) {
 		this.x = x;
 	}
 
 	@Override
 	public void setY(float y) {
 		this.y = y;
 	}
 
 	@Override
 	public Feature clone() {
 		final Feature f = new Feature();
 
 		f.x = x;
 		f.y = y;
 		f.val = val;
 		// f.aff_img = aff_img;
 		// f.aff_img_gradx = aff_img_gradx;
 		// f.aff_img_grady = aff_img_grady;
 		// f.aff_x = aff_x;
 		// f.aff_y = aff_y;
 		// f.aff_Axx = aff_Axx;
 		// f.aff_Ayx = aff_Ayx;
 		// f.aff_Axy = aff_Axy;
 		// f.aff_Ayy = aff_Ayy;
 
 		return f;
 	}
 
 	@Override
 	public boolean equals(Object o) {
 		if (this == o)
 			return true;
 		if (!(o instanceof Feature))
 			return false;
 
 		if (((Feature) o).x == x && ((Feature) o).y == y && ((Feature) o).val == val)
 			return true;
 		return false;
 	}
 
 	@Override
 	public int hashCode() {
 		int hash = 17;
 		hash = (int) ((31 * hash) + x);
 		hash = (int) ((31 * hash) + y);
 		hash = ((31 * hash) + val);
 		// hash = (int) ((31 * hash) + aff_img;
 		// hash = (int) ((31 * hash) + aff_img_gradx;
 		// hash = (int) ((31 * hash) + aff_img_grady;
 		// hash = (int) ((31 * hash) + aff_x;
 		// hash = (int) ((31 * hash) + aff_y;
 		// hash = (int) ((31 * hash) + aff_Axx;
 		// hash = (int) ((31 * hash) + aff_Ayx;
 		// hash = (int) ((31 * hash) + aff_Axy;
 		// hash = (int) ((31 * hash) + aff_Ayy;
 
 		return hash;
 	}
 
 	@Override
 	public String toString() {
 		return "Feature(" + x + ", " + y + ", " + val + ")";
 	}
 
 	@Override
 	public void copyFrom(Point2d p)
 	{
 		setX(p.getX());
 		setY(p.getY());
 	}
 
 	@Override
 	public Float getOrdinate(int dimension) {
 		if (dimension == 0)
 			return x;
 		return y;
 	}
 
 	@Override
 	public int getDimensions() {
 		return 2;
 	}
 
 	@Override
 	public void translate(float x, float y) {
 		this.x += x;
 		this.y += y;
 	}
 
 	@Override
 	public Feature transform(Matrix transform) {
 		if (transform.getRowDimension() == 3) {
 			float xt = (float) transform.get(0, 0) * getX() + (float) transform.get(0, 1) * getY()
 					+ (float) transform.get(0, 2);
 			float yt = (float) transform.get(1, 0) * getX() + (float) transform.get(1, 1) * getY()
 					+ (float) transform.get(1, 2);
 			final float zt = (float) transform.get(2, 0) * getX() + (float) transform.get(2, 1) * getY()
 					+ (float) transform.get(2, 2);
 
 			xt /= zt;
 			yt /= zt;
 
 			final Feature f = this.clone();
 			f.x = xt;
 			f.y = yt;
 			return f;
 		} else if (transform.getRowDimension() == 2) {
 			final float xt = (float) transform.get(0, 0) * getX() + (float) transform.get(0, 1) * getY();
 			final float yt = (float) transform.get(1, 0) * getX() + (float) transform.get(1, 1) * getY();
 
 			final Feature f = this.clone();
 			f.x = xt;
 			f.y = yt;
 			return f;
 		}
 		throw new IllegalArgumentException("Transform matrix has unexpected size");
 	}
 
 	@Override
 	public Point2d minus(Point2d a) {
 		final Point2d p = this.clone();
 		p.setX(this.getX() - a.getX());
 		p.setY(this.getY() - a.getY());
 		return p;
 	}
 
 	@Override
 	public void readASCII(Scanner in) throws IOException {
 		x = in.nextFloat();
 		y = in.nextFloat();
 		val = in.nextInt();
 	}
 
 	@Override
 	public String asciiHeader() {
 		return this.getClass().getName();
 	}
 
 	@Override
 	public void readBinary(DataInput in) throws IOException {
 		x = in.readFloat();
 		y = in.readFloat();
 		val = in.readInt();
 	}
 
 	@Override
 	public byte[] binaryHeader() {
 		return this.getClass().getName().getBytes();
 	}
 
 	@Override
 	public void writeASCII(PrintWriter out) throws IOException {
 		out.format("%f %f %d", x, y, val);
 	}
 
 	@Override
 	public void writeBinary(DataOutput out) throws IOException {
 		out.writeFloat(x);
 		out.writeFloat(y);
 		out.writeInt(val);
 	}
 
 	@Override
 	public void translate(Point2d v) {
 		this.translate(v.getX(), v.getY());
 	}
 
 	@Override
 	public Feature copy() {
 		return clone();
 	}
 
 	@Override
 	public void setOrdinate(int dimension, Number value) {
 		if (dimension == 0)
 			x = value.floatValue();
 		if (dimension == 1)
 			y = value.floatValue();
 	}
 }