/*
 *                    BioJava development code
 *
 * This code may be freely distributed and modified under the
 * terms of the GNU Lesser General Public Licence.  This should
 * be distributed with the code.  If you do not have a copy,
 * see:
 *
 *      http://www.gnu.org/copyleft/lesser.html
 *
 * Copyright for this code is held jointly by the individual
 * authors.  These should be listed in @author doc comments.
 *
 * For more information on the BioJava project and its aims,
 * or to join the biojava-l mailing list, visit the home page
 * at:
 *
 *      http://www.biojava.org/
 *
 */
package org.biojava.nbio.structure.align.util;

import java.io.StringWriter;
import java.util.Locale;

import javax.vecmath.AxisAngle4d;
import javax.vecmath.Matrix3d;
import javax.vecmath.Matrix4d;
import javax.vecmath.Vector3d;

import org.biojava.nbio.structure.Atom;
import org.biojava.nbio.structure.AtomImpl;
import org.biojava.nbio.structure.Calc;
import org.biojava.nbio.structure.StructureException;
import org.biojava.nbio.structure.align.model.AFPChain;
import org.biojava.nbio.structure.contact.Pair;
import org.biojava.nbio.structure.geometry.Matrices;
import org.biojava.nbio.structure.jama.Matrix;

/**
 * Calculates the rotation axis for an alignment
 *
 * <p>A superposition of two structures is generally represented as a rotation
 * matrix plus a translation vector. However, it can also be represented as an
 * axis of rotation plus some translation.
 *
 * <p>This class calculates the rotation axis and stores it as four properties:
 * <ul><li>A unit vector parallel to the rotation axis ({@link #getRotationAxis()})
 * <li>The angle of rotation ({@link #getAngle()})
 * <li>A point on the rotation axis ({@link #getRotationPos()})
 * <li>Some translation parallel to the axis ({@link #getScrewTranslation()})
 * </ul>
 *
 * <p>The axis of rotation is poorly defined and numerically unstable for small
 * angles. Therefore it's direction is left as null for angles less than
 * {@link #MIN_ANGLE}.
 *
 * @author Spencer Bliven
 *
 */
public final class RotationAxis {

	/**
	 * Minimum angle to calculate rotation axes. 5 degrees.
	 */
	static final double MIN_ANGLE = Math.toRadians(5.);

	private double theta;
	private Atom rotationAxis; // axis of rotation
	private Atom rotationPos; // a point on the axis of rotation
	private Atom screwTranslation; //translation parallel to the axis of rotation
	private Atom otherTranslation; // translation perpendicular to the axis of rotation

	/**
	 * The rotation angle
	 * @return the angle, in radians
	 */
	public double getAngle() {
		return theta;
	}

	/**
	 * Get a unit vector along the rotation axis
	 * @return rotationAxis
	 */
	public Atom getRotationAxis() {
		return rotationAxis;
	}

	/**
	 * Returns the rotation axis and angle in a single javax.vecmath.AxisAngle4d object
	 * @return
	 */
	public AxisAngle4d getAxisAngle4d() {
		return new AxisAngle4d(rotationAxis.getX(),rotationAxis.getY(),rotationAxis.getZ(),theta);
	}

	/**
	 * Get a position on the rotation axis.
	 *
	 * Specifically, project the origin onto the rotation axis
	 * @return rotationPos
	 */
	public Atom getRotationPos() {
		return rotationPos;
	}

	/**
	 * Get the component of translation parallel to the axis of rotation
	 * @return
	 */
	public Atom getScrewTranslation() {
		return screwTranslation;
	}

	public Vector3d getVector3dScrewTranslation() {
		return new Vector3d(screwTranslation.getX(),screwTranslation.getY(),screwTranslation.getZ());
	}

	public double getTranslation() {
		return Calc.amount(screwTranslation);
	}

