/*******************************************************************************
 * Copyright (c) 2007- VTT Technical Research Centre of Finland.
 * All rights reserved. This program and the accompanying materials
 * are made available under the terms of the Eclipse Public License v1.0
 * which accompanies this distribution, and is available at
 * http://www.eclipse.org/legal/epl-v10.html
 *
 * Contributors:
 *     VTT Technical Research Centre of Finland - initial API and implementation
 *******************************************************************************/
package org.simantics.proconf.g3d.base;

import javax.vecmath.AxisAngle4d;
import javax.vecmath.Matrix3d;
import javax.vecmath.Point3d;
import javax.vecmath.Quat4d;
import javax.vecmath.Tuple3d;
import javax.vecmath.Vector3d;

import com.jme.math.Vector2f;

/**
 * Some useful geometry related math functions. Beware, methods may modify their input parameters!
 * 
 * @author Marko Luukkainen
 *
 */
public class MathTools {
    
    static double EPS = 0.001;
    
    public static Vector3d closestPointOnEdge(Vector3d point, Vector3d edgePoint1, Vector3d edgePoint2) {
        point.sub(edgePoint1);
        Vector3d v = new Vector3d(edgePoint2);
        v.sub(edgePoint1);
        double t = v.dot(point);
        t /= v.lengthSquared();
        if (t <= 0.0f)
          return edgePoint1;
        if (t >= 1.0f)
          return edgePoint2;
        v.scale(t);
        v.add(edgePoint1);
        return v;   
    }
    
    public static Vector3d closestPointOnStraight(Point3d point, Point3d straightPoint, Vector3d straightDir) {
        Vector3d v = new Vector3d(point);
        v.sub(straightPoint);
        double t = straightDir.dot(v);
        t /= straightDir.lengthSquared();
        v.set(straightDir);
        v.scale(t);
        v.add(straightPoint);
        return v;   
    }
    
    public static Vector3d closestPointOnStraight(Point3d point, Point3d straightPoint, Vector3d straightDir, double u[]) {
        Vector3d v = new Vector3d(point);
        v.sub(straightPoint);
        u[0] = straightDir.dot(v);
        u[0] /= straightDir.lengthSquared();
        v.set(straightDir);
        v.scale(u[0]);
        v.add(straightPoint);
        return v;   
    }
    
    public static double distanceFromPlane(Vector3d point, Vector3d planeNormal, Tuple3d planePoint) {
        point.sub(planePoint);
        
        return planeNormal.dot(point);
    }
      
    public static double distanceFromPlane(Vector3d point, Vector3d planeNormal, float d) {
        return (planeNormal.dot(point) + d);
    }
    
    public static boolean intersectStraightPlane(Tuple3d linePoint, Vector3d lineDir, Tuple3d planePoint, Vector3d planeNormal, Tuple3d intersectPoint) {
        intersectPoint.set(planePoint);
        intersectPoint.sub(linePoint);
        double u = planeNormal.dot(new Vector3d(intersectPoint));
        double v = planeNormal.dot(lineDir);
        if (Math.abs(v) < EPS)
            return false;
        u /= v;
        intersectPoint.set(lineDir);
        intersectPoint.scale(u);
        intersectPoint.add(linePoint);
        return true;
    }
    
    public static boolean intersectStraightPlane(Tuple3d linePoint, Vector3d lineDir, Tuple3d planePoint, Vector3d planeNormal, Vector3d intersectPoint, double[] u) {
        intersectPoint.set(planePoint);
        intersectPoint.sub(linePoint);
        u[0] = planeNormal.dot(intersectPoint);
        double v = planeNormal.dot(lineDir);
        if (Math.abs(v) < EPS)
            return false;
        u[0] /= v;
        intersectPoint.set(lineDir);
        intersectPoint.scale(u[0]);
        intersectPoint.add(linePoint);
        return true;
    }
    
    public static boolean intersectLineLine(Vector3d p1,Vector3d p2,Vector3d p3,Vector3d p4,Vector3d pa,Vector3d pb) {
            Vector3d p13 = new Vector3d();
            Vector3d p43 = new Vector3d();
            Vector3d p21 = new Vector3d();
            double d1343,d4321,d1321,d4343,d2121;
            double numer,denom;
            p13.sub(p1, p3);
            p43.sub(p4,p3);
            if (Math.abs(p43.x)  < EPS && Math.abs(p43.y)  < EPS && Math.abs(p43.z)  < EPS)
               return false;
            p21.sub(p2,p1);
            if (Math.abs(p21.x)  < EPS && Math.abs(p21.y)  < EPS && Math.abs(p21.z)  < EPS)
               return false;

            d1343 = p13.dot(p43);
            d4321 = p43.dot(p21);
            d1321 = p13.dot(p21);
            d4343 = p43.lengthSquared();
            d2121 = p21.lengthSquared();

            denom = d2121 * d4343 - d4321 * d4321;
            if (Math.abs(denom) < EPS)
               return false;
            numer = d1343 * d4321 - d1321 * d4343;

            double mua = numer / denom;
            double mub = (d1343 + d4321 * mua) / d4343;
 
            pa.x = p1.x + mua * p21.x;
            pa.y = p1.y + mua * p21.y;
            pa.z = p1.z + mua * p21.z;
            pb.x = p3.x + mub * p43.x;
            pb.y = p3.y + mub * p43.y;
            pb.z = p3.z + mub * p43.z;

            return true;
    }
    
