/*
 * Java port of Bullet (c) 2008 Martin Dvorak <jezek2@advel.cz>
 * 
 * ShapeHull implemented by John McCutchan.
 *
 * Bullet Continuous Collision Detection and Physics Library
 * Copyright (c) 2003-2008 Erwin Coumans  http://www.bulletphysics.com/
 *
 * This software is provided 'as-is', without any express or implied warranty.
 * In no event will the authors be held liable for any damages arising from
 * the use of this software.
 * 
 * Permission is granted to anyone to use this software for any purpose, 
 * including commercial applications, and to alter it and redistribute it
 * freely, subject to the following restrictions:
 * 
 * 1. The origin of this software must not be misrepresented; you must not
 *    claim that you wrote the original software. If you use this software
 *    in a product, an acknowledgment in the product documentation would be
 *    appreciated but is not required.
 * 2. Altered source versions must be plainly marked as such, and must not be
 *    misrepresented as being the original software.
 * 3. This notice may not be removed or altered from any source distribution.
 */

package com.bulletphysics.collision.shapes;

import com.bulletphysics.linearmath.MiscUtil;
import com.bulletphysics.linearmath.convexhull.*;
import com.bulletphysics.util.IntArrayList;
import com.bulletphysics.util.ObjectArrayList;
import cz.advel.stack.Stack;
import javax.vecmath.Vector3f;
import javax.vecmath.Vector3d;

/**
 * ShapeHull takes a {@link ConvexShape}, builds the convex hull using {@link HullLibrary}
 * and provides triangle indices and vertices.
 * 
 * @author jezek2
 */
public class ShapeHull  implements java.io.Serializable
{
    static final long serialVersionUID = -1102208318940158328L;

	protected ObjectArrayList<Vector3f> vertices = new ObjectArrayList<Vector3f>();
	protected ObjectArrayList<Vector3f> normals = new ObjectArrayList<Vector3f>();
	protected IntArrayList indices = new IntArrayList();
	protected int numIndices;
	protected ConvexShape shape;

	protected ObjectArrayList<Vector3f> unitSpherePoints = new ObjectArrayList<Vector3f>();

	public ShapeHull(ConvexShape shape) {
		this.shape = shape;
		this.vertices.clear();
		this.normals.clear();
		this.indices.clear();
		this.numIndices = 0;

//		MiscUtil.resize(unitSpherePoints, NUM_UNITSPHERE_POINTS+ConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS*2, Vector3f.class);
////		for (int i=constUnitSpherePoints.size(); --i>=0;) {
////			unitSpherePoints.getQuick(i).set(constUnitSpherePoints.getQuick(i));
////		}
//                int lines = (int)Math.sqrt(NUM_UNITSPHERE_POINTS);
//                int rows = lines;
//                Vector3f vec = new Vector3f();
//                Superellipsoid supere = new Superellipsoid();
//		for (int i=lines; --i>=0;)
//                {
//                    for (int j=rows; --j>=0;)
//                    {
//                        double u = i; u /= lines-1;
//                        double v = j; v /= rows-1;
//                        
//                        supere.biparamFunction(u, v, vec);
//                        
//                        unitSpherePoints.getQuick(i*rows + j).set(vec);
//                    }
//                }
	}

