package com.kangacult.octogone;

import com.badlogic.gdx.math.Matrix4;
import com.badlogic.gdx.math.Vector3;
import com.badlogic.gdx.physics.bullet.collision.ClosestNotMeConvexResultCallback;
import com.badlogic.gdx.physics.bullet.collision.btBroadphaseProxy;
import com.badlogic.gdx.physics.bullet.collision.btCollisionObject;
import com.badlogic.gdx.physics.bullet.collision.btConvexShape;
import com.badlogic.gdx.physics.bullet.dynamics.btDynamicsWorld;
import com.badlogic.gdx.physics.bullet.dynamics.btRigidBody;

public class Movement {
    /**Convenience method for sweeping through world.
     * IMPORTANT: Remember to dispose of the returned callback */
    private static Matrix4 trace_transform = new Matrix4();
    private static Matrix4 trace_comparison = new Matrix4();
    public static ClosestNotMeConvexResultCallback sweep(btDynamicsWorld world,
            btConvexShape shape, btCollisionObject subject, Vector3 start, Vector3 end) {
        trace_transform.setToTranslation(start);
        trace_comparison.setToTranslation(end);
        ClosestNotMeConvexResultCallback callback = 
                new ClosestNotMeConvexResultCallback(subject, start, end);
        callback.setCollisionFilterMask(
                (short) (btBroadphaseProxy.CollisionFilterGroups.AllFilter ^
                        btBroadphaseProxy.CollisionFilterGroups.DebrisFilter));
        world.convexSweepTest(
                shape, trace_transform, trace_comparison, callback, 0.04f);
        return callback;
    }

    private static Vector3 dest = new Vector3();
    private static Vector3 normal = new Vector3();
    private static Vector3 point = new Vector3();
    public static void move(float deltaTime, btDynamicsWorld world, btRigidBody body, 
            btConvexShape shape, Vector3 position, Vector3 velocity,
            Vector3 direction, float speed, MotionState motion) {
        float currentSpeed = velocity.len();
        
        //Dampen Velocity
        if (currentSpeed < 0.001) {
            velocity.setZero();
            currentSpeed = 0f;
        } else {
            float scale = (currentSpeed - 10f * deltaTime) / currentSpeed;
            if (scale < 0) scale = 0;
            velocity.scl(scale);
            currentSpeed *= scale;
        }
        
        //Ground Check
        if (velocity.z < 1.0f) {
            dest.set(position);
            dest.add(0f, 0f, -0.3f);
            ClosestNotMeConvexResultCallback callback = sweep(world, shape, body, position, dest);
            if (! callback.hasHit()) {
                velocity.mulAdd(world.getGravity(), deltaTime);
            }
            callback.dispose();
        }
        
        //Accelerate
        float acc = 10.0f * speed * deltaTime;
        if (acc > speed - currentSpeed) {
            acc = speed - currentSpeed;
            if (acc < 0) acc = 0;
        }
        velocity.mulAdd(direction, acc);
        
        float moveRemaining = 1.0f;
        for (int attempts = 0; moveRemaining > 0.001f && attempts <= 2; attempts++) {
            point.set(position);
            dest.set(point);
            dest.mulAdd(velocity, deltaTime * moveRemaining);
            ClosestNotMeConvexResultCallback moveTrace = sweep(world, shape, body, point, dest);
            float fraction = moveTrace.getClosestHitFraction();
            moveTrace.getHitNormalWorld(normal);
            position.mulAdd(velocity, deltaTime * fraction * moveRemaining);

            if (fraction >= 1.0f) {
                moveRemaining = 0f;
            } else {
                moveRemaining *= fraction;
            }

            //Bounce away from collision
            if (fraction < 1.0f) {
                position.mulAdd(normal, 0.01f);

                float dot = velocity.dot(normal);
                velocity.mulAdd(normal, -dot);
                if (dot > 0f) {
                    velocity.mulAdd(normal, -0.01f);
                } else {
                    velocity.mulAdd(normal, 0.01f);
                }
            }
        }
        
        motion.transform.setToTranslation(position);
    }
}
