#include <btBulletDynamicsCommon.h>
#include "BulletCollision/CollisionDispatch/btGhostObject.h"
#include <iostream>

enum CollisionTypes {
    COL_COLLECTIBLE = 64, //<Collide with powerups
    COL_STATIC_ENVIRONMENT = 128,
    COL_PLAYERS = 256
};

namespace CollisionGroups{
    static const int environmentCollidesWith = COL_PLAYERS;
    static const int playerCollidesWith = COL_COLLECTIBLE | COL_STATIC_ENVIRONMENT;
    static const int collectibleCollidesWith = COL_PLAYERS;
}

//Physics
btAxisSweep3*                       _phyBroadphase;
btDefaultCollisionConfiguration*    _phyCollisionConfig;
btCollisionDispatcher*              _phyDispatcher;
btSequentialImpulseConstraintSolver* _phySolver;
btDynamicsWorld*                    _phyWorld;
btPairCachingGhostObject*           _phyAttackGhostObject;


void collisionEvent(const btCollisionObject* pBody0, const btCollisionObject * pBody1){
    btCollisionObject* ghostObj = static_cast<btCollisionObject*>(_phyAttackGhostObject);
    btTransform t = _phyAttackGhostObject->getWorldTransform();
    btVector3 v1,v2;
    _phyAttackGhostObject->getCollisionShape()->getAabb(t,v1,v2);
    std::cout << v1.x() << " " << v1.y() << " " << v1.z() << std::endl;
    std::cout << v2.x() << " " << v2.y() << " " << v2.z() << std::endl;
    static int count = 0;
    if (ghostObj == pBody0){
        ++count;
        if (pBody1->getCollisionFlags() != COL_PLAYERS){
            std::cout << count << " Hit non-character" << std::endl << std::endl;
        }
    } else {
        ++count;
        if (pBody0->getCollisionFlags() != COL_PLAYERS){
            std::cout << count << " Hit non-character" << std::endl << std::endl;
        }
    }
}

void checkCollisionEvents(){
    // ATTACK
    btManifoldArray   manifoldArray;
    btBroadphasePairArray& pairArray = _phyAttackGhostObject->getOverlappingPairCache()->getOverlappingPairArray();
    int numPairs = pairArray.size();
    for (int i=0;i<numPairs;i++){
        manifoldArray.clear();

        const btBroadphasePair& pair = pairArray[i];
        //unless we manually perform collision detection on this pair, the contacts are in the dynamics world paircache:
        btBroadphasePair* collisionPair = _phyWorld->getPairCache()->findPair(pair.m_pProxy0,pair.m_pProxy1);
        if (!collisionPair)
            continue;

        if (collisionPair->m_algorithm)
            collisionPair->m_algorithm->getAllContactManifolds(manifoldArray);

        for (int j=0;j<manifoldArray.size();j++){
            btPersistentManifold* manifold = manifoldArray[j];

            // TODO: ADD STUFF
            collisionEvent( manifold->getBody0(), manifold->getBody1() );

            btScalar directionSign = manifold->getBody0() == _phyAttackGhostObject ? btScalar(-1.0) : btScalar(1.0);
            for (int p=0;p<manifold->getNumContacts();p++){
                const btManifoldPoint&pt = manifold->getContactPoint(p);

                if (pt.getDistance()<0.f){
                    const btVector3& ptA = pt.getPositionWorldOnA();
                    const btVector3& ptB = pt.getPositionWorldOnB();
                    const btVector3& normalOnB = pt.m_normalWorldOnB;
                    /// work here

                }
            }
        }
    }
}

void main(){
    // Initialize physics
    _phyBroadphase = new btAxisSweep3(btVector3(-10000,-10000,-10000), btVector3(10000,10000,10000), 1024);
    _phyCollisionConfig = new btDefaultCollisionConfiguration();
    _phyDispatcher = new btCollisionDispatcher(_phyCollisionConfig);
    _phySolver = new btSequentialImpulseConstraintSolver();
    _phyWorld = new btDiscreteDynamicsWorld(_phyDispatcher, _phyBroadphase, _phySolver, _phyCollisionConfig);
    _phyWorld->setGravity(btVector3(0,-9.8,0));

    // Static object
    btRigidBody* _phyBody = new btRigidBody(0, new btDefaultMotionState(), new btSphereShape(0.5) );
    _phyBody->setCollisionFlags(COL_STATIC_ENVIRONMENT);
    _phyWorld->addRigidBody(_phyBody, COL_STATIC_ENVIRONMENT, CollisionGroups::environmentCollidesWith);

    // Ghost object
    _phyAttackGhostObject = new btPairCachingGhostObject();
    _phyAttackGhostObject->setCollisionShape(  new btSphereShape( 0.5) );
    //_phyAttackGhostObject->setCollisionShape(  new btBoxShape( btVector3(0.5, 0.5, 0.5)) );
    _phyAttackGhostObject->setCollisionFlags(btCollisionObject::CF_NO_CONTACT_RESPONSE | COL_PLAYERS);
    _phyWorld->getBroadphase()->getOverlappingPairCache()->setInternalGhostPairCallback(new btGhostPairCallback());


    btTransform hitboxTrans;
    hitboxTrans.setIdentity();
    // #######################################################
    //hitboxTrans.setOrigin(btVector3(1.3, 0, 0)); // no hit
    hitboxTrans.setOrigin(btVector3(1.27, 0, 0)); // hit
    // #######################################################
    _phyAttackGhostObject->setWorldTransform(hitboxTrans);
    _phyWorld->addCollisionObject(_phyAttackGhostObject, COL_PLAYERS, CollisionGroups::playerCollidesWith);

    for (int i = 0; i < 300; i++) {
        _phyWorld->stepSimulation(1 / 60.f, 10);
        checkCollisionEvents();
    }
    std::cin.get();
}