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

import com.bulletphysics.BulletGlobals;
import com.bulletphysics.BulletStats;
import com.bulletphysics.collision.dispatch.CollisionObject;

import com.bulletphysics.util.ObjectArrayList;
import com.bulletphysics.collision.broadphase.BroadphaseInterface;
import com.bulletphysics.collision.broadphase.BroadphaseNativeType;
import com.bulletphysics.collision.broadphase.DbvtBroadphase;
import com.bulletphysics.collision.dispatch.CollisionDispatcher;
import com.bulletphysics.collision.dispatch.DefaultCollisionConfiguration;
import com.bulletphysics.collision.shapes.BoxShape;
import com.bulletphysics.collision.shapes.SphereShape;
import com.bulletphysics.collision.shapes.CollisionShape;
import com.bulletphysics.collision.shapes.CompoundShape;
import com.bulletphysics.collision.shapes.StaticPlaneShape;
import com.bulletphysics.collision.shapes.CylinderShape;
import com.bulletphysics.collision.shapes.ConvexShape;
import com.bulletphysics.collision.shapes.ShapeHull;
import com.bulletphysics.collision.shapes.PolyhedralConvexShape;
import com.bulletphysics.collision.shapes.ConcaveShape;
import com.bulletphysics.collision.shapes.TriangleCallback;
import com.bulletphysics.collision.shapes.InternalTriangleIndexCallback;
//import com.bulletphysics.demos.opengl.DemoApplication;
//import com.bulletphysics.demos.opengl.GLDebugDrawer;
//import com.bulletphysics.demos.opengl.IGL;
//import com.bulletphysics.demos.opengl.LWJGL;
import javax.media.opengl.GL;
import javax.media.opengl.glu.GLU;
import com.bulletphysics.dynamics.RigidBody;
import com.bulletphysics.dynamics.RigidBodyConstructionInfo;
import com.bulletphysics.dynamics.DynamicsWorld;
import com.bulletphysics.dynamics.DiscreteDynamicsWorld;
import com.bulletphysics.dynamics.constraintsolver.ConstraintSolver;
import com.bulletphysics.dynamics.constraintsolver.SequentialImpulseConstraintSolver;
import com.bulletphysics.linearmath.DefaultMotionState;
import com.bulletphysics.linearmath.DebugDrawModes;
import com.bulletphysics.linearmath.Transform;
import com.bulletphysics.linearmath.TransformUtil;
import com.bulletphysics.linearmath.VectorUtil;
import com.bulletphysics.linearmath.Clock;
import com.bulletphysics.util.ObjectPool;
import com.bulletphysics.util.IntArrayList;
import javax.vecmath.Vector3f;
import javax.vecmath.Color3f;
//import org.lwjgl.LWJGLException;
//import static com.bulletphysics.demos.opengl.IGL.*;

import com.bulletphysics.linearmath.MiscUtil;
//import com.bulletphysics.dynamics.constraintsolver.TypedConstraint;
import com.bulletphysics.dynamics.constraintsolver.Generic6DofConstraint;

import java.util.Map;
import java.util.HashMap;

/**
 *
 * @author nbriere
 */
public class PhysicsNode extends Composite // Object3D
{
    static final long serialVersionUID = 7284096213977007708L;
    
transient
        DynamicsWorld transientDynamicsWorld = null;
    
        float gravity;
        float floor;
        float wind;
        float friction; // global plane
        
    PhysicsNode()
    {
        super("Physics");
        
        live = true;
    }
    
    void Reset()
    {
        initPhysics();
    }
    
    DynamicsWorld GetDynamics()
    {
        if (transientDynamicsWorld == null)
            initPhysics();
        
        return transientDynamicsWorld;
    }
    
    static protected Clock clock = new Clock();

    static public float getDeltaTimeMicroseconds()
    {
        //#ifdef USE_BT_CLOCK
        float dt = clock.getTimeMicroseconds();
        clock.reset();
        return dt;
    //#else
    //return btScalar(16666.);
    //#endif
    }
    
    float totalms = 0;
    