	public boolean buildHull(float margin)
        {
            int lines = (int)Math.sqrt(NUM_UNITSPHERE_POINTS);
            int rows = lines;
                
            MiscUtil.resize(vertices, NUM_UNITSPHERE_POINTS*2, Vector3f.class);
            MiscUtil.resize(normals, NUM_UNITSPHERE_POINTS*2, Vector3f.class);

            Vector3f vec = new Vector3f();
            Vector3f norm = new Vector3f();
            Vector3f normout = new Vector3f();
            Vector3f vec2 = new Vector3f();
            Superellipsoid supere = new Superellipsoid();
            
            CapsuleShape capsule = (CapsuleShape)shape;
            
            int loops = 1;
            
            if (capsule.anchor)
                loops++;
            
            for (int loop=loops; --loop>=0;)
                for (int j=rows; --j>=0;)
                {
                    int k=0;

                    for (int i=lines; --i>=0;)
                    {
                        double u = i; u /= lines-1;
                        double v = j; v /= rows-1;

                        supere.biparamFunction(u, v, vec, norm);

                        capsule.localGetSupportingVertexWithNorm(vec, vec2, norm, normout);
                        
                        if (loop == 1)
                        {
                            //vec2.scale(0.5f);
                            switch (capsule.getUpAxis())
                            {
                                case 0: vec2.x /= capsule.getHalfHeight(); vec2.x *= capsule.getRadius()/2; vec2.x -= capsule.getHalfHeight(); break;
                                case 1: vec2.y /= capsule.getHalfHeight(); vec2.y *= capsule.getRadius()/2; vec2.y -= capsule.getHalfHeight(); break;
                                case 2: vec2.z /= capsule.getHalfHeight(); vec2.z *= capsule.getRadius()/2; vec2.z -= capsule.getHalfHeight(); break;
                            }
                        }
                        vertices.getQuick(loop*NUM_UNITSPHERE_POINTS + i*rows + j).set(vec2);
                        normals.getQuick(loop*NUM_UNITSPHERE_POINTS + i*rows + j).set(normout);
                    }
                }
            
            numIndices = (lines*(rows-1) * 6) * loops;
            
            int i = 0;
            int j = 0;
            
            MiscUtil.resize(indices, numIndices, 0);
            for (int in=0; in<numIndices/loops;)
            {
                indices.set(in++, i*(rows-1) + j);
                indices.set(in++, (i+1)*(rows-1) + j+1);
                indices.set(in++, i*(rows-1) + j+1);
                indices.set(in++, i*(rows-1) + j);
                indices.set(in++, (i+1)*(rows-1) + j);
                indices.set(in++, (i+1)*(rows-1) + j+1);
                
                j++;
                if (j >= rows-1)
                {
                    j = 0;
                    i++;
                }
            }
            
            i = 0;
            j = 0;
            
            for (int in=numIndices/loops; in<numIndices;)
            {
                indices.set(in++, NUM_UNITSPHERE_POINTS + i*(rows-1) + j);
                indices.set(in++, NUM_UNITSPHERE_POINTS + (i+1)*(rows-1) + j+1);
                indices.set(in++, NUM_UNITSPHERE_POINTS + i*(rows-1) + j+1);
                indices.set(in++, NUM_UNITSPHERE_POINTS + i*(rows-1) + j);
                indices.set(in++, NUM_UNITSPHERE_POINTS + (i+1)*(rows-1) + j);
                indices.set(in++, NUM_UNITSPHERE_POINTS + (i+1)*(rows-1) + j+1);
                
                j++;
                if (j >= rows-1)
                {
                    j = 0;
                    i++;
                }
            }
            
            return true;
        }
        