    public static boolean intersectStraightStraight(Vector3d p1,Vector3d p21,Vector3d p3,Vector3d p43,Tuple3d pa,Tuple3d pb) {
        Vector3d p13 = new Vector3d();

        double d1343,d4321,d1321,d4343,d2121;
        double numer,denom;
        
        p13.sub(p1, p3);
        if (Math.abs(p43.x)  < EPS && Math.abs(p43.y)  < EPS && Math.abs(p43.z)  < EPS)
           return false;
        if (Math.abs(p21.x)  < EPS && Math.abs(p21.y)  < EPS && Math.abs(p21.z)  < EPS)
           return false;

        d1343 = p13.dot(p43);
        d4321 = p43.dot(p21);
        d1321 = p13.dot(p21);
        d4343 = p43.lengthSquared();
        d2121 = p21.lengthSquared();

        denom = d2121 * d4343 - d4321 * d4321;
        if (Math.abs(denom) < EPS)
           return false;
        numer = d1343 * d4321 - d1321 * d4343;

        double mua = numer / denom;
        double mub = (d1343 + d4321 * mua) / d4343;

        pa.x = p1.x + mua * p21.x;
        pa.y = p1.y + mua * p21.y;
        pa.z = p1.z + mua * p21.z;
        pb.x = p3.x + mub * p43.x;
        pb.y = p3.y + mub * p43.y;
        pb.z = p3.z + mub * p43.z;

        return true;
   }
    
    /**
     * Calculate the line segment PaPb that is the shortest route between
     *  two lines P1P2 and P3P4. Calculate also the values of mua and mub where
     *  Pa = P1 + mua (P2 - P1)
     *  Pb = P3 + mub (P4 - P3)
     * @param p1
     * @param p21
     * @param p3
     * @param p43
     * @param pa
     * @param pb
     * @param mu
     * @return
     */
    public static boolean intersectStraightStraight(Tuple3d p1,Vector3d p21,Tuple3d p3,Vector3d p43,Tuple3d pa,Tuple3d pb, double mu[]) {
        Vector3d p13 = new Vector3d();

        double d1343,d4321,d1321,d4343,d2121;
        double numer,denom;
        double EPS = 0.001;
        p13.sub(p1, p3);
        if (Math.abs(p43.x)  < EPS && Math.abs(p43.y)  < EPS && Math.abs(p43.z)  < EPS)
           return false;
        if (Math.abs(p21.x)  < EPS && Math.abs(p21.y)  < EPS && Math.abs(p21.z)  < EPS)
           return false;

        d1343 = p13.dot(p43);
        d4321 = p43.dot(p21);
        d1321 = p13.dot(p21);
        d4343 = p43.lengthSquared();
        d2121 = p21.lengthSquared();

        denom = d2121 * d4343 - d4321 * d4321;
        if (Math.abs(denom) < EPS)
           return false;
        numer = d1343 * d4321 - d1321 * d4343;

        mu[0] = numer / denom;
        mu[1] = (d1343 + d4321 * mu[0]) / d4343;

        pa.x = p1.x + mu[0] * p21.x;
        pa.y = p1.y + mu[0] * p21.y;
        pa.z = p1.z + mu[0] * p21.z;
        pb.x = p3.x + mu[1] * p43.x;
        pb.y = p3.y + mu[1] * p43.y;
        pb.z = p3.z + mu[1] * p43.z;

        return true;
   }
   
   public static AxisAngle4d getFromEuler2(Vector3d euler) {
       AxisAngle4d aa = new AxisAngle4d();
       aa.angle = euler.length();
       Vector3d normal = new Vector3d(euler);
       if (aa.angle > EPS) {
           normal.normalize();
           aa.x = normal.x;
           aa.y = normal.y;
           aa.z = normal.z;
       } else {
           aa.x = 1.0;
           aa.y = 0.0;
           aa.z = 0.0;
       }
       
       return aa;
   }
   
   public static Vector3d getEuler(AxisAngle4d aa) {
       Vector3d euler = new Vector3d(aa.x,aa.y,aa.z);
       euler.scale(aa.angle);
       return euler;
   }
   
   public static void rotate(Quat4d q, Tuple3d in, Tuple3d out) {
       // p' = q * p * q'
       double tw =           - q.x*in.x - q.y*in.y - q.z*in.z;
       double tx =  q.w*in.x            + q.y*in.z - q.z*in.y;
       double ty =  q.w*in.y - q.x*in.z            + q.z*in.x;
       double tz =  q.w*in.z + q.x*in.y - q.y*in.x           ;
       
       //temp * q' -> x = -x, y = -y z = -z
       //out.w = tw*q.w + tx*q.x + ty*q.y + tz*q.z;
       out.x =  -tw*q.x + tx*q.w - ty*q.z + tz*q.y;
       out.y =  -tw*q.y + tx*q.z + ty*q.w - tz*q.x;
       out.z =  -tw*q.z - tx*q.y + ty*q.x + tz*q.w;  
   }
   