    void drawSelf(iCameraPane display, Object3D /*Composite*/ root, boolean selected, boolean blocked)
    {
        if (Globals.isLIVE() && live && display.DrawMode() == display.SHADOW) // FUCK
        {
            float ms = getDeltaTimeMicroseconds();
            float minFPS = 1000000f / 60f;
            if (ms > minFPS)
            {
                ms = minFPS;
            }

            if (CameraPane.AntialiasingEnabled())
                ms /= CameraPane.ACSIZE;
        
            totalms += ms;

            if (totalms > 2000000)
            {
            //    spawnRagdoll(true);
                totalms = 0;
            }

//            assert (dynamicsWorld != null);
//            
//            if (transientDynamicsWorld == null)
////                staticdynamicsWorld = dynamicsWorld;
//                initPhysics();
//            
//            assert(dynamicsWorld == staticdynamicsWorld);
            
            GetDynamics().stepSimulation(ms / 1000000.f);
        }
        
        super.drawSelf(display, root, selected, blocked);
    }
    
    void Step()
    {
        float ms = getDeltaTimeMicroseconds();
        
        float minFPS = 1000000f / 60f;
        if (ms > minFPS)
        {
            ms = minFPS;
        }

//        if (transientDynamicsWorld == null)
//            initPhysics();
            
        GetDynamics().stepSimulation(ms / 1000000.f);
    }
    
    public void initPhysics()
    {
        //if (dynamicsWorld != null) // && staticdynamicsWorld == null)
        //    staticdynamicsWorld = dynamicsWorld;
        
        // Setup the basic world
        //if (staticdynamicsWorld == null)
        //if (transientDynamicsWorld == null) // && ragdolls.size() == 0)
        {
            DefaultCollisionConfiguration collision_config = new DefaultCollisionConfiguration();

            CollisionDispatcher dispatcher = new CollisionDispatcher(collision_config);

            Vector3f worldAabbMin = new Vector3f(-10000, -10000, -10000);
            Vector3f worldAabbMax = new Vector3f(10000, 10000, 10000);
            //BroadphaseInterface overlappingPairCache = new AxisSweep3(worldAabbMin, worldAabbMax);
            //BroadphaseInterface overlappingPairCache = new SimpleBroadphase();
            BroadphaseInterface overlappingPairCache = new DbvtBroadphase();

            //#ifdef USE_ODE_QUICKSTEP
            //btConstraintSolver* constraintSolver = new OdeConstraintSolver();
            //#else
            ConstraintSolver constraintSolver = new SequentialImpulseConstraintSolver();
            //#endif

            if (transientDynamicsWorld != null)
                ResetDynamics(transientDynamicsWorld);

            transientDynamicsWorld = new DiscreteDynamicsWorld(dispatcher, overlappingPairCache, constraintSolver, collision_config);

            //staticdynamicsWorld = dynamicsWorld;
//		dynamicsWorld.setDebugDrawer(new GLDebugDrawer(gl));

        // Setup a big ground box
//        {
            CollisionShape groundShape = new BoxShape(new Vector3f(200f, 10f, 200f));
            Transform groundTransform = new Transform();
            groundTransform.setIdentity();
            groundTransform.origin.set(0f, -floor - 10, 0f);
            localCreateRigidBody(0f, groundTransform, groundShape);
            
            groundShape = new BoxShape(new Vector3f(200f, 200f, 200f));
            groundTransform = new Transform();
            groundTransform.setIdentity();
            groundTransform.origin.set(0f, 100, -201f);
            //localCreateRigidBody(0f, groundTransform, groundShape);
            
            groundShape = new BoxShape(new Vector3f(200f, 200f, 200f));
            groundTransform = new Transform();
            groundTransform.setIdentity();
            groundTransform.origin.set(-201f, 100, 0f);
            //localCreateRigidBody(0f, groundTransform, groundShape);
            
            groundShape = new BoxShape(new Vector3f(200f, 200f, 200f));
            groundTransform = new Transform();
            groundTransform.setIdentity();
            groundTransform.origin.set(201f, 100, 0f);
            //localCreateRigidBody(0f, groundTransform, groundShape);
            
            groundShape = new BoxShape(new Vector3f(200f, 200f, 200f));
            groundTransform = new Transform();
            groundTransform.setIdentity();
            groundTransform.origin.set(0f, 100, 201f);
            //localCreateRigidBody(0f, groundTransform, groundShape);
            
            transientDynamicsWorld.setGravity(new Vector3f(0, -gravity, wind));
            
            SetDynamics(transientDynamicsWorld);
        }
//        else
        {
//            if (transientDynamicsWorld == null)
//            {
//                staticdynamicsWorld = ragdolls.get(0).ownerWorld;
//                clientResetScene();
//            }
//            else
//            {
//                if (ragdolls.size() > 0)
//                    assert(staticdynamicsWorld == ragdolls.get(0).ownerWorld);
//            }
        }

        //dynamicsWorld = staticdynamicsWorld;
            
        // Spawn one ragdoll
//      spawnRagdoll();
//
//        myoffset = dynamicsWorld.getNumCollisionObjects() - 1;
//        
//        if (myoffset == 0)
            clientResetScene();
    }