	/**
	 * Calculate the rotation axis for the first block of an AFPChain
	 * @param afpChain
	 * @throws StructureException
	 * @throws NullPointerException if afpChain does not contain a valid rotation matrix and shift vector
	 */
	public RotationAxis(AFPChain afpChain) throws StructureException {
		if(afpChain.getAlnLength() < 1) {
			throw new StructureException("No aligned residues");
		}
		init(afpChain.getBlockRotationMatrix()[0],afpChain.getBlockShiftVector()[0]);
	}

	/**
	 * Create a rotation axis from a vector, a point, and an angle.
	 *
	 * The result will be a pure rotation, with no screw component.
	 * @param axis A vector parallel to the axis of rotation
	 * @param pos A point on the axis of rotation
	 * @param theta The angle to rotate (radians)
	 */
	public RotationAxis(Atom axis, Atom pos, double theta) {
		this.rotationAxis = Calc.unitVector(axis);
		this.rotationPos = (Atom) pos.clone();
		this.theta = theta;
		this.screwTranslation = new AtomImpl(); //zero
		this.otherTranslation = null; //deprecated
	}

	/**
	 * Determine the location of the rotation axis based on a rotation matrix and a translation vector
	 * @param rotation
	 * @param translation
	 */
	public RotationAxis(Matrix rotation, Atom translation) {
		init(rotation, translation);
	}

	/**
	 * Create a rotation axis from a Matrix4d containing a rotational
	 * component and a translational component.
	 *
	 * @param transform
	 */
	public RotationAxis(Matrix4d transform) {

		Matrix rot = Matrices.getRotationJAMA(transform);
		Atom transl = Calc.getTranslationVector(transform);
		init(rot,transl);
	}

	/**
	 * Get the rotation matrix corresponding to this axis
	 * @return A 3x3 rotation matrix
	 */
	public Matrix getRotationMatrix() {
		return getRotationMatrix(theta);
	}

	/**
	 * Get the rotation matrix corresponding to a rotation about this axis
	 * @param theta The amount to rotate
	 * @return A 3x3 rotation matrix
	 */
	public Matrix getRotationMatrix(double theta) {
		if( rotationAxis == null) {
			// special case for pure translational axes
			return Matrix.identity(3, 3);
		}
		double x = rotationAxis.getX();
		double y = rotationAxis.getY();
		double z = rotationAxis.getZ();
		double cos = Math.cos(theta);
		double sin = Math.sin(theta);
		double com = 1 - cos;
		return new Matrix(new double[][] {
				{com*x*x + cos, com*x*y+sin*z, com*x*z+-sin*y},
				{com*x*y-sin*z, com*y*y+cos, com*y*z+sin*x},
				{com*x*z+sin*y, com*y*z-sin*x, com*z*z+cos},
		});
	}

	/**
	 * Returns the rotation order o that gives the lowest value of {@code |2PI / o - theta},
	 * given that the value is strictly lower than {@code threshold}, for orders {@code o=1,...,maxOrder}.
	 */
	public int guessOrderFromAngle(double threshold, int maxOrder) {
		double bestDelta = threshold;
		int bestOrder = 1;
		for (int order = 2; order < maxOrder; order++) {
			double delta = Math.abs(2 * Math.PI / order - theta);
			if (delta < bestDelta) {
				bestOrder = order;
				bestDelta = delta;
			}
		}
		return bestOrder;
	}


	/**
	 * Returns a matrix that describes both rotation and translation.
	 */
	public Matrix getFullMatrix() {
		return null; // TODO, easy
	}

