/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package accel.tests;

import com.jme3.math.FastMath;
import com.jme3.math.Quaternion;
import com.jme3.math.Vector3f;
import java.math.BigDecimal;

/**
 *
 * @author leo
 */
public class PrecisionTest2 {

    public static Vector3f multLocal(Quaternion q, Vector3f v) {
        float x = q.getX();
        float y = q.getY();
        float z = q.getZ();
        float w = q.getW();

        float vx = y*v.z-z*v.y;
        float vy = z*v.x-x*v.z;
        float vz = x*v.y-y*v.x;
        vx += vx; vy += vy; vz += vz;
        v.x += w*vx + y*vz-z*vy;
        v.y += w*vy + z*vx-x*vz;
        v.z += w*vz + x*vy-y*vx;

        return v;
    }
    public static void mult15(Quaternion q, Vector3f v, Vector3f s) {
        //15 mult, 15 add
        float x = q.getX();
        float y = q.getY();
        float z = q.getZ();
        float w = q.getW();

        float vx = y*v.z-z*v.y;
        float vy = z*v.x-x*v.z;
        float vz = x*v.y-y*v.x;
        vx += vx; vy += vy; vz += vz;
        s.x = v.x + w*vx + y*vz-z*vy;
        s.y = v.y + w*vy + z*vx-x*vz;
        s.z = v.z + w*vz + x*vy-y*vx;
    }
    public static void main(String[] args) {
        silent = true; //set to false to see outout
        int iterations = 100; // number of iterations

        Quaternion q = new Quaternion();
        q.fromAngleNormalAxis(0.4547f, new Vector3f(0.5454f,0.21f,0.12f).normalizeLocal());
        PrecisionTest.QuaternionHD qHD = new PrecisionTest.QuaternionHD(q.getX(),q.getY(),
                q.getZ(), q.getW());



        Vector3f resHD15 = new Vector3f();
        Vector3f resHD60 = new Vector3f();
        Vector3f res15 = new Vector3f();
        Vector3f res60 = new Vector3f();
        Vector3f v = new Vector3f(128.55f,5145.48f,1351.54f);

        double totalError15=0, totalError60=0;

        int score15 = 0, score60 = 0, draw = 0;
        for(int i = 0; i < iterations; i++) {
           out("Quaternion \t" + q);
           out("QuaternionHD \t" + qHD);
           out("v " + v);
           //compute HighPrecision result using both methods
           qHD.mult15(v, resHD15);
           qHD.mult60(v, resHD60);




           out("HD15 " + resHD15);

           out("HD60 " + resHD60);

            q.mult(v, res60);

            out("Local: " +multLocal(q, new Vector3f(v)));
            out("LocalQ: " + q.multLocal(new Vector3f(v)));

//           mult15V2(q, v, res60);
            mult15(q, v, res15);

            out("mult15 " + res15);
            out("mult15V2 " + res60);

           if(resHD15.x != resHD60.x || resHD15.y != resHD60.y || resHD15.z != resHD60.z) {
              out("*HD Difference*");
               //resD15 != resD60
               //now the question is which one is more correct, resD15 or resD60
               //eg. use mathematical program to check which one produces better results
           }

           out("Comparing---------------");
            out("diff15 " + resHD15.subtract(res15));
            out("diff15V2 " + resHD15.subtract(res60));

            float e15 = error(resHD15.subtract(res15));
            float e60 = error(resHD15.subtract(res60));
            totalError15 += e15;
            totalError60 += e60;
            if(e15 < e60) score15++;
            else if(e60 < e15) {
                score60++; out("qqqq");
            }
            else draw++;

            v.set(FastMath.nextRandomFloat()*10000f,
                    FastMath.nextRandomFloat()*10000f,
                    FastMath.nextRandomFloat()*10000f);
            q.fromAngleNormalAxis(FastMath.PI*FastMath.nextRandomFloat(),
                    new Vector3f(2*FastMath.nextRandomFloat()-1f,
                    2*FastMath.nextRandomFloat()-1f,
                    2*FastMath.nextRandomFloat()-1f).normalizeLocal());
            out("-----------------------");
            qHD = new PrecisionTest.QuaternionHD(q.getX(),q.getY(),
                q.getZ(), q.getW());
        }
        out("-----------------------");
        System.out.println("Results: (15,60,Draws): " + score15+","+score60+","+draw);
        if(score15 > score60)
            System.out.println("mult15 is more precise");
        else if(score60 > score15)
            System.out.println("mult15V2 is more precise");
        else System.out.println("Draw");

        double avgError15 = totalError15/iterations;
        double avgError60 = totalError60/iterations;
        System.out.println("Average Error mult15: " + avgError15);
        System.out.println("Average Error mult60: " + avgError60);
    }
    static boolean silent = false;
    public static void out(String msg) {
        if(silent) return;
        System.out.println(msg);
    }
    private static float error(Vector3f v) {
        return FastMath.abs(v.x)+FastMath.abs(v.y)+FastMath.abs(v.z);
    }
    public static class QuaternionHD  {
        public BigDecimal x,y,z,w;
        public QuaternionHD(float x, float y, float z, float w) {
            this.x = new BigDecimal(x+"");
            this.y = new BigDecimal(y+"");
            this.z = new BigDecimal(z+"");
            this.w = new BigDecimal(w+"");
        }
         public void mult15(Vector3f v, Vector3f store) {
            //15 mult, 15 add
            BigDecimal ix = new BigDecimal(v.x+"");
            BigDecimal iy = new BigDecimal(v.y+"");
            BigDecimal iz = new BigDecimal(v.z+"");

            BigDecimal vx = y.multiply(iz).subtract(z.multiply(iy));
            BigDecimal vy = z.multiply(ix).subtract(x.multiply(iz));
            BigDecimal vz = x.multiply(iy).subtract(y.multiply(ix));

            vx = vx.add(vx); vy = vy.add(vy); vz = vz.add(vz);

            BigDecimal sx = ix.add(w.multiply(vx)).add(y.multiply(vz)).subtract(z.multiply(vy));
            BigDecimal sy = iy.add(w.multiply(vy)).add(z.multiply(vx)).subtract(x.multiply(vz));
            BigDecimal sz = iz.add(w.multiply(vz)).add(x.multiply(vy)).subtract(y.multiply(vx));
//            out("precision: "+sx.precision()+", " + sy.precision()+ ", " + sz.precision());
//            out("Result15: "+sx.toPlainString()+", "+sy.toPlainString()+", "+sz.toPlainString());
            store.set(sx.floatValue(), sy.floatValue(), sz.floatValue());
         }
         public void mult60(Vector3f v, Vector3f store) {
            BigDecimal vx = new BigDecimal(v.x+"");
            BigDecimal vy = new BigDecimal(v.y+"");
            BigDecimal vz = new BigDecimal(v.z+"");
            BigDecimal n2 = new BigDecimal("2");

            BigDecimal sx =  w.multiply( w) .multiply( vx)
                    .add(n2 .multiply( y) .multiply( w) .multiply( vz))
                    .subtract(n2 .multiply( z) .multiply( w) .multiply( vy))
                    .add( x .multiply( x).multiply( vx))
                    .add( n2 .multiply( y) .multiply( x ).multiply( vy))
                    .add( n2 .multiply( z) .multiply( x) .multiply( vz))
                    .subtract( z .multiply( z) .multiply( vx))
                    .subtract(y.multiply( y) .multiply( vx));
            BigDecimal sy = n2 .multiply( x) .multiply( y) .multiply( vx)
                    .add( y .multiply( y) .multiply( vy))
                    .add( n2 .multiply( z) .multiply( y) .multiply( vz))
                    .add( n2 .multiply( w).multiply( z) .multiply( vx))
                    .subtract( z .multiply( z) .multiply( vy))
                    .add( w .multiply( w) .multiply( vy))
                    .subtract( n2 .multiply( x) .multiply( w) .multiply( vz))
                    .subtract( x .multiply( x) .multiply( vy));
            BigDecimal sz = n2 .multiply( x) .multiply( z) .multiply( vx)
                    .add( n2 .multiply( y) .multiply( z) .multiply( vy))
                    .add( z .multiply( z) .multiply( vz))
                    .subtract( n2 .multiply( w).multiply( y) .multiply( vx))
                    .subtract( y .multiply( y) .multiply( vz))
                    .add(n2 .multiply( w) .multiply( x) .multiply( vy))
                    .subtract( x .multiply( x) .multiply( vz))
                    .add( w.multiply( w) .multiply( vz));
//             out("precision: "+sx.precision()+", " + sy.precision()+ ", " + sz.precision());
//             out("Result60: "+sx.toPlainString()+", "+sy.toPlainString()+", "+sz.toPlainString());
            store.set(sx.floatValue(), sy.floatValue(), sz.floatValue());
         }
        @Override
        public String toString() {
            String s = "[";
            s += x.toPlainString()+", ";
            s += y.toPlainString()+", ";
            s += z.toPlainString()+", ";
            s += w.toPlainString()+"]";
            return s;
        }

    }
}