/*
 * Copyright (C) 2011-2015, Peter Abeles. All Rights Reserved.
 *
 * This file is part of Geometric Regression Library (GeoRegression).
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

package georegression.struct.se;

import georegression.geometry.ConvertRotation3D_F64;
import georegression.struct.EulerType;
import georegression.struct.affine.Affine2D_F64;
import georegression.struct.point.Vector3D_F64;
import org.ejml.data.DenseMatrix64F;
import org.ejml.ops.CommonOps;


/**
 * Various operations related to {@link SpecialEuclidean} transformations.
 *
 * @author Peter Abeles
 */
public class SpecialEuclideanOps_F64 {

	/**
	 * Sets the provided transform so that it does not transform any points.
	 *
	 * @param se The transform which is to be set to no motion.
	 */
	public static void setToNoMotion( Se3_F64 se ) {
		CommonOps.setIdentity( se.getR() );
		se.getT().set( 0, 0, 0 );
	}

	/**
	 * Converts {@link Se2_F64} into {@link Affine2D_F64}.
	 * @param se (Input) Se2
	 * @param affine (Output) Equivalent affine.  If null a new object will be declared.
	 * @return Equivalent affine.
	 */
	public static Affine2D_F64 toAffine( Se2_F64 se , Affine2D_F64 affine ) {
		if( affine == null )
			affine = new Affine2D_F64();

		affine.a11 = se.c;
		affine.a12 = -se.s;
		affine.a21 = se.s;
		affine.a22 = se.c;

		affine.tx = se.T.x;
		affine.ty = se.T.y;

		return affine;
	}

	/**
	 * Converts it into a 4 by 4 homogeneous matrix.
	 *
	 * @param se  original 3D transform
	 * @param ret Where the results will be written to.  If null a new matrix is declared. Modified.
	 * @return equivalent homogeneous transform.
	 */
	public static DenseMatrix64F toHomogeneous( Se3_F64 se, DenseMatrix64F ret ) {
		if( ret == null )
			ret = new DenseMatrix64F( 4, 4 );
		else {
			ret.set( 3, 0, 0 );
			ret.set( 3, 1, 0 );
			ret.set( 3, 2, 0 );
		}

		CommonOps.insert( se.getR(), ret, 0, 0 );
		Vector3D_F64 T = se.getT();

		ret.set( 0, 3, T.x );
		ret.set( 1, 3, T.y );
		ret.set( 2, 3, T.z );
		ret.set( 3, 3, 1 );

		return ret;
	}

	/**
	 * Converts a homogeneous representation into {@link Se3_F64}.
	 *
	 * @param H   Homogeneous 4 by 4 matrix.
	 * @param ret If not null where the results are written to.
	 * @return Se3_F64 transform.
	 */
	public static Se3_F64 toSe3_F64( DenseMatrix64F H, Se3_F64 ret ) {
		if( H.numCols != 4 || H.numRows != 4 )
			throw new IllegalArgumentException( "The homogeneous matrix must be 4 by 4 by definition." );

		if( ret == null )
			ret = new Se3_F64();

		ret.setTranslation( (double) H.get( 0, 3 ), (double) H.get( 1, 3 ), (double) H.get( 2, 3 ) );

		CommonOps.extract( H, 0, 3, 0, 3, ret.getR(), 0, 0 );

		return ret;
	}

	/**
	 * Converts it into a 3 by 3 homogeneous matrix.
	 *
	 * @param se  original 2D transform
	 * @param ret Where the results will be written to.  If null a new matrix is declared. Modified.
	 * @return equivalent homogeneous transform.
	 */
	public static DenseMatrix64F toHomogeneous( Se2_F64 se, DenseMatrix64F ret ) {
		if( ret == null )
			ret = new DenseMatrix64F( 3, 3 );
		else {
			ret.set( 2, 0, 0 );
			ret.set( 2, 1, 0 );
		}

		final double c = se.getCosineYaw();
		final double s = se.getSineYaw();

		ret.set( 0, 0, c );
		ret.set( 0, 1, -s );
		ret.set( 1, 0, s );
		ret.set( 1, 1, c );
		ret.set( 0, 2, se.getX() );
		ret.set( 1, 2, se.getY() );
		ret.set( 2, 2, 1 );

		return ret;
	}

	/**
	 * Converts a homogeneous representation into {@link Se2_F64}.
	 *
	 * @param H   Homogeneous 3 by 3 matrix.
	 * @param ret If not null where the results are written to.
	 * @return Se3_F64 transform.
	 */
	public static Se2_F64 toSe2( DenseMatrix64F H, Se2_F64 ret ) {
		if( H.numCols != 3 || H.numRows != 3 )
			throw new IllegalArgumentException( "The homogeneous matrix must be 3 by 3 by definition." );

		if( ret == null )
			ret = new Se2_F64();

		ret.setTranslation( (double) H.get( 0, 2 ), (double) H.get( 1, 2 ) );

		double c = (double) H.get( 0, 0 );
		double s = (double) H.get( 1, 0 );

		ret.setYaw( Math.atan2( s, c ) );

		return ret;
	}

	/**
	 * Sets the value of an {@link Se3_F64} using Euler XYZ coordinates for the rotation and
	 * a translation vector.
	 *
	 * @param rotX Rotation around X axis.
	 * @param rotY Rotation around Y axis.
	 * @param rotZ Rotation around Z axis.
	 * @param dx   Translation along x-axis.
	 * @param dy   Translation along y-axis.
	 * @param dz   Translation along z-axis.
	 * @param se   If not null then the transform is written here.
	 * @return The transform.
	 */
	public static Se3_F64 setEulerXYZ( double rotX, double rotY, double rotZ,
									   double dx, double dy, double dz,
									   Se3_F64 se ) {
		if( se == null )
			se = new Se3_F64();

		ConvertRotation3D_F64.eulerToMatrix(EulerType.XYZ, rotX, rotY, rotZ, se.getR() );
		Vector3D_F64 T = se.getT();
		T.x = dx;
		T.y = dy;
		T.z = dz;

		return se;
	}
}