	public boolean buildHull0(float margin)
        {
		Vector3f norm = Stack.alloc(Vector3f.class);

		int numSampleDirections = NUM_UNITSPHERE_POINTS;
		{
			int numPDA = shape.getNumPreferredPenetrationDirections();
			if (numPDA != 0) {
				for (int i=0; i<numPDA; i++) {
					shape.getPreferredPenetrationDirection(i, norm);
					unitSpherePoints.getQuick(numSampleDirections).set(norm);
					numSampleDirections++;
				}
			}
		}

		ObjectArrayList<Vector3f> supportPoints = new ObjectArrayList<Vector3f>();
		MiscUtil.resize(supportPoints, NUM_UNITSPHERE_POINTS + ConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2, Vector3f.class);

		for (int i=0; i<numSampleDirections; i++) {
			shape.localGetSupportingVertex(unitSpherePoints.getQuick(i), supportPoints.getQuick(i));
		}

		HullDesc hd = new HullDesc();
		hd.flags = HullFlags.TRIANGLES;
		hd.vcount = numSampleDirections;

		//#ifdef BT_USE_DOUBLE_PRECISION
		//hd.mVertices = &supportPoints[0];
		//hd.mVertexStride = sizeof(btVector3);
		//#else
		hd.vertices = supportPoints;
		//hd.vertexStride = 3 * 4;
		//#endif

		HullLibrary hl = new HullLibrary();
		HullResult hr = new HullResult();
		if (!hl.createConvexHull(hd, hr)) {
			return false;
		}

		MiscUtil.resize(vertices, hr.numOutputVertices, Vector3f.class);
		MiscUtil.resize(normals, hr.numOutputVertices, Vector3f.class);

		for (int i=0; i<hr.numOutputVertices; i++) {
			vertices.getQuick(i).set(hr.outputVertices.getQuick(i));
		}
		numIndices = hr.numIndices;
		MiscUtil.resize(indices, numIndices, 0);
		for (int i=0; i<numIndices; i++) {
			indices.set(i, hr.indices.get(i));
		}

		// free temporary hull result that we just copied
		hl.releaseResult(hr);

		return true;
	}

	public int numTriangles() {
		return numIndices / 3;
	}

	public int numVertices() {
		return vertices.size();
	}

	public int numIndices() {
		return numIndices;
	}

	public ObjectArrayList<Vector3f> getVertexPointer() {
		return vertices;
	}

	public ObjectArrayList<Vector3f> getNormalPointer() {
		return normals;
	}

	public IntArrayList getIndexPointer() {
		return indices;
	}

	////////////////////////////////////////////////////////////////////////////
	
	private static int NUM_UNITSPHERE_POINTS = 15*15; // 420;
	
	private static ObjectArrayList<Vector3f> constUnitSpherePoints = new ObjectArrayList<Vector3f>();
	