   public static void getMatrix(Quat4d quat, Matrix3d m) {
		   m.m00 = 1.0f - 2.0 * (quat.y * quat.y + quat.z * quat.z);
		   m.m01 = 2.0 * (quat.x * quat.y + quat.w * quat.z);
		   m.m02 = 2.0 * (quat.x * quat.z - quat.w * quat.y);
		   m.m10 = 2.0 * (quat.x * quat.y - quat.w * quat.z);
		   m.m11 = 1.0 - 2.0f * (quat.x * quat.x + quat.z * quat.z);
		   m.m12 = 2.0 * (quat.y * quat.z + quat.w * quat.x);
		   m.m20 = 2.0 * (quat.x * quat.z + quat.w * quat.y);
		   m.m21 = 2.0 * (quat.y * quat.z - quat.w * quat.x);
		   m.m22 = 1.0 - 2.0f * (quat.x * quat.x + quat.y * quat.y);

   }
   
   public static void getQuat(Matrix3d mat, Quat4d quat) {
	   double tr = mat.m00 + mat.m11 + mat.m22;
		if (tr > 0.0) {
			double s = Math.sqrt(tr + 1.0);
			quat.w = 0.5 * s;
			s = 0.5 / s;
			quat.x = (mat.m21 - mat.m12) * s;
			quat.y = (mat.m02 - mat.m20) * s;
			quat.z = (mat.m10 - mat.m01) * s;
		} else {
			int i = 0, j, k;
			if (mat.m11 > mat.m00)
				i = 1;
			if (mat.m22 > mat.getElement(i, i))
				i = 2;
			int nxt[] = { 1, 2, 0 };

			j = nxt[i];
			k = nxt[j];

			double s = Math
					.sqrt((mat.getElement(i, i) - (mat.getElement(j, j) + mat
							.getElement(k, k))) + 1.0);

			double q[] = new double[3];
			q[i] = s * 0.5;

			if (Math.abs(s) > 0.001)
				s = 0.5 / s;

			quat.w = (mat.getElement(k, j) - mat.getElement(j, k)) * s;
			q[j] = (mat.getElement(j, i) + mat.getElement(i, j)) * s;
			q[k] = (mat.getElement(k, i) + mat.getElement(i, k)) * s;

			quat.x = q[0];
			quat.y = q[1];
			quat.z = q[2];
		}
	}
   
   
	/*
	 * Cohen-Sutherland
	 */
	
	private static final int IN = 0;
	private static final int LEFT = 1;
	private static final int RIGHT = 2;
	private static final int BOTTOM = 4;
	private static final int TOP = 8;
	
	
	private static int bitcode(Vector2f p1, Vector2f min, Vector2f max) {
		int code = IN;
		if (p1.x < min.x)
			code |= LEFT;
		else if (p1.x > max.x)
			code |= RIGHT;
		if (p1.y < min.y)
			code |= BOTTOM;
		else if (p1.y > max.y)
			code |= TOP;
		return code;
	}
	
	public static boolean clipLineRectangle(Vector2f p1,Vector2f p2, Vector2f min, Vector2f max, Vector2f r1, Vector2f r2) {
		while (true) {
			int o1 = bitcode(p1, min, max);
			int o2 = bitcode(p2, min, max);
			int and = o1 & o2;
			int or = o1 | o2;
			if (and != IN) {
				return false;
			}
			if (or == IN) {
				r1.set(p1);
				r2.set(p2);
				return true;
			}
			if (o1 == IN) {
				Vector2f t = p1;
				p1 = p2;
				p2 = t;
				int t2 = o1;
				o1 = o2;
				o2 = t2;
			}
			if ((o1 & TOP) != IN) {
				float t = (max.y - p1.y) / (p2.y - p1.y);
				p1.x += t * (p2.x - p1.x);
				p1.y = max.y;
			} else if ((o1 & BOTTOM) != IN) {
				float t = (min.y - p1.y) / (p2.y - p1.y);
				p1.x += t * (p2.x - p1.x);
				p1.y = min.y;
			} else if ((o1 & LEFT) != IN) {
				float t = (min.x - p1.x) / (p2.x - p1.x);
				p1.y += t * (p2.y - p1.y);
				p1.x = min.x;
			} else if ((o1 & RIGHT) != IN) {
				float t = (max.x - p1.x) / (p2.x - p1.x);
				p1.y += t * (p2.y - p1.y);
				p1.x = max.x;
			} else {
				throw new RuntimeException("Error in clipping code");
			}
		}
		
	}
	
	public static double square(double d) {
		return d * d;
	}
}