/*
 *                    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/
 *
 * Created on 08.05.2004
 *
 */
package org.biojava.nbio.structure;

import java.util.ArrayList;
import java.util.Collection;
import java.util.List;

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

import org.biojava.nbio.structure.geometry.CalcPoint;
import org.biojava.nbio.structure.geometry.Matrices;
import org.biojava.nbio.structure.geometry.SuperPositionSVD;
import org.biojava.nbio.structure.jama.Matrix;
import org.slf4j.Logger;
import org.slf4j.LoggerFactory;

/**
 * Utility operations on Atoms, AminoAcids, Matrices, Point3d, etc.
 * <p>
 * Currently the coordinates of an Atom are stored as an array of size 3
 * (double[3]). It would be more powerful to use Point3d from javax.vecmath.
 * Overloaded methods for Point3d operations exist in the {@link CalcPoint}
 * Class.
 *
 * @author Andreas Prlic
 * @author Aleix Lafita
 * @since 1.4
 * @version %I% %G%
 */

public class Calc {

        private final static Logger logger = LoggerFactory.getLogger(Calc.class);

        /**
         * calculate distance between two atoms.
         *
         * @param a
         *            an Atom object
         * @param b
         *            an Atom object
         * @return a double
         */
        public static final double getDistance(Atom a, Atom b) {
                double x = a.getX() - b.getX();
                double y = a.getY() - b.getY();
                double z = a.getZ() - b.getZ();

                double s = x * x + y * y + z * z;

                return Math.sqrt(s);
        }

        /**
         * Will calculate the square of distances between two atoms. This will be
         * faster as it will not perform the final square root to get the actual
         * distance. Use this if doing large numbers of distance comparisons - it is
         * marginally faster than getDistance().
         *
         * @param a
         *            an Atom object
         * @param b
         *            an Atom object
         * @return a double
         */
        public static double getDistanceFast(Atom a, Atom b) {
                double x = a.getX() - b.getX();
                double y = a.getY() - b.getY();
                double z = a.getZ() - b.getZ();

                return x * x + y * y + z * z;
        }

        public static final Atom invert(Atom a) {
                double[] coords = new double[] { 0.0, 0.0, 0.0 };
                Atom zero = new AtomImpl();
                zero.setCoords(coords);
                return subtract(zero, a);
        }

        /**
         * add two atoms ( a + b).
         *
         * @param a
         *            an Atom object
         * @param b
         *            an Atom object
         * @return an Atom object
         */
        public static final Atom add(Atom a, Atom b) {

                Atom c = new AtomImpl();
                c.setX(a.getX() + b.getX());
                c.setY(a.getY() + b.getY());
                c.setZ(a.getZ() + b.getZ());

                return c;
        }

        /**
         * subtract two atoms ( a - b).
         *
         * @param a
         *            an Atom object
         * @param b
         *            an Atom object
         * @return n new Atom object representing the difference
         */
        public static final Atom subtract(Atom a, Atom b) {
                Atom c = new AtomImpl();
                c.setX(a.getX() - b.getX());
                c.setY(a.getY() - b.getY());
                c.setZ(a.getZ() - b.getZ());

                return c;
        }

        /**
         * Vector product (cross product).
         *
         * @param a
         *            an Atom object
         * @param b
         *            an Atom object
         * @return an Atom object
         */
        public static final Atom vectorProduct(Atom a, Atom b) {

                Atom c = new AtomImpl();
                c.setX(a.getY() * b.getZ() - a.getZ() * b.getY());
                c.setY(a.getZ() * b.getX() - a.getX() * b.getZ());
                c.setZ(a.getX() * b.getY() - a.getY() * b.getX());
                return c;

        }

        /**
         * Scalar product (dot product).
         *
         * @param a
         *            an Atom object
         * @param b
         *            an Atom object
         * @return a double
         */
        public static final double scalarProduct(Atom a, Atom b) {
                return a.getX() * b.getX() + a.getY() * b.getY() + a.getZ() * b.getZ();
        }

        /**
         * Gets the length of the vector (2-norm)
         *
         * @param a
         *            an Atom object
         * @return Square root of the sum of the squared elements
         */
        public static final double amount(Atom a) {
                return Math.sqrt(scalarProduct(a, a));
        }

        /**
         * Gets the angle between two vectors
         *
         * @param a
         *            an Atom object
         * @param b
         *            an Atom object
         * @return Angle between a and b in degrees, in range [0,180]. If either
         *         vector has length 0 then angle is not defined and NaN is returned
         */
        public static final double angle(Atom a, Atom b) {

                Vector3d va = new Vector3d(a.getCoordsAsPoint3d());
                Vector3d vb = new Vector3d(b.getCoordsAsPoint3d());

                return Math.toDegrees(va.angle(vb));

        }

        /**
         * Returns the unit vector of vector a .
         *
         * @param a
         *            an Atom object
         * @return an Atom object
         */
        public static final Atom unitVector(Atom a) {
                double amount = amount(a);

                double[] coords = new double[3];

                coords[0] = a.getX() / amount;
                coords[1] = a.getY() / amount;
                coords[2] = a.getZ() / amount;

                a.setCoords(coords);
                return a;

        }

        /**
         * Calculate the torsion angle, i.e. the angle between the normal vectors of
         * the two plains a-b-c and b-c-d. See
         * http://en.wikipedia.org/wiki/Dihedral_angle
         * 
         * @param a
         *            an Atom object
         * @param b
         *            an Atom object
         * @param c
         *            an Atom object
         * @param d
         *            an Atom object
         * @return the torsion angle in degrees, in range +-[0,180]. If either first
         *         3 or last 3 atoms are colinear then torsion angle is not defined
         *         and NaN is returned
         */
        public static final double torsionAngle(Atom a, Atom b, Atom c, Atom d) {

                Atom ab = subtract(a, b);
                Atom cb = subtract(c, b);
                Atom bc = subtract(b, c);
                Atom dc = subtract(d, c);

                Atom abc = vectorProduct(ab, cb);
                Atom bcd = vectorProduct(bc, dc);

                double angl = angle(abc, bcd);

                /* calc the sign: */
                Atom vecprod = vectorProduct(abc, bcd);
                double val = scalarProduct(cb, vecprod);
                if (val < 0.0)
                        angl = -angl;

                return angl;
        }

        /**
         * Calculate the phi angle.
         *
         * @param a
         *            an AminoAcid object
         * @param b
         *            an AminoAcid object
         * @return a double
         * @throws StructureException
         *             if aminoacids not connected or if any of the 4 needed atoms
         *             missing
         */
        public static final double getPhi(AminoAcid a, AminoAcid b)
                        throws StructureException {

                if (!isConnected(a, b)) {
                        throw new StructureException(
                                        "can not calc Phi - AminoAcids are not connected!");
                }

                Atom a_C = a.getC();
                Atom b_N = b.getN();
                Atom b_CA = b.getCA();
                Atom b_C = b.getC();

                // C and N were checked in isConnected already
                if (b_CA == null)
                        throw new StructureException(
                                        "Can not calculate Phi, CA atom is missing");

                return torsionAngle(a_C, b_N, b_CA, b_C);
        }

        /**
         * Calculate the psi angle.
         *
         * @param a
         *            an AminoAcid object
         * @param b
         *            an AminoAcid object
         * @return a double
         * @throws StructureException
         *             if aminoacids not connected or if any of the 4 needed atoms
         *             missing
         */
        public static final double getPsi(AminoAcid a, AminoAcid b)
                        throws StructureException {
                if (!isConnected(a, b)) {
                        throw new StructureException(
                                        "can not calc Psi - AminoAcids are not connected!");
                }

                Atom a_N = a.getN();
                Atom a_CA = a.getCA();
                Atom a_C = a.getC();
                Atom b_N = b.getN();

                // C and N were checked in isConnected already
                if (a_CA == null)
                        throw new StructureException(
                                        "Can not calculate Psi, CA atom is missing");

                return torsionAngle(a_N, a_CA, a_C, b_N);

        }

        /**
         * Test if two amino acids are connected, i.e. if the distance from C to N <
         * 2.5 Angstrom.
         *
         * If one of the AminoAcids has an atom missing, returns false.
         *
         * @param a
         *            an AminoAcid object
         * @param b
         *            an AminoAcid object
         * @return true if ...
         */
        public static final boolean isConnected(AminoAcid a, AminoAcid b) {
                Atom C = null;
                Atom N = null;

                C = a.getC();
                N = b.getN();

                if (C == null || N == null)
                        return false;

                // one could also check if the CA atoms are < 4 A...
                double distance = getDistance(C, N);
                return distance < 2.5;
        }

        /**
         * Rotate a single Atom aroud a rotation matrix. The rotation Matrix must be
         * a pre-multiplication 3x3 Matrix.
         *
         * If the matrix is indexed m[row][col], then the matrix will be
         * pre-multiplied (y=atom*M)
         * 
         * @param atom
         *            atom to be rotated
         * @param m
         *            a rotation matrix represented as a double[3][3] array
         */
        public static final void rotate(Atom atom, double[][] m) {

                double x = atom.getX();
                double y = atom.getY();
                double z = atom.getZ();

                double nx = m[0][0] * x + m[0][1] * y + m[0][2] * z;
                double ny = m[1][0] * x + m[1][1] * y + m[1][2] * z;
                double nz = m[2][0] * x + m[2][1] * y + m[2][2] * z;

                atom.setX(nx);
                atom.setY(ny);
                atom.setZ(nz);
        }

        /**
         * Rotate a structure. The rotation Matrix must be a pre-multiplication
         * Matrix.
         *
         * @param structure
         *            a Structure object
         * @param rotationmatrix
         *            an array (3x3) of double representing the rotation matrix.
         * @throws StructureException
         *             ...
         */
        public static final void rotate(Structure structure,
                        double[][] rotationmatrix) throws StructureException {

                if (rotationmatrix.length != 3) {
                        throw new StructureException("matrix does not have size 3x3 !");
                }
                AtomIterator iter = new AtomIterator(structure);
                while (iter.hasNext()) {
                        Atom atom = iter.next();
                        Calc.rotate(atom, rotationmatrix);
                }
        }

        /**
         * Rotate a Group. The rotation Matrix must be a pre-multiplication Matrix.
         *
         * @param group
         *            a group object
         * @param rotationmatrix
         *            an array (3x3) of double representing the rotation matrix.
         * @throws StructureException
         *             ...
         */
        public static final void rotate(Group group, double[][] rotationmatrix)
                        throws StructureException {

                if (rotationmatrix.length != 3) {
                        throw new StructureException("matrix does not have size 3x3 !");
                }
                AtomIterator iter = new AtomIterator(group);
                while (iter.hasNext()) {
                        Atom atom = null;

                        atom = iter.next();
                        rotate(atom, rotationmatrix);

                }
        }

        /**
         * Rotate an Atom around a Matrix object. The rotation Matrix must be a
         * pre-multiplication Matrix.
         *
         * @param atom
         *            atom to be rotated
         * @param m
         *            rotation matrix to be applied to the atom
         */
        public static final void rotate(Atom atom, Matrix m) {

                double x = atom.getX();
                double y = atom.getY();
                double z = atom.getZ();
                double[][] ad = new double[][] { { x, y, z } };

                Matrix am = new Matrix(ad);
                Matrix na = am.times(m);

                atom.setX(na.get(0, 0));
                atom.setY(na.get(0, 1));
                atom.setZ(na.get(0, 2));

        }

        /**
         * Rotate a group object. The rotation Matrix must be a pre-multiplication
         * Matrix.
         *
         * @param group
         *            a group to be rotated
         * @param m
         *            a Matrix object representing the rotation matrix
         */
        public static final void rotate(Group group, Matrix m) {

                AtomIterator iter = new AtomIterator(group);

                while (iter.hasNext()) {
                        Atom atom = iter.next();
                        rotate(atom, m);

                }

        }

        /**
         * Rotate a structure object. The rotation Matrix must be a
         * pre-multiplication Matrix.
         *
         * @param structure
         *            the structure to be rotated
         * @param m
         *            rotation matrix to be applied
         */
        public static final void rotate(Structure structure, Matrix m) {

                AtomIterator iter = new AtomIterator(structure);

                while (iter.hasNext()) {
                        Atom atom = iter.next();
                        rotate(atom, m);

                }

        }

        /**
         * Transform an array of atoms at once. The transformation Matrix must be a
         * post-multiplication Matrix.
         *
         * @param ca
         *            array of Atoms to shift
         * @param t
         *            transformation Matrix4d
         */
        public static void transform(Atom[] ca, Matrix4d t) {
                for (Atom atom : ca)
                        Calc.transform(atom, t);
        }

        /**
         * Transforms an atom object, given a Matrix4d (i.e. the vecmath library
         * double-precision 4x4 rotation+translation matrix). The transformation
         * Matrix must be a post-multiplication Matrix.
         *
         * @param atom
         * @param m
         */
        public static final void transform(Atom atom, Matrix4d m) {

                Point3d p = new Point3d(atom.getX(), atom.getY(), atom.getZ());
                m.transform(p);

                atom.setX(p.x);
                atom.setY(p.y);
                atom.setZ(p.z);
        }

        /**
         * Transforms a group object, given a Matrix4d (i.e. the vecmath library
         * double-precision 4x4 rotation+translation matrix). The transformation
         * Matrix must be a post-multiplication Matrix.
         *
         * @param group
         * @param m
         */
        public static final void transform(Group group, Matrix4d m) {
                for (Atom atom : group.getAtoms()) {
                        transform(atom, m);
                }
                for (Group altG : group.getAltLocs()) {
                        transform(altG, m);
                }
        }

        /**
         * Transforms a structure object, given a Matrix4d (i.e. the vecmath library
         * double-precision 4x4 rotation+translation matrix). The transformation
         * Matrix must be a post-multiplication Matrix.
         *
         * @param structure
         * @param m
         */
        public static final void transform(Structure structure, Matrix4d m) {
                for (int n=0; n<structure.nrModels();n++) {
                        for (Chain c : structure.getChains(n)) {
                                transform(c, m);
                        }
                }
        }

        /**
         * Transforms a chain object, given a Matrix4d (i.e. the vecmath library
         * double-precision 4x4 rotation+translation matrix). The transformation
         * Matrix must be a post-multiplication Matrix.
         *
         * @param chain
         * @param m
         */
        public static final void transform(Chain chain, Matrix4d m) {

                for (Group g : chain.getAtomGroups()) {
                        transform(g, m);
                }
        }

        /**
         * Translates an atom object, given a Vector3d (i.e. the vecmath library
         * double-precision 3-d vector)
         * 
         * @param atom
         * @param v
         */
        public static final void translate(Atom atom, Vector3d v) {

                atom.setX(atom.getX() + v.x);
                atom.setY(atom.getY() + v.y);
                atom.setZ(atom.getZ() + v.z);
        }

        /**
         * Translates a group object, given a Vector3d (i.e. the vecmath library
         * double-precision 3-d vector)
         * 
         * @param group
         * @param v
         */
        public static final void translate(Group group, Vector3d v) {

                for (Atom atom : group.getAtoms()) {
                        translate(atom, v);
                }
                for (Group altG : group.getAltLocs()) {
                        translate(altG, v);
                }
        }

        /**
         * Translates a chain object, given a Vector3d (i.e. the vecmath library
         * double-precision 3-d vector)
         * 
         * @param chain
         * @param v
         */
        public static final void translate(Chain chain, Vector3d v) {

                for (Group g : chain.getAtomGroups()) {
                        translate(g, v);
                }
        }

        /**
         * Translates a Structure object, given a Vector3d (i.e. the vecmath library
         * double-precision 3-d vector)
         * 
         * @param structure
         * @param v
         */
        public static final void translate(Structure structure, Vector3d v) {
                
                for (int n=0; n<structure.nrModels();n++) {
                        for (Chain c : structure.getChains(n)) {
                                translate(c, v);
                        }
                }
        }

        /**
         * calculate structure + Matrix coodinates ...
         *
         * @param s
         *            the structure to operate on
         * @param matrix
         *            a Matrix object
         */
        public static final void plus(Structure s, Matrix matrix) {
                AtomIterator iter = new AtomIterator(s);
                Atom oldAtom = null;
                Atom rotOldAtom = null;
                while (iter.hasNext()) {
                        Atom atom = null;

                        atom = iter.next();
                        try {
                                if (oldAtom != null) {
                                        logger.debug("before {}", getDistance(oldAtom, atom));
                                }
                        } catch (Exception e) {
                                logger.error("Exception: ", e);
                        }
                        oldAtom = (Atom) atom.clone();

                        double x = atom.getX();
                        double y = atom.getY();
                        double z = atom.getZ();
                        double[][] ad = new double[][] { { x, y, z } };

                        Matrix am = new Matrix(ad);
                        Matrix na = am.plus(matrix);

                        double[] coords = new double[3];
                        coords[0] = na.get(0, 0);
                        coords[1] = na.get(0, 1);
                        coords[2] = na.get(0, 2);
                        atom.setCoords(coords);
                        try {
                                if (rotOldAtom != null) {
                                        logger.debug("after {}", getDistance(rotOldAtom, atom));
                                }
                        } catch (Exception e) {
                                logger.error("Exception: ", e);
                        }
                        rotOldAtom = (Atom) atom.clone();
                }

        }

        /**
         * shift a structure with a vector.
         *
         * @param structure
         *            a Structure object
         * @param a
         *            an Atom object representing a shift vector
         */
        public static final void shift(Structure structure, Atom a) {

                AtomIterator iter = new AtomIterator(structure);
                while (iter.hasNext()) {
                        Atom atom = null;

                        atom = iter.next();

                        Atom natom = add(atom, a);
                        double x = natom.getX();
                        double y = natom.getY();
                        double z = natom.getZ();
                        atom.setX(x);
                        atom.setY(y);
                        atom.setZ(z);

                }
        }

        /**
         * Shift a vector.
         *
         * @param a
         *            vector a
         * @param b
         *            vector b
         */
        public static final void shift(Atom a, Atom b) {

                Atom natom = add(a, b);
                double x = natom.getX();
                double y = natom.getY();
                double z = natom.getZ();
                a.setX(x);
                a.setY(y);
                a.setZ(z);
        }

        /**
         * Shift a Group with a vector.
         *
         * @param group
         *            a group object
         * @param a
         *            an Atom object representing a shift vector
         */
        public static final void shift(Group group, Atom a) {

                AtomIterator iter = new AtomIterator(group);
                while (iter.hasNext()) {
                        Atom atom = null;

                        atom = iter.next();

                        Atom natom = add(atom, a);
                        double x = natom.getX();
                        double y = natom.getY();
                        double z = natom.getZ();
                        atom.setX(x);
                        atom.setY(y);
                        atom.setZ(z);

                }
        }

        /**
         * Returns the centroid of the set of atoms.
         * 
         * @param atomSet
         *            a set of Atoms
         * @return an Atom representing the Centroid of the set of atoms
         */
        public static final Atom getCentroid(Atom[] atomSet) {
                
                // if we don't catch this case, the centroid returned is (NaN,NaN,NaN), which can cause lots of problems down the line
                if (atomSet.length==0) 
                        throw new IllegalArgumentException("Atom array has length 0, can't calculate centroid!");


                double[] coords = new double[3];

                coords[0] = 0;
                coords[1] = 0;
                coords[2] = 0;

                for (Atom a : atomSet) {
                        coords[0] += a.getX();
                        coords[1] += a.getY();
                        coords[2] += a.getZ();
                }

                int n = atomSet.length;
                coords[0] = coords[0] / n;
                coords[1] = coords[1] / n;
                coords[2] = coords[2] / n;

                Atom vec = new AtomImpl();
                vec.setCoords(coords);
                return vec;

        }

        /**
         * Returns the center of mass of the set of atoms. Atomic masses of the
         * Atoms are used.
         * 
         * @param points
         *            a set of Atoms
         * @return an Atom representing the center of mass
         */
        public static Atom centerOfMass(Atom[] points) {
                Atom center = new AtomImpl();

                float totalMass = 0.0f;
                for (Atom a : points) {
                        float mass = a.getElement().getAtomicMass();
                        totalMass += mass;
                        center = scaleAdd(mass, a, center);
                }

                center = scaleEquals(center, 1.0f / totalMass);
                return center;
        }

        /**
         * Multiply elements of a by s (in place)
         * 
         * @param a
         * @param s
         * @return the modified a
         */
        public static Atom scaleEquals(Atom a, double s) {
                double x = a.getX();
                double y = a.getY();
                double z = a.getZ();

                x *= s;
                y *= s;
                z *= s;

                // Atom b = new AtomImpl();
                a.setX(x);
                a.setY(y);
                a.setZ(z);

                return a;
        }

        /**
         * Multiply elements of a by s
         * 
         * @param a
         * @param s
         * @return A new Atom with s*a
         */
        public static Atom scale(Atom a, double s) {
                double x = a.getX();
                double y = a.getY();
                double z = a.getZ();

                Atom b = new AtomImpl();
                b.setX(x * s);
                b.setY(y * s);
                b.setZ(z * s);

                return b;
        }

        /**
         * Perform linear transformation s*X+B, and store the result in b
         * 
         * @param s
         *            Amount to scale x
         * @param x
         *            Input coordinate
         * @param b
         *            Vector to translate (will be modified)
         * @return b, after modification
         */
        public static Atom scaleAdd(double s, Atom x, Atom b) {

                double xc = s * x.getX() + b.getX();
                double yc = s * x.getY() + b.getY();
                double zc = s * x.getZ() + b.getZ();

                // Atom a = new AtomImpl();
                b.setX(xc);
                b.setY(yc);
                b.setZ(zc);

                return b;
        }

        /**
         * Returns the Vector that needs to be applied to shift a set of atoms to
         * the Centroid.
         * 
         * @param atomSet
         *            array of Atoms
         * @return the vector needed to shift the set of atoms to its geometric
         *         center
         */
        public static final Atom getCenterVector(Atom[] atomSet) {
                Atom centroid = getCentroid(atomSet);

                return getCenterVector(atomSet, centroid);

        }

        /**
         * Returns the Vector that needs to be applied to shift a set of atoms to
         * the Centroid, if the centroid is already known
         * 
         * @param atomSet
         *            array of Atoms
         * @return the vector needed to shift the set of atoms to its geometric
         *         center
         */
        public static final Atom getCenterVector(Atom[] atomSet, Atom centroid) {

                double[] coords = new double[3];
                coords[0] = 0 - centroid.getX();
                coords[1] = 0 - centroid.getY();
                coords[2] = 0 - centroid.getZ();

                Atom shiftVec = new AtomImpl();
                shiftVec.setCoords(coords);
                return shiftVec;

        }

        /**
         * Center the atoms at the Centroid.
         * 
         * @param atomSet
         *            a set of Atoms
         * @return an Atom representing the Centroid of the set of atoms
         * @throws StructureException
         * */
        public static final Atom[] centerAtoms(Atom[] atomSet)
                        throws StructureException {

                Atom centroid = getCentroid(atomSet);
                return centerAtoms(atomSet, centroid);
        }

        /**
         * Center the atoms at the Centroid, if the centroid is already know.
         * 
         * @param atomSet
         *            a set of Atoms
         * @return an Atom representing the Centroid of the set of atoms
         * @throws StructureException
         * */
        public static final Atom[] centerAtoms(Atom[] atomSet, Atom centroid)
                        throws StructureException {

                Atom shiftVector = getCenterVector(atomSet, centroid);

                Atom[] newAtoms = new AtomImpl[atomSet.length];

                for (int i = 0; i < atomSet.length; i++) {
                        Atom a = atomSet[i];
                        Atom n = add(a, shiftVector);
                        newAtoms[i] = n;
                }
                return newAtoms;
        }

        /**
         * creates a virtual C-beta atom. this might be needed when working with GLY
         *
         * thanks to Peter Lackner for a python template of this method.
         * 
         * @param amino
         *            the amino acid for which a "virtual" CB atom should be
         *            calculated
         * @return a "virtual" CB atom
         * @throws StructureException
         */
        public static final Atom createVirtualCBAtom(AminoAcid amino)
                        throws StructureException {

                AminoAcid ala = StandardAminoAcid.getAminoAcid("ALA");
                Atom aN = ala.getN();
                Atom aCA = ala.getCA();
                Atom aC = ala.getC();
                Atom aCB = ala.getCB();

                Atom[] arr1 = new Atom[3];
                arr1[0] = aN;
                arr1[1] = aCA;
                arr1[2] = aC;

                Atom[] arr2 = new Atom[3];
                arr2[0] = amino.getN();
                arr2[1] = amino.getCA();
                arr2[2] = amino.getC();

                // ok now we got the two arrays, do a Superposition:

                SuperPositionSVD svd = new SuperPositionSVD(false);

                Matrix4d transform = svd.superpose(Calc.atomsToPoints(arr1), Calc.atomsToPoints(arr2));
                Matrix rotMatrix = Matrices.getRotationJAMA(transform);
                Atom tranMatrix = getTranslationVector(transform);
                
                Calc.rotate(aCB, rotMatrix);

                Atom virtualCB = Calc.add(aCB, tranMatrix);
                virtualCB.setName("CB");

                return virtualCB;
        }

        /**
         * Gets euler angles for a matrix given in ZYZ convention. (as e.g. used by
         * Jmol)
         *
         * @param m
         *            the rotation matrix
         * @return the euler values for a rotation around Z, Y, Z in degrees...
         */
        public static final double[] getZYZEuler(Matrix m) {
                double m22 = m.get(2, 2);
                double rY = Math.toDegrees(Math.acos(m22));
                double rZ1, rZ2;
                if (m22 > .999d || m22 < -.999d) {
                        rZ1 = Math.toDegrees(Math.atan2(m.get(1, 0), m.get(1, 1)));
                        rZ2 = 0;
                } else {
                        rZ1 = Math.toDegrees(Math.atan2(m.get(2, 1), -m.get(2, 0)));
                        rZ2 = Math.toDegrees(Math.atan2(m.get(1, 2), m.get(0, 2)));
                }
                return new double[] { rZ1, rY, rZ2 };
        }

        /**
         * Convert a rotation Matrix to Euler angles. This conversion uses
         * conventions as described on page:
         * http://www.euclideanspace.com/maths/geometry/rotations/euler/index.htm
         * Coordinate System: right hand Positive angle: right hand Order of euler
         * angles: heading first, then attitude, then bank
         *
         * @param m
         *            the rotation matrix
         * @return a array of three doubles containing the three euler angles in
         *         radians
         */
        public static final double[] getXYZEuler(Matrix m) {
                double heading, attitude, bank;

                // Assuming the angles are in radians.
                if (m.get(1, 0) > 0.998) { // singularity at north pole
                        heading = Math.atan2(m.get(0, 2), m.get(2, 2));
                        attitude = Math.PI / 2;
                        bank = 0;

                } else if (m.get(1, 0) < -0.998) { // singularity at south pole
                        heading = Math.atan2(m.get(0, 2), m.get(2, 2));
                        attitude = -Math.PI / 2;
                        bank = 0;

                } else {
                        heading = Math.atan2(-m.get(2, 0), m.get(0, 0));
                        bank = Math.atan2(-m.get(1, 2), m.get(1, 1));
                        attitude = Math.asin(m.get(1, 0));
                }
                return new double[] { heading, attitude, bank };
        }

        /**
         * This conversion uses NASA standard aeroplane conventions as described on
         * page:
         * http://www.euclideanspace.com/maths/geometry/rotations/euler/index.htm
         * Coordinate System: right hand Positive angle: right hand Order of euler
         * angles: heading first, then attitude, then bank. matrix row column
         * ordering: [m00 m01 m02] [m10 m11 m12] [m20 m21 m22]
         * 
         * @param heading
         *            in radians
         * @param attitude
         *            in radians
         * @param bank
         *            in radians
         * @return the rotation matrix
         */
        public static final Matrix matrixFromEuler(double heading, double attitude,
                        double bank) {
                // Assuming the angles are in radians.
                double ch = Math.cos(heading);
                double sh = Math.sin(heading);
                double ca = Math.cos(attitude);
                double sa = Math.sin(attitude);
                double cb = Math.cos(bank);
                double sb = Math.sin(bank);

                Matrix m = new Matrix(3, 3);
                m.set(0, 0, ch * ca);
                m.set(0, 1, sh * sb - ch * sa * cb);
                m.set(0, 2, ch * sa * sb + sh * cb);
                m.set(1, 0, sa);
                m.set(1, 1, ca * cb);
                m.set(1, 2, -ca * sb);
                m.set(2, 0, -sh * ca);
                m.set(2, 1, sh * sa * cb + ch * sb);
                m.set(2, 2, -sh * sa * sb + ch * cb);

                return m;
        }

        /**
         * Calculates the angle from centerPt to targetPt in degrees. The return
         * should range from [0,360), rotating CLOCKWISE, 0 and 360 degrees
         * represents NORTH, 90 degrees represents EAST, etc...
         *
         * Assumes all points are in the same coordinate space. If they are not, you
         * will need to call SwingUtilities.convertPointToScreen or equivalent on
         * all arguments before passing them to this function.
         *
         * @param centerPt
         *            Point we are rotating around.
         * @param targetPt
         *            Point we want to calculate the angle to.
         * @return angle in degrees. This is the angle from centerPt to targetPt.
         */
        public static double calcRotationAngleInDegrees(Atom centerPt, Atom targetPt) {
                // calculate the angle theta from the deltaY and deltaX values
                // (atan2 returns radians values from [-PI,PI])
                // 0 currently points EAST.
                // NOTE: By preserving Y and X param order to atan2, we are expecting
                // a CLOCKWISE angle direction.
                double theta = Math.atan2(targetPt.getY() - centerPt.getY(),
                                targetPt.getX() - centerPt.getX());

                // rotate the theta angle clockwise by 90 degrees
                // (this makes 0 point NORTH)
                // NOTE: adding to an angle rotates it clockwise.
                // subtracting would rotate it counter-clockwise
                theta += Math.PI / 2.0;

                // convert from radians to degrees
                // this will give you an angle from [0->270],[-180,0]
                double angle = Math.toDegrees(theta);

                // convert to positive range [0-360)
                // since we want to prevent negative angles, adjust them now.
                // we can assume that atan2 will not return a negative value
                // greater than one partial rotation
                if (angle < 0) {
                        angle += 360;
                }

                return angle;
        }

        public static void main(String[] args) {
                Atom a = new AtomImpl();
                a.setX(0);
                a.setY(0);
                a.setZ(0);

                Atom b = new AtomImpl();
                b.setX(1);
                b.setY(1);
                b.setZ(0);

                logger.info("Angle between atoms: ", calcRotationAngleInDegrees(a, b));
        }

        public static void rotate(Atom[] ca, Matrix matrix) {
                for (Atom atom : ca)
                        Calc.rotate(atom, matrix);
        }

        /**
         * Shift an array of atoms at once.
         * 
         * @param ca
         *            array of Atoms to shift
         * @param b
         *            reference Atom vector
         */
        public static void shift(Atom[] ca, Atom b) {
                for (Atom atom : ca)
                        Calc.shift(atom, b);
        }

        /**
         * Convert JAMA rotation and translation to a Vecmath transformation matrix.
         * Because the JAMA matrix is a pre-multiplication matrix and the Vecmath
         * matrix is a post-multiplication one, the rotation matrix is transposed to
         * ensure that the transformation they produce is the same.
         *
         * @param rot
         *            3x3 Rotation matrix
         * @param trans
         *            3x1 translation vector in Atom coordinates
         * @return 4x4 transformation matrix
         */
        public static Matrix4d getTransformation(Matrix rot, Atom trans) {
                return new Matrix4d(new Matrix3d(rot.getColumnPackedCopy()),
                                new Vector3d(trans.getCoordsAsPoint3d()), 1.0);
        }
        
        /**
         * Extract the translational vector as an Atom of a transformation matrix.
         *
         * @param transform
         *            Matrix4d
         * @return Atom shift vector
         */
        public static Atom getTranslationVector(Matrix4d transform) {

                Atom transl = new AtomImpl();
                double[] coords = { transform.m03, transform.m13, transform.m23 };
                transl.setCoords(coords);
                return transl;
        }

        /**
         * Convert an array of atoms into an array of vecmath points
         * 
         * @param atoms
         *            list of atoms
         * @return list of Point3ds storing the x,y,z coordinates of each atom
         */
        public static Point3d[] atomsToPoints(Atom[] atoms) {
                Point3d[] points = new Point3d[atoms.length];
                for (int i = 0; i < atoms.length; i++) {
                        points[i] = atoms[i].getCoordsAsPoint3d();
                }
                return points;
        }
        /**
         * Convert an array of atoms into an array of vecmath points
         * 
         * @param atoms
         *            list of atoms
         * @return list of Point3ds storing the x,y,z coordinates of each atom
         */
        public static List<Point3d> atomsToPoints(Collection<Atom> atoms) {
                ArrayList<Point3d> points = new ArrayList<>(atoms.size());
                for (Atom atom : atoms) {
                        points.add(atom.getCoordsAsPoint3d());
                }
                return points;
        }

        /**
         * Calculate the RMSD of two Atom arrays, already superposed.
         * 
         * @param x
         *            array of Atoms superposed to y
         * @param y
         *            array of Atoms superposed to x
         * @return RMSD
         */
        public static double rmsd(Atom[] x, Atom[] y) {
                return CalcPoint.rmsd(atomsToPoints(x), atomsToPoints(y));
        }

        /**
         * Calculate the TM-Score for the superposition.
         *
         * <em>Normalizes by the <strong>minimum</strong>-length structure (that is, {@code min\{len1,len2\}}).</em>
         *
         * Atom sets must be pre-rotated.
         *
         * <p>
         * Citation:<br/>
         * <i>Zhang Y and Skolnick J (2004). "Scoring function for automated
         * assessment of protein structure template quality". Proteins 57: 702 -
         * 710.</i>
         *
         * @param atomSet1
         *            atom array 1
         * @param atomSet2
         *            atom array 2
         * @param len1
         *            The full length of the protein supplying atomSet1
         * @param len2
         *            The full length of the protein supplying atomSet2
         * @return The TM-Score
         * @throws StructureException
         */
        public static double getTMScore(Atom[] atomSet1, Atom[] atomSet2, int len1, int len2) throws StructureException {
                return getTMScore(atomSet1, atomSet2, len1, len2,true);
        }

        /**
         * Calculate the TM-Score for the superposition.
         *
         * Atom sets must be pre-rotated.
         *
         * <p>
         * Citation:<br/>
         * <i>Zhang Y and Skolnick J (2004). "Scoring function for automated
         * assessment of protein structure template quality". Proteins 57: 702 -
         * 710.</i>
         *
         * @param atomSet1
         *            atom array 1
         * @param atomSet2
         *            atom array 2
         * @param len1
         *            The full length of the protein supplying atomSet1
         * @param len2
         *            The full length of the protein supplying atomSet2
         * @param normalizeMin
         *            Whether to normalize by the <strong>minimum</strong>-length structure,
         *            that is, {@code min\{len1,len2\}}. If false, normalized by the {@code max\{len1,len2\}}).
         *
         * @return The TM-Score
         * @throws StructureException
         */
        public static double getTMScore(Atom[] atomSet1, Atom[] atomSet2, int len1,
                        int len2, boolean normalizeMin) throws StructureException {
                if (atomSet1.length != atomSet2.length) {
                        throw new StructureException(
                                        "The two atom sets are not of same length!");
                }
                if (atomSet1.length > len1) {
                        throw new StructureException(
                                        "len1 must be greater or equal to the alignment length!");
                }
                if (atomSet2.length > len2) {
                        throw new StructureException(
                                        "len2 must be greater or equal to the alignment length!");
                }

                int Lnorm;
                if (normalizeMin) {
                        Lnorm = Math.min(len1, len2);
                } else {
                        Lnorm = Math.max(len1, len2);
                }

                int Laln = atomSet1.length;

                double d0 = 1.24 * Math.cbrt(Lnorm - 15.) - 1.8;
                double d0sq = d0 * d0;

                double sum = 0;
                for (int i = 0; i < Laln; i++) {
                        double d = Calc.getDistance(atomSet1[i], atomSet2[i]);
                        sum += 1. / (1 + d * d / d0sq);
                }

                return sum / Lnorm;
        }
}