    public RigidBody localCreateRigidBody(float mass, Transform startTransform, CollisionShape shape)
    {
        // rigidbody is dynamic if and only if mass is non zero, otherwise static
        boolean isDynamic = (mass != 0f);

        Vector3f localInertia = new Vector3f(0f, 0f, 0f);
        if (isDynamic)
        {
            shape.calculateLocalInertia(mass, localInertia);
        }

        // using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects

        //#define USE_MOTIONSTATE 1
        //#ifdef USE_MOTIONSTATE
        DefaultMotionState myMotionState = new DefaultMotionState(startTransform);

        float friction = 1;
        
        RigidBodyConstructionInfo cInfo = new RigidBodyConstructionInfo(mass, myMotionState, shape, localInertia, friction);

        RigidBody body = new RigidBody(cInfo);
        //#else
        //btRigidBody* body = new btRigidBody(mass,0,shape,localInertia);	
        //body->setWorldTransform(startTransform);
        //#endif//

        transientDynamicsWorld.addRigidBody(body);

        return body;
    }

    public void clientResetScene()
    {
//        System.exit(0);
        //#ifdef SHOW_NUM_DEEP_PENETRATIONS
        BulletStats.gNumDeepPenetrationChecks = 0;
        BulletStats.gNumGjkChecks = 0;
        //#endif //SHOW_NUM_DEEP_PENETRATIONS

        int numObjects = 0;
        if (transientDynamicsWorld != null)
        {
            transientDynamicsWorld.stepSimulation(1f / 60f, 0);
            numObjects = transientDynamicsWorld.getNumCollisionObjects();
        }

        for (int i = 0; i < numObjects; i++)
        {
//            if (i > 0)
//            System.exit(0);
            CollisionObject colObj = transientDynamicsWorld.getCollisionObjectArray().getQuick(i);
            RigidBody body = RigidBody.upcast(colObj);
            if (body != null)
            {
                DefaultMotionState myMotionState = (DefaultMotionState) body.getMotionState();
                if (//false)
                        myMotionState != null)
                {
                    myMotionState.graphicsWorldTrans.set(myMotionState.startWorldTrans);
                    colObj.setWorldTransform(myMotionState.graphicsWorldTrans);
                    colObj.setInterpolationWorldTransform(myMotionState.startWorldTrans);
                    colObj.activate();
                }
                // removed cached contact points
                transientDynamicsWorld.getBroadphase().getOverlappingPairCache().cleanProxyFromPairs(colObj.getBroadphaseHandle(), transientDynamicsWorld.getDispatcher());

                // ??? body = RigidBody.upcast(colObj);
                if (body != null && !body.isStaticObject())
                {
                    RigidBody.upcast(colObj).setLinearVelocity(new Vector3f(0f, 0f, 0f));
                    RigidBody.upcast(colObj).setAngularVelocity(new Vector3f(0f, 0f, 0f));
                }
                
//                if (body.getUserPointer() != null)
//                    ((RagDoll)body.getUserPointer()).init(transientDynamicsWorld, new Vector3f(0f, 0f, 0f), 0.45f, true);
                
            }

        /*
        //quickly search some issue at a certain simulation frame, pressing space to reset
        int fixed=18;
        for (int i=0;i<fixed;i++)
        {
        getDynamicsWorld()->stepSimulation(1./60.f,1);
        }
         */
        }
    }

    void createEditWindow(GroupEditor callee, boolean newWindow)
    {
        //editWindow = (new SphereEditor(this, deepCopy(), callee)).GetEditor();
        if (newWindow)
        {
            objectUI = new PhysicsEditor(this, deepCopy(), callee);
        } else
        {
            objectUI = new PhysicsEditor(this, callee);
        }
        editWindow = objectUI.GetEditor();
    }

}