package cz.cuni.amis.pogamut.base3d.worldview.objects;

import javax.vecmath.Point3d;

/**
 * Rotation within the world.
 *
 * Rotation is represented as yaw, roll and pitch.
 *
 * FIXME[js]: Add working methods and consider imports from Tuple3d.
 *
 * @author Juraj 'Loque' Simlovic
 */
public class Rotation implements IRotable
{
	/**
	 * Rotation yaw. Yaw is rotation to the left or right. E.g. turn left.
	 * The value ranges from 0..65535.
	 */
	public double yaw = 0;
	/**
	 * Rotation roll. Roll is twist of head. E.g. Tilt the head to shoulder.
	 * The value ranges from 0..65535.
	 */
	public double roll = 0;
	/**
	 * Rotation pitch. Pitch is rotation up and down. E.g. look down.
	 * The value ranges from 0..65535.
	 */
	public double pitch = 0;

	/* ********************************************************************** */

	/**
	 * Rotation yaw. Yaw is rotation to the left or right. E.g. turn left.
	 * @return Rotation yaw. The value ranges from 0..65535.
	 */
	public double getYaw()
	{
		return yaw;
	}

	/**
	 * Rotation pitch. Pitch is rotation up and down. E.g. look down.
	 * @return Rotation pitch. The value ranges from 0..65535.
	 */
	public double getPitch()
	{
		return pitch;
	}

	/**
	 * Rotation roll. Roll is twist of head. E.g. Tilt the head to shoulder.
	 * @return Rotation roll. The value ranges from 0..65535.
	 */
	public double getRoll()
	{
		return roll;
	}

	/* ********************************************************************** */

	/**
	 * Retreives the rotation itself to implement {@link IRotable}.
	 * @return The rotation itself (note: does not create a copy).
	 */
	@Override
	public Rotation getRotation()
	{
		return this;
	}

	/**
	 * Retreives javax.vecmath.Point3d representation of the rotation.
	 * @return javax.vecmath.Point3d representation with x, y and z values set.
	 */
	public Point3d getPoint3d()
	{
		return new Point3d (pitch, yaw, roll);
	}
             
	/* ********************************************************************** */

	/**
	 * Creates rotation with all values set to zeroes.
	 */
	public Rotation()
	{
	}

	/**
	 * Creates rotation with specified values.
	 * @param yaw Rotation yaw.
	 * @param roll Rotation roll.
	 * @param pitch Rotation pitch.
	 */
	public Rotation(double yaw, double roll, double pitch)
	{
		this.yaw = yaw;
		this.roll = roll;
		this.pitch = pitch;
	}
	
    public static final double LinearInterp(double a, double b, double alpha) {
    	return a + (b-a) * alpha;
    }
    
    public static final double LogInterp(double a, double b, double alpha) {    	
    	return a + (b-a) * Math.log(1 + alpha * (Math.E - 1));
    }
    
    public static final double ExpInterp(double a, double b, double alpha) {
    	// (0,1);(1,e)
    	return a + (b-a) * (Math.exp(alpha) - 1)/(Math.E - 1);
    }
    
    public static final Rotation RotationLinearInterp(Rotation a, Rotation b, double alpha) {
    	return new Rotation(
    			LinearInterp(a.yaw, b.yaw, alpha),
    			LinearInterp(a.roll, b.roll, alpha),
    			LinearInterp(a.pitch, b.pitch, alpha)
    	);
    }
    
    public final Rotation RotationLinearInterp(Rotation b, double alpha) {
    	return RotationLinearInterp(this, b, alpha);
    }     
    
    public final Rotation RotationLogInterp(Rotation b, double alpha) {
    	return RotationLogInterp(this, b, alpha);
    }
    
    public static final Rotation RotationLogInterp(Rotation a, Rotation b, double alpha) {
    	return new Rotation(
    			LogInterp(a.yaw, b.yaw, alpha),
    			LogInterp(a.roll, b.roll, alpha),
    			LogInterp(a.pitch, b.pitch, alpha)
    	);
    }
    
    public final Rotation RotationExpInterp(Rotation b, double alpha) {
    	return RotationExpInterp(this, b, alpha);
    }
    
    public static final Rotation RotationExpInterp(Rotation a, Rotation b, double alpha) {
    	return new Rotation(
    			ExpInterp(a.yaw, b.yaw, alpha),
    			ExpInterp(a.roll, b.roll, alpha),
    			ExpInterp(a.pitch, b.pitch, alpha)
    	);
    }   

	/* ********************************************************************** */

	@Override
	public String toString()
	{
		return String.format ("[%.2f, %.2f, %.2f]", yaw, roll, pitch);
	}

}