	/**
	 * Initialize variables
	 *
	 * @param rotation
	 * @param translation
	 */
	private void init(Matrix rotation, Atom translation) {
		if(rotation.getColumnDimension() != 3 || rotation.getRowDimension() != 3) {
			throw new IllegalArgumentException("Expected 3x3 rotation matrix");
		}


		// Calculate angle
		double c = (rotation.trace()-1)/2.0; //=cos(theta)
		// c is sometimes slightly out of the [-1,1] range due to numerical instabilities
		if( -1-1e-8 < c && c < -1 ) c = -1;
		if( 1+1e-8 > c && c > 1 ) c = 1;
		if( -1 > c || c > 1 ) {
			throw new IllegalArgumentException("Input matrix is not a valid rotation matrix.");
		}
		this.theta = Math.acos(c);

		if(theta < MIN_ANGLE) {
			calculateTranslationalAxis(rotation,translation);
		} else {
			calculateRotationalAxis(rotation, translation, c);
		}
	}

	/**
	 * Calculate the rotation axis for the normal case, where there is a
	 * significant rotation angle
	 * @param rotation
	 * @param translation
	 * @param c
	 */
	private void calculateRotationalAxis(Matrix rotation, Atom translation,
			double c) {
		// Calculate magnitude of rotationAxis components, but not signs
		double sum=0;
		double[] rotAx = new double[3];
		for(int i=0;i<3;i++) {
			rotAx[i] = Math.sqrt(rotation.get(i, i)-c);
			sum+=rotAx[i]*rotAx[i];
		}
		for(int i=0;i<3;i++) {
			rotAx[i] /= Math.sqrt(sum);
		}

		// Now determine signs
		double d0 = rotation.get(2,1)-rotation.get(1,2); //=2u[0]*sin(theta)
		double d1 = rotation.get(0,2)-rotation.get(2,0); //=2u[1]*sin(theta)
		double d2 = rotation.get(1,0)-rotation.get(0,1); //=2u[2]*sin(theta)

		double s12 = rotation.get(2,1)+rotation.get(1,2); //=2*u[1]*u[2]*(1-cos(theta))
		double s02 = rotation.get(0,2)+rotation.get(2,0); //=2*u[0]*u[2]*(1-cos(theta))
		double s01 = rotation.get(1,0)+rotation.get(0,1); //=2*u[0]*u[1]*(1-cos(theta))

		//Only use biggest d for the sign directly, for numerical stability around 180deg
		if( Math.abs(d0) < Math.abs(d1) ) { // not d0
			if( Math.abs(d1) < Math.abs(d2) ) { //d2
				if(d2>=0){ //u[2] positive
					if( s02 < 0 ) rotAx[0] = -rotAx[0];
					if( s12 < 0 ) rotAx[1] = -rotAx[1];
				} else { //u[2] negative
					rotAx[2] = -rotAx[2];
					if( s02 >= 0 ) rotAx[0] = -rotAx[0];
					if( s12 >= 0 ) rotAx[1] = -rotAx[1];
				}
			} else { //d1
				if(d1>=0) {//u[1] positive
					if( s01 < 0) rotAx[0] = -rotAx[0];
					if( s12 < 0) rotAx[2] = -rotAx[2];
				} else { //u[1] negative
					rotAx[1] = -rotAx[1];
					if( s01 >= 0) rotAx[0] = -rotAx[0];
					if( s12 >= 0) rotAx[2] = -rotAx[2];
				}
			}
		} else { // not d1
			if( Math.abs(d0) < Math.abs(d2) ) { //d2
				if(d2>=0){ //u[2] positive
					if( s02 < 0 ) rotAx[0] = -rotAx[0];
					if( s12 < 0 ) rotAx[1] = -rotAx[1];
				} else { //u[2] negative
					rotAx[2] = -rotAx[2];
					if( s02 >= 0 ) rotAx[0] = -rotAx[0];
					if( s12 >= 0 ) rotAx[1] = -rotAx[1];
				}
			} else { //d0
				if(d0>=0) { //u[0] positive
					if( s01 < 0 ) rotAx[1] = -rotAx[1];
					if( s02 < 0 ) rotAx[2] = -rotAx[2];
				} else { //u[0] negative
					rotAx[0] = -rotAx[0];
					if( s01 >= 0 ) rotAx[1] = -rotAx[1];
					if( s02 >= 0 ) rotAx[2] = -rotAx[2];
				}
			}
		}

		rotationAxis = new AtomImpl();
		rotationAxis.setCoords(rotAx);

		// Calculate screw = (rotationAxis dot translation)*u
		double dotProduct = Calc.scalarProduct(rotationAxis, translation);

		screwTranslation = Calc.scale(rotationAxis, dotProduct);
		otherTranslation = Calc.subtract(translation, screwTranslation);

		Atom hypot = Calc.vectorProduct(otherTranslation,rotationAxis);
		Calc.scaleEquals(hypot,.5/Math.tan(theta/2.0));

		// Calculate rotation axis position
		rotationPos = Calc.scaleAdd(.5,otherTranslation, hypot);
	}

	/**
	 * Handle cases with small angles of rotation
	 * @param rotation
	 * @param translation
	 */
	private void calculateTranslationalAxis(Matrix rotation, Atom translation) {
		// set axis parallel to translation
		rotationAxis = Calc.scale(translation, 1./Calc.amount(translation));

		// position is undefined
		rotationPos = null;

		screwTranslation = translation;
		otherTranslation = new AtomImpl();
		otherTranslation.setCoords(new double[] {0,0,0});
	}

	/**
	 * Returns a Jmol script which will display the axis of rotation. This
	 * consists of a cyan arrow along the axis, plus an arc showing the angle
	 * of rotation.
	 * <p>
	 * As the rotation angle gets smaller, the axis of rotation becomes poorly
	 * defined and would need to get farther and farther away from the protein.
	 * This is not particularly useful, so we arbitrarily draw it parallel to
	 * the translation and omit the arc.
	 * @param atoms Some atoms from the protein, used for determining the bounds
	 *  	  of the axis.
	 *
	 * @return The Jmol script, suitable for calls to
	 * {@link org.biojava.nbio.structure.align.gui.jmol.StructureAlignmentJmol#evalString() jmol.evalString()}
	 */
	public String getJmolScript(Atom[] atoms){
		return getJmolScript(atoms, 0);
	}