	static {
		constUnitSpherePoints.add(new Vector3f(0.000000f, -0.000000f, -1.000000f));
		constUnitSpherePoints.add(new Vector3f(0.723608f, -0.525725f, -0.447219f));
		constUnitSpherePoints.add(new Vector3f(-0.276388f, -0.850649f, -0.447219f));
		constUnitSpherePoints.add(new Vector3f(-0.894426f, -0.000000f, -0.447216f));
		constUnitSpherePoints.add(new Vector3f(-0.276388f, 0.850649f, -0.447220f));
		constUnitSpherePoints.add(new Vector3f(0.723608f, 0.525725f, -0.447219f));
		constUnitSpherePoints.add(new Vector3f(0.276388f, -0.850649f, 0.447220f));
		constUnitSpherePoints.add(new Vector3f(-0.723608f, -0.525725f, 0.447219f));
		constUnitSpherePoints.add(new Vector3f(-0.723608f, 0.525725f, 0.447219f));
		constUnitSpherePoints.add(new Vector3f(0.276388f, 0.850649f, 0.447219f));
		constUnitSpherePoints.add(new Vector3f(0.894426f, 0.000000f, 0.447216f));
		constUnitSpherePoints.add(new Vector3f(-0.000000f, 0.000000f, 1.000000f));
		constUnitSpherePoints.add(new Vector3f(0.425323f, -0.309011f, -0.850654f));
		constUnitSpherePoints.add(new Vector3f(-0.162456f, -0.499995f, -0.850654f));
		constUnitSpherePoints.add(new Vector3f(0.262869f, -0.809012f, -0.525738f));
		constUnitSpherePoints.add(new Vector3f(0.425323f, 0.309011f, -0.850654f));
		constUnitSpherePoints.add(new Vector3f(0.850648f, -0.000000f, -0.525736f));
		constUnitSpherePoints.add(new Vector3f(-0.525730f, -0.000000f, -0.850652f));
		constUnitSpherePoints.add(new Vector3f(-0.688190f, -0.499997f, -0.525736f));
		constUnitSpherePoints.add(new Vector3f(-0.162456f, 0.499995f, -0.850654f));
		constUnitSpherePoints.add(new Vector3f(-0.688190f, 0.499997f, -0.525736f));
		constUnitSpherePoints.add(new Vector3f(0.262869f, 0.809012f, -0.525738f));
		constUnitSpherePoints.add(new Vector3f(0.951058f, 0.309013f, 0.000000f));
		constUnitSpherePoints.add(new Vector3f(0.951058f, -0.309013f, 0.000000f));
		constUnitSpherePoints.add(new Vector3f(0.587786f, -0.809017f, 0.000000f));
		constUnitSpherePoints.add(new Vector3f(0.000000f, -1.000000f, 0.000000f));
		constUnitSpherePoints.add(new Vector3f(-0.587786f, -0.809017f, 0.000000f));
		constUnitSpherePoints.add(new Vector3f(-0.951058f, -0.309013f, -0.000000f));
		constUnitSpherePoints.add(new Vector3f(-0.951058f, 0.309013f, -0.000000f));
		constUnitSpherePoints.add(new Vector3f(-0.587786f, 0.809017f, -0.000000f));
		constUnitSpherePoints.add(new Vector3f(-0.000000f, 1.000000f, -0.000000f));
		constUnitSpherePoints.add(new Vector3f(0.587786f, 0.809017f, -0.000000f));
		constUnitSpherePoints.add(new Vector3f(0.688190f, -0.499997f, 0.525736f));
		constUnitSpherePoints.add(new Vector3f(-0.262869f, -0.809012f, 0.525738f));
		constUnitSpherePoints.add(new Vector3f(-0.850648f, 0.000000f, 0.525736f));
		constUnitSpherePoints.add(new Vector3f(-0.262869f, 0.809012f, 0.525738f));
		constUnitSpherePoints.add(new Vector3f(0.688190f, 0.499997f, 0.525736f));
		constUnitSpherePoints.add(new Vector3f(0.525730f, 0.000000f, 0.850652f));
		constUnitSpherePoints.add(new Vector3f(0.162456f, -0.499995f, 0.850654f));
		constUnitSpherePoints.add(new Vector3f(-0.425323f, -0.309011f, 0.850654f));
		constUnitSpherePoints.add(new Vector3f(-0.425323f, 0.309011f, 0.850654f));
		constUnitSpherePoints.add(new Vector3f(0.162456f, 0.499995f, 0.850654f));
	}
	
	static int tablelength = 257;
	
	static double power1 = -1;
	static double[] powtable1 = new double[tablelength];
	static double power2 = -1;
	static double[] powtable2 = new double[tablelength];
	
class Superellipsoid //extends Biparam /*Sphere*/ implements java.io.Serializable
{

    Superellipsoid()
    {
//        inPnt = new cVector();
        du = new Vector3f();
        dv = new Vector3f();
//		
//        name = "Superellipsoid";
        east = 0.75;
        north = 1;
        //uDivs = vDivs = 32;
//        uDivs = 35;
//        vDivs = 34;
//        minUDivs = 3;
//		minVDivs = 2;
    }

    double signPower(double base, double pow)
    {
		//if (base == 0)
		//	base = 0.0000001;
		
		if (base == 0 && pow == 0 || base == 1)
			return 1;
		
		if (base == 0)
			return 0;
		
		if (base == -1)
			return -1;
		
        if (base >= 0)
            return (double)Mathpow(base, pow);
        else
            return -(double)Mathpow(-base, pow);
    }

	double Mathpow(double base, double pow)
	{
		int basei = (int) (base * (tablelength - 1));
		
		double[] table = null;
		
		if (pow == power1)
		{
			table = powtable1;
		}
		else if (pow == power2)
		{
			table = powtable2;
		}
		else
		{
			if (power2 == -1)
			{
				power2 = pow;
				table = powtable2 = new double[tablelength];
			}
			else
			{
				power1 = pow;
				table = powtable1 = new double[tablelength];
				power2 = -1;
			}
		}
		
		if (basei != 0 && table[basei] == 0)
		{
			table[basei] = Math.pow(basei/(tablelength - 1.0), pow);
			//if (table[basei] == 0)
			//	System.out.println("POW == 0 : " + basei);
		}
		
		if (table[basei+1] == 0)
		{
			table[basei+1] = Math.pow((basei+1.0)/(tablelength - 1.0), pow);
			//if (table[basei+1] == 0)
			//	System.out.println("POW == 0");
		}
		
		double offset = base * (tablelength - 1) - basei;
		//System.out.println("offset = " + offset);
		
		return table[basei] * (1 - offset) + table[basei+1] * offset;
	}
	
    double DsignPower(double base, double pow)
    {
		if (base == 0)
			base = 0.0000000001;
		
		if (base == 1 || base == -1)
			return pow;
			
        if (base > 0)
            return pow * (double)(Mathpow(base, pow) / base); // - 1);
        else // if (base < 0)
            return pow * (double)(Mathpow(-base, pow) / -base); // - 1);
		//else
		//{
		//	if (pow == 1)
		//		return 1;
		//	else
		//		return 0;
		//}
    }

	Vector3f du;
	Vector3f dv;
	
    //Vertex
    void
            biparamFunction(double u, double v, Vector3f out, Vector3f norm)
    {
		//System.out.println("U = " + u + "; V = " + v);
        double uAng = u * 2 * Math.PI; // LA.toRadians(u * 360);
        double vAng = v * Math.PI - Math.PI/2; // LA.toRadians(v * 180 - 90);
		double mcu = Math.cos(uAng);
		double msu = Math.sin(uAng);
		double mcv = Math.cos(vAng);
		double msv = Math.sin(vAng);
		if (u == 1)
		{
			mcu = 1;
			msu = 0;
		}
		if (v == 1)
		{
			mcv = 0;
			msv = 1;
		}
		if (v == 0)
		{
			mcv = 0;
			msv = -1;
		}
        double cu = signPower(mcu, north);
        double su = signPower(msu, north);
        double cv = signPower(mcv, east);
        double sv = signPower(msv, east);
        double z = radius * cv * cu;
        double x = radius * cv * su;
        double y = radius * sv;
		
        double dcu = DsignPower(mcu, north) * -msu;
        double dsu = DsignPower(msu, north) * mcu;
        double dcv = DsignPower(mcv, east) * -msv;
        double dsv = DsignPower(msv, east) * mcv;
		
		du.z = (float)(cv * dcu);
		du.x = (float)(cv * dsu);
		du.y = (float)(0);
		dv.z = (float)(dcv * cu);
		dv.x = (float)(dcv * su);
		dv.y = (float)(dsv);
		
		//if (du.length() == 0)
		//	System.out.println("DU is null");
		//if (dv.length() == 0)
		//	System.out.println("DV is null");
                out./*pos.*/x = (float)x; // = LA.newVector(x, y, z);
		out./*pos.*/y = (float)y;
		out./*pos.*/z = (float)z;
		
                if (du.dot(du) == 0)
                {
                    norm.set(out);
                }
                else
                {
                    du.normalize();
                    dv.normalize();

                    norm.cross(du, dv);
//                    if (norm.dot(norm) == 0)
//                        norm.z = 1;
                }
                
                norm.normalize();
//        Vertex temp = new Vertex();
//		
//        temp.norm = LA.newVector(x, y, z);
//		LA.vecCross(du,dv, temp.norm);
//		if (temp.norm.length() == 0)
//			temp.norm.y = 2*v - 1;
//		else
//			LA.vecNormalize(temp.norm);

		//if (temp.norm.dot(temp/*.pos*/) < 0)
			//temp.norm.mul(-1);
		
        //LA.vecAdd(temp/*.pos*/, center, temp/*.pos*/);

       // return temp;
    }


    double east;
    double north;
	double radius = 1;
//    private cVector inPnt;
}
}