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

import edu.uprm.walsaip.vte.core.util.GLUtils;


/**
 * 
 * @author Ricardo Veguilla (veguilla@ece.uprm.edu)
 * 
 */
public class Angle extends Vector {
	public static final int YAW = 0;
	public static final int PITCH = 1;
	public static final int ROLL = 2;



	public Angle() {
		super();
	}

	public Angle(float yaw, float pitch, float roll) {
		this();
		array[YAW] = GLUtils.range360(yaw);
		array[PITCH] = GLUtils.range360(pitch);
		array[ROLL] = GLUtils.range360(roll);
	}

	public void setAngles(float yaw, float pitch, float roll) {
		array[YAW] = GLUtils.range360(yaw);
		array[PITCH] = GLUtils.range360(pitch);
		array[ROLL] = GLUtils.range360(roll);
		
	}

	
	public void setFromArray(float[] angles) {
		
		array[YAW] = GLUtils.range360(angles[YAW]);
		array[PITCH] = GLUtils.range360(angles[PITCH]);
		array[ROLL] = GLUtils.range360(angles[ROLL]);
		
	}


	public void setYaw(float yaw) {
		array[YAW] = GLUtils.range360(yaw);
	}

	public void setPitch(float pitch) {
		array[PITCH] = GLUtils.range360(pitch);
		
		
	}

	public void setRoll(float roll) {
		array[ROLL] = GLUtils.range360(roll);
	}

	public void incrementYaw(float delta) {
		array[YAW] = GLUtils.range360(array[YAW] + delta);
		
		
	}

	public void incrementPitch(float delta) {
		array[PITCH] = GLUtils.range360(array[PITCH] + delta);
		
	}

	public void incrementRoll(float delta) {
		array[ROLL] = GLUtils.range360(array[ROLL] + delta);
		

	}


	public String toString() {
		return "YAW[" + array[YAW] + "] PITCH[" + array[PITCH]
				+ "] ROLL[" + array[ROLL]+"]";
	}
	
	public float getYaw() {
		return array[YAW];
	}
	public float getPitch() {
		return array[PITCH];
	}
	public float getRoll() {
		return array[ROLL];
	}
	
	public static Angle newInstance(Angle v) {
		Angle n = new Angle();
		n.array[0] = v.array[0];
		n.array[1] = v.array[1];
		n.array[2] = v.array[2];
		n.array[3] = v.array[3];
		return n;
	}
	
}
