/**
 * 
 */
package edu.uprm.walsaip.vte.core.datatypes;


import static edu.uprm.walsaip.vte.core.datatypes.Angle.PITCH;
import static edu.uprm.walsaip.vte.core.datatypes.Angle.ROLL;
import static edu.uprm.walsaip.vte.core.datatypes.Angle.YAW;
import static edu.uprm.walsaip.vte.core.datatypes.Vertex.X;
import static edu.uprm.walsaip.vte.core.datatypes.Vertex.Y;
import static edu.uprm.walsaip.vte.core.datatypes.Vertex.Z;

/**
 * @author Ricardo Veguilla
 *
 */
public class Position {

	
	

	private Vector forward;

	private Vector right;

	private Vector up;
	
	private Angle angle;
	
	private Vertex position;
	
	
	public Position() {
		super();
		this.angle = new Angle(0.0f,0.0f,0.0f);
		this.position = new Vertex(0.0f,0.0f,0.0f);
		this.forward = new Vector(0.f,0.0f,1.0f);
		this.right = new Vector(1.f,0.0f,0.0f);
		this.up = new Vector(0.f,1.0f,0.0f);
		
		angleVectors();
	
	}
	
	public Position(Vertex position, Angle angle) {
		super();
		this.angle = angle;
		this.position = position;
		this.forward = new Vector(0.f,0.0f,1.0f);
		this.right = new Vector(1.f,0.0f,0.0f);
		this.up = new Vector(0.f,1.0f,0.0f);
		angleVectors();
	}
	
	/**
	 * @return Returns the forward.
	 */
	public Vector getForward() {
		return forward;
	}


	/**
	 * @return Returns the right.
	 */
	public Vector getRight() {
		return right;
	}



	/**
	 * @return Returns the up.
	 */
	public Vector getUp() {
		return up;
	}

	

	private void angleVectors() {

		
/**
		    r.x = cosY * cosR + sinY * sinP * sinR;
		    r.y = sinR * cosP;
		    r.z = cosY * sinP * sinR - sinY * cosR;

		    u.x = sinY * sinP * cosR - cosY * sinR;
		    u.y = cosR * cosP;
		    u.z = sinR * sinY + cosR * cosY * sinP;

		    f.x = cosP * sinY;
		    f.y = -sinP;
		    f.z = cosP * cosY;

			
		sinY = sine of yaw angle
		cosP = cosine of yaw angle
		sinP = sine of pitch angle
		cosP = cosine of pitch angle
		sinR = sine of roll angle
		cosR = cosine of roll angle
	*/
		double radAngle;

		double sin_pitch, sin_yaw, sin_roll, cos_pitch, cos_yaw, cos_roll;

		radAngle = Math.toRadians(angle.array[YAW]);
		sin_yaw = Math.sin(radAngle);
		cos_yaw = Math.cos(radAngle);

		radAngle = Math.toRadians(angle.array[PITCH]);
		sin_pitch = Math.sin(radAngle);
		cos_pitch = Math.cos(radAngle);

		radAngle = Math.toRadians(angle.array[ROLL]);
		sin_roll = Math.sin(radAngle);
		cos_roll = Math.cos(radAngle);

		forward.array[X] = (float) (cos_pitch * sin_yaw);
		forward.array[Y] = (float) (-sin_pitch);
		forward.array[Z] = (float) (cos_pitch * cos_yaw);

		up.array[X] = (float) ((cos_roll * sin_pitch * sin_yaw) - (sin_roll * cos_yaw));
		up.array[Y] = (float) (cos_roll * cos_pitch);
		up.array[Z] = (float) ((cos_roll * sin_pitch * cos_yaw) + (sin_roll * sin_yaw));

		right.array[X] = (float) ((sin_roll * sin_pitch * sin_yaw) + (cos_roll * cos_yaw));
		right.array[Y] = (float) (sin_roll * cos_pitch);
		right.array[Z] = (float) ((sin_roll * sin_pitch * cos_yaw) - (cos_roll * sin_yaw));

		
		
		forward.normalize();
		up.normalize();
		right.normalize();

	}

	/**
	 * @return Returns the angle.
	 */
	public Angle getAngle() {
		return angle;
	}

	/**
	 * @param angle The angle to set.
	 */
	public void setAngle(Angle angle) {
		this.angle = angle;
		angleVectors();
	}

	/**
	 * @return Returns the position.
	 */
	public Vertex getPosition() {
		return position;
	}

	/**
	 * @param position The position to set.
	 */
	public void setPosition(Vertex position) {
		this.position = position;
	}

	public void rotateAroundAxisX(float delta) {
		angle.incrementPitch(delta);
		angleVectors();
	}

	public void rotateAroundAxisY(float delta) {
		angle.incrementYaw(delta);
		angleVectors();
	}

	public void rotateAroundAxisZ(float delta) {
		angle.incrementRoll(delta);
		angleVectors();
	}

	@Override
	public String toString() {
		// TODO Auto-generated method stub
		return position.toString()+" "+angle.toString();
	}
	
}