	/**
	 * Find a segment of the axis that covers the specified set of atoms.
	 * <p>
	 * Projects the input atoms onto the rotation axis and returns the bounding
	 * points.
	 * <p>
	 * In the case of a pure translational axis, the axis location is undefined
	 * so the center of mass will be used instead.
	 * @param atoms
	 * @return two points defining the axis segment
	 */
	public Pair<Atom> getAxisEnds(Atom[] atoms) {
		// Project each Atom onto the rotation axis to determine limits
		double min, max;
		min = max = Calc.scalarProduct(rotationAxis,atoms[0]);
		for(int i=1;i<atoms.length;i++) {
			double prod = Calc.scalarProduct(rotationAxis,atoms[i]);
			if(prod<min) min = prod;
			if(prod>max) max = prod;
		}
		double uLen = Calc.scalarProduct(rotationAxis,rotationAxis);// Should be 1, but double check
		min/=uLen;
		max/=uLen;

		// Project the origin onto the axis. If the axis is undefined, use the center of mass
		Atom axialPt;
		if(rotationPos == null) {
			Atom center = Calc.centerOfMass(atoms);

			// Project center onto the axis
			Atom centerOnAxis = Calc.scale(rotationAxis, Calc.scalarProduct(center, rotationAxis));

			// Remainder is projection of origin onto axis
			axialPt = Calc.subtract(center, centerOnAxis);

		} else {
			axialPt = rotationPos;
		}

		// Find end points of the rotation axis to display
		Atom axisMin = (Atom) axialPt.clone();
		Calc.scaleAdd(min, rotationAxis, axisMin);
		Atom axisMax = (Atom) axialPt.clone();
		Calc.scaleAdd(max, rotationAxis, axisMax);

		return new Pair<>(axisMin, axisMax);
	}
	/**
	 * Returns a Jmol script which will display the axis of rotation. This
	 * consists of a cyan arrow along the axis, plus an arc showing the angle
	 * of rotation.
	 * <p>
	 * As the rotation angle gets smaller, the axis of rotation becomes poorly
	 * defined and would need to get farther and farther away from the protein.
	 * This is not particularly useful, so we arbitrarily draw it parallel to
	 * the translation and omit the arc.
	 * @param atoms Some atoms from the protein, used for determining the bounds
	 *  	  of the axis.
	 * @param axisID in case of representing more than one axis in the same jmol
	 * 		  panel, indicate the ID number.
	 *
	 * @return The Jmol script, suitable for calls to
	 * {@link org.biojava.nbio.structure.align.gui.jmol.StructureAlignmentJmol#evalString() jmol.evalString()}
	 */
	public String getJmolScript(Atom[] atoms, int axisID){
		final double width=.5;// width of JMol object
		final String axisColor = "yellow"; //axis color
		final String screwColor = "orange"; //screw translation color

		Pair<Atom> endPoints = getAxisEnds(atoms);
		Atom axisMin = endPoints.getFirst();
		Atom axisMax = endPoints.getSecond();

		StringWriter result = new StringWriter();

		// set arrow heads to a reasonable length
		result.append("set defaultDrawArrowScale 2.0;");

		// draw axis of rotation
		result.append(
				String.format(Locale.US, "draw ID rot"+axisID+" CYLINDER {%f,%f,%f} {%f,%f,%f} WIDTH %f COLOR %s ;",
						axisMin.getX(),axisMin.getY(),axisMin.getZ(),
						axisMax.getX(),axisMax.getY(),axisMax.getZ(), width, axisColor ));

		// draw screw component
		boolean positiveScrew = Math.signum(rotationAxis.getX()) == Math.signum(screwTranslation.getX());
		if( positiveScrew ) {
			// screw is in the same direction as the axis
			result.append( String.format(Locale.US,
					"draw ID screw"+axisID+" VECTOR {%f,%f,%f} {%f,%f,%f} WIDTH %f COLOR %s ;",
					axisMax.getX(),axisMax.getY(),axisMax.getZ(),
					screwTranslation.getX(),screwTranslation.getY(),screwTranslation.getZ(),
					width, screwColor ));
		} else {
			// screw is in the opposite direction as the axis
			result.append( String.format(Locale.US,
					"draw ID screw"+axisID+" VECTOR {%f,%f,%f} {%f,%f,%f} WIDTH %f COLOR %s ;",
					axisMin.getX(),axisMin.getY(),axisMin.getZ(),
					screwTranslation.getX(),screwTranslation.getY(),screwTranslation.getZ(),
					width, screwColor ));
		}

		// draw angle of rotation
		if(rotationPos != null) {
			result.append(System.getProperty("line.separator"));
			result.append(String.format(Locale.US, "draw ID rotArc"+axisID+" ARC {%f,%f,%f} {%f,%f,%f} {0,0,0} {0,%f,%d} SCALE 500 DIAMETER %f COLOR %s;",
					axisMin.getX(),axisMin.getY(),axisMin.getZ(),
					axisMax.getX(),axisMax.getY(),axisMax.getZ(),
					Math.toDegrees(theta),
					positiveScrew ? 0 : 1 , // draw at the opposite end from the screw arrow
							width, axisColor ));
		}

		return result.toString();
	}

	/**
	 * Projects a given point onto the axis of rotation
	 * @param point
	 * @return An atom which lies on the axis, or null if the RotationAxis is purely translational
	 */
	public Atom getProjectedPoint(Atom point) {
		if(rotationPos == null) {
			// translation only
			return null;
		}

		Atom localPoint = Calc.subtract(point, rotationPos);
		double dot = Calc.scalarProduct(localPoint, rotationAxis);

		Atom localProjected = Calc.scale(rotationAxis, dot);
		Atom projected = Calc.add(localProjected, rotationPos);
		return projected;
	}

	/**
	 * Get the distance from a point to the axis of rotation
	 * @param point
	 * @return The distance to the axis, or NaN if the RotationAxis is purely translational
	 */
	public double getProjectedDistance(Atom point) {
		Atom projected = getProjectedPoint(point);
		if( projected == null) {
			// translation only
			return Double.NaN;
		}


		return Calc.getDistance(point, projected);

	}

	public void rotate(Atom[] atoms, double theta) {
		Matrix rot = getRotationMatrix(theta);
		if(rotationPos == null) {
			// Undefined rotation axis; do nothing
			return;
		}
		Atom negPos;

			negPos = Calc.invert(rotationPos);

		for(Atom a: atoms) {
			Calc.shift(a, negPos);
		}
		Calc.rotate(atoms, rot);
		for(Atom a: atoms) {
			Calc.shift(a, rotationPos);
		}
	}

	/**
	 * Calculate the rotation angle for a structure
	 * @param afpChain
	 * @return The rotation angle, in radians
	 * @throws StructureException If the alignment doesn't contain any blocks
	 * @throws NullPointerException If the alignment doesn't have a rotation matrix set
	 */
	public static double getAngle(AFPChain afpChain) throws StructureException {
		if(afpChain.getBlockNum() < 1) {
			throw new StructureException("No aligned residues");
		}
		Matrix rotation = afpChain.getBlockRotationMatrix()[0];

		if(rotation == null) {
			throw new NullPointerException("AFPChain does not contain a rotation matrix");
		}
		return getAngle(rotation);
	}
	/**
	 * Calculate the rotation angle for a given matrix
	 * @param rotation Rotation matrix
	 * @return The angle, in radians
	 */
	public static double getAngle(Matrix rotation) {
		double c = (rotation.trace()-1)/2.0; //=cos(theta)
		// c is sometimes slightly out of the [-1,1] range due to numerical instabilities
		if( -1-1e-8 < c && c < -1 ) c = -1;
		if( 1+1e-8 > c && c > 1 ) c = 1;
		if( -1 > c || c > 1 ) {
			throw new IllegalArgumentException("Input matrix is not a valid rotation matrix.");
		}
		return Math.acos(c);
	}

	/**
	 *
	 * @return If the rotation axis is well defined, rather than purely translational
	 */
	public boolean isDefined() {
		return rotationPos != null;
	}

	/**
	 * Quickly compute the rotation angle from a rotation matrix.
	 * @param transform 4D transformation matrix. Translation components are ignored.
	 * @return Angle, from 0 to PI
	 */
	public static double getAngle(Matrix4d transform) {
		// Calculate angle
		double c = (transform.m00 + transform.m11 + transform.m22 - 1)/2.0; //=cos(theta)
		// c is sometimes slightly out of the [-1,1] range due to numerical instabilities
		if( -1-1e-8 < c && c < -1 ) c = -1;
		if( 1+1e-8 > c && c > 1 ) c = 1;
		if( -1 > c || c > 1 ) {
			throw new IllegalArgumentException("Input matrix is not a valid rotation matrix.");
		}
		return Math.acos(c);
	}

	/**
	 * Quickly compute the rotation angle from a rotation matrix.
	 * @param transform 3D rotation matrix
	 * @return Angle, from 0 to PI
	 */
	public static double getAngle(Matrix3d transform) {
		// Calculate angle
		double c = (transform.m00 + transform.m11 + transform.m22 - 1)/2.0; //=cos(theta)
		// c is sometimes slightly out of the [-1,1] range due to numerical instabilities
		if( -1-1e-8 < c && c < -1 ) c = -1;
		if( 1+1e-8 > c && c > 1 ) c = 1;
		if( -1 > c || c > 1 ) {
			throw new IllegalArgumentException("Input matrix is not a valid rotation matrix.");
		}
		return Math.acos(c);
	}
}
