package awa.imu;

import java.text.DecimalFormat;
import java.text.DecimalFormatSymbols;
import java.text.NumberFormat;
import java.util.Locale;

public class Euler 
{
    private static final DecimalFormatSymbols usfs = new DecimalFormatSymbols(Locale.US);
    //private static final NumberFormat msFormat = new DecimalFormat(" 0.0;-0.0", usfs);
    private static final NumberFormat msFormat = new DecimalFormat(" 0.000;-0.000", usfs);
    
    private static final double PI = Math.PI;
    private static final double PI2 = PI * 2;
    
    public double phi;
    public double theta;
    public double psi;
    
    public static Euler sub(Euler a, Euler b)
    {
        Euler result = new Euler();
        result.phi = normalizeRad(a.phi - b.phi);
        result.theta = normalizeRad(a.theta - b.theta);
        result.psi = normalizeRad(a.psi - b.psi);
        
        return result;
    }
    
    public static double toNormRad(double angdeg) 
    {
        if (angdeg > 180.0) angdeg -= 360.0;
        if (angdeg < -180.0) angdeg += 360.0;
        
        return angdeg / 180.0 * Math.PI;
    }
    
    public static double normalizeRad(double angrad)
    {
        if (angrad > Math.PI) angrad -= PI2;
        if (angrad < -Math.PI) angrad += PI2;
        
        return angrad;
    }
    
    public String toString()
    {
        StringBuffer sb = new StringBuffer();
        sb.append("phi:").append(msFormat.format(Math.toDegrees(phi)));
        sb.append(" theta:").append(msFormat.format(Math.toDegrees(theta)));
        sb.append(" psi:").append(msFormat.format(Math.toDegrees(psi)));
        //sb.append("phi:").append(msFormat.format(phi));
        //sb.append(" theta:").append(msFormat.format(theta));
        //sb.append(" psi:").append(msFormat.format(psi));
        
        return sb.toString();
    }

}