public class MyKinematicCharacter extends ActionInterface {

    private PairCachingGhostObject ghostObject;
    private float radius;
    private float[] glMatrix;
    private float deltaTime,speed;
    private Vector3f gravity;
    private Vector3f moveDirection;
    private boolean onGround;
    private Timer timer;
    private float rayLength=2f;
    private CollisionWorld world;
    private Transform transform;
    private Vector3f currentGravitySpeed;
    private boolean jump;

    public MyKinematicCharacter(SphereShape shape)
    {
        timer=new Timer();
        gravity=new Vector3f(0,-9.81f/4f,0);
        moveDirection=new Vector3f();
        ghostObject=new PairCachingGhostObject();
        ghostObject.setCollisionShape(shape);
        ghostObject.setWorldTransform(new Transform(new Matrix4f(new Quat4f(0, 0, 0, 1), new Vector3f(0, 0, 0), 1.0f)));
        radius = shape.getRadius();
        glMatrix=new float[16];
        transform=new Transform();
        currentGravitySpeed=new Vector3f();
        ghostObject.setCollisionFlags(CollisionFlags.CHARACTER_OBJECT);
    }

    public PairCachingGhostObject GetGhostObject()
    {
        return ghostObject;
    }

    public void SetPosition(Vector3f position)
    {
        ghostObject.setWorldTransform(new Transform(new Matrix4f(new Quat4f(0,0,0,1), position, 1.0f)));
    }

    public void SetRayLength(float rayLength)
    {
        this.rayLength=rayLength;
    }

    public Vector3f GetCurrentPosition()
    {
        ghostObject.getWorldTransform(transform);
        return transform.origin;
    }

    public void SetGravity(Vector3f gravity)
    {
        this.gravity=gravity;
    }

    public void Stop()
    {
        System.out.println("Stop!");
        if(!jump)moveDirection=new Vector3f();
    }

    public void MoveOnce(Vector3f direction, float speed, float forTime) {
        if(onGround) {
            this.speed = speed;
            moveDirection = direction;
            timer.schedule(new TimerTask() {
                @Override
                public void run() {
                    Stop();
                }
            }, (long) forTime);
        }
    }

    public void MoveAlways(Vector3f direction, float speed)
    {
        if(onGround) {
            this.speed = speed;
            moveDirection = direction;
        }
    }

    public void Draw(GL2 gl2, GLUT glut)
    {
        gl2.glPushMatrix();
        ghostObject.getWorldTransform(new Transform()).getOpenGLMatrix(glMatrix);
        gl2.glMultMatrixf(FloatBuffer.wrap(glMatrix));
        gl2.glDisable(GL2.GL_LIGHTING);
        gl2.glColor3f(0.7f,0,0);
        glut.glutSolidSphere(radius,10,10);
        gl2.glEnable(GL2.GL_LIGHTING);
        gl2.glPopMatrix();
    }


    private void Gravity()
    {
        if(!onGround)
        {
            currentGravitySpeed=MyUtil.AddVector(MyUtil.MulVector(gravity, deltaTime), currentGravitySpeed);
            SetPosition(MyUtil.AddVector(GetCurrentPosition(), currentGravitySpeed));
        }
        else
        {
            currentGravitySpeed=new Vector3f();
        }
    }

    private void CheckGround()
    {
        Vector3f toWorld=new Vector3f(GetCurrentPosition());
        Vector3f fromWorld = new Vector3f(GetCurrentPosition());
        fromWorld.y-=radius;
        toWorld.y-=(rayLength+radius);
        CollisionWorld.RayResultCallback rayInfo = new CollisionWorld.ClosestRayResultCallback(fromWorld,toWorld);

        rayInfo.collisionFilterGroup=2;
        world.rayTest(fromWorld, toWorld, rayInfo);
        if(rayInfo.hasHit())
        {
            onGround=true;
            currentGravitySpeed=new Vector3f();
        }
        else
        {
            onGround=false;
        }
    }

    public void Jump(float speed)
    {
        MoveAlways(new Vector3f(0,1,0),speed);
        jump=true;
        onGround=false;
    }

    private void CalculatePos(Vector3f moveDirection)
    {
        Vector3f newPosition = MyUtil.MulVector(moveDirection, speed*deltaTime);
        newPosition=MyUtil.AddVector(newPosition, GetCurrentPosition());
        SetPosition(newPosition);
    }

    private void PreStep()
    {
        if(GetCurrentPosition().y<0)
        {
            SetPosition(new Vector3f(-10,5,10));
        }

        CheckGround();
        if(!onGround && !jump)
        {
            Gravity();
        }
        else
        {
            if(moveDirection.length()>0)
            {
                Vector3f fromWorld = new Vector3f(GetCurrentPosition());
                Vector3f toWorld = new Vector3f(GetCurrentPosition());
                fromWorld.y-=radius;
                toWorld.y=fromWorld.y;
                Vector3f temp=MyUtil.MulVector(moveDirection,rayLength);
                toWorld=MyUtil.AddVector(toWorld,temp);
                CollisionWorld.ClosestRayResultCallback callback = new CollisionWorld.ClosestRayResultCallback(fromWorld,toWorld);
                callback.collisionFilterGroup=2;
                world.rayTest(fromWorld,toWorld,callback);

                if(callback.hasHit())
                {
                    double angle=MyUtil.AngleToY(callback);
                    System.out.println(angle+ " " + callback.collisionObject.getCollisionShape().getName());
                    if(angle>=60.0)
                    {
                        Stop();
                    }
                    else
                    {
                        CalculatePos(MyUtil.InvertXZ(callback.hitNormalWorld));
                    }
                }
                else
                {
                    CalculatePos(moveDirection);
                }

                if(jump)
                {
                    moveDirection = MyUtil.SubVector(moveDirection,new Vector3f(0,0.1f,0));
                    if(moveDirection.y<=0)
                    {
                        moveDirection=new Vector3f();
                        jump=false;
                    }
                }
            }
        }
    }

    @Override
    public void updateAction(CollisionWorld collisionWorld, float deltaTime) {
        this.deltaTime=deltaTime;
        world=collisionWorld;

        PreStep();
    }

    @Override
    public void debugDraw(IDebugDraw iDebugDraw) {

    }
}