package cz.fidentis.analyst.icp;

import javax.vecmath.Point3d;
import javax.vecmath.Vector3d;

/**
 * IcpTransformation class is holding computed data for transformation.
 *
 * @author Maria Kocurekova
 */
public class IcpTransformation {

    private final Quaternion rotation;
    private final double scaleFactor;
    private final Vector3d translation;
    private final double meanD;

    /**
     * Constructor for IcpTransformation.
     *
     * @param translation Translation represents translation of ComputedFacet on x, y, z axis.
     * @param rotation Rotation is represented by Quaternion
     *                 (x, y, z coordinate and scalar component).
     * @param scaleFactor ScaleFactor represents scale between two objects.
     *                    In case there is no scale the value is 0.
     * @param meanD MeanD represents mean distance between objects.
     */
    public IcpTransformation(Vector3d translation, Quaternion rotation, double scaleFactor, double meanD) {
        this.translation = translation;
        this.scaleFactor = scaleFactor;
        this.rotation = rotation;
        this.meanD = meanD;
    }

    /**
     * Getter for rotation.
     * @return Return rotation parameter.
     */
    public Quaternion getRotation() {
        return rotation;
    }

    /**
     * Getter for scale.
     * @return Return scale factor.
     */
    public double getScaleFactor() {
        return scaleFactor;
    }

    /**
     * Getter for translation.
     * @return Return translation parameter.
     */
    public Vector3d getTranslation() {
        return translation;
    }

    /**
     * Getter for mean distance.
     * @return Return mean distance parameter.
     */
    public double getMeanD() {
        return meanD;
    }
    
    /**
     * Apply transformation to given 3D point.
     *
     * @param point Original point
     * @param scale Whether to scale as well
     * @return transformed point or {@code null}
     */
    public Point3d transformPoint(Point3d point, boolean scale) {
        if (point == null) {
            return null;
        }
        
        Quaternion rotQuat = new Quaternion(point.x, point.y, point.z, 1);
        Quaternion rotationCopy = Quaternion.multiply(rotQuat, getRotation().getConjugate());
        rotQuat = Quaternion.multiply(getRotation(), rotationCopy);
        
        if(scale && !Double.isNaN(getScaleFactor())) {
            return new Point3d(
                    rotQuat.x * getScaleFactor() + getTranslation().x,
                    rotQuat.y * getScaleFactor() + getTranslation().y,
                    rotQuat.z * getScaleFactor() + getTranslation().z
            );
        } else {
            return new Point3d(
                    rotQuat.x + getTranslation().x,
                    rotQuat.y + getTranslation().y ,
                    rotQuat.z + getTranslation().z
            );
        }
    }
}
