#include "PhysicsLogicSystem.h"
#include <PxPhysicsAPI.h>
#include "extensions/PxExtensionsAPI.h"
#include <foundation\PxFoundation.h>
#include <iostream>
#include "OgreLogManager.h"
#include "vehicle/PxVehicleUtil.h"
using namespace Demo;

bool                    gIsVehicleInAir = true;

PxF32 gSteerVsForwardSpeedData[2 * 8] =
{
    0.0f, 0.75f,
    5.0f, 0.75f,
    30.0f, 0.125f,
    120.0f, 0.1f,
    PX_MAX_F32, PX_MAX_F32,
    PX_MAX_F32, PX_MAX_F32,
    PX_MAX_F32, PX_MAX_F32,
    PX_MAX_F32, PX_MAX_F32
};
PxFixedSizeLookupTable<8> gSteerVsForwardSpeedTable(gSteerVsForwardSpeedData, 4);

PxVehicleKeySmoothingData gKeySmoothingData =
{
    {
        6.0f,   //rise rate eANALOG_INPUT_ACCEL
        6.0f,   //rise rate eANALOG_INPUT_BRAKE
        6.0f,   //rise rate eANALOG_INPUT_HANDBRAKE
        2.5f,   //rise rate eANALOG_INPUT_STEER_LEFT
        2.5f,   //rise rate eANALOG_INPUT_STEER_RIGHT
    },
    {
        10.0f,  //fall rate eANALOG_INPUT_ACCEL
        10.0f,  //fall rate eANALOG_INPUT_BRAKE
        10.0f,  //fall rate eANALOG_INPUT_HANDBRAKE
        5.0f,   //fall rate eANALOG_INPUT_STEER_LEFT
        5.0f    //fall rate eANALOG_INPUT_STEER_RIGHT
    }
};
bool                    gVehicleOrderComplete = false;
bool                    gMimicKeyInputs = false;
PxBatchQuery*           gBatchQuery = NULL;

PhysicsLogicSystem::PhysicsLogicSystem(GameState* state)
: LogicSystem(state),
mPxPhysics(0),
mPxScene(0),
mVehicle(0)
{
    mPxAllocator = new physx::PxDefaultAllocator;
    mPxErrorCallback = new physx::PxDefaultErrorCallback;
}

PhysicsLogicSystem::~PhysicsLogicSystem()
{
    delete mPxErrorCallback;
    delete mPxAllocator;
}

void PhysicsLogicSystem::initalizePhysicsWorld()
{
    assert(!mPxScene && "Trying to initialize a physics world without destroying the existing one.");
    mPxFoundation = PxCreateFoundation(PX_PHYSICS_VERSION, *mPxAllocator, *mPxErrorCallback);
    PxProfileZoneManager* profileZoneManager = &PxProfileZoneManager::createProfileZoneManager(mPxFoundation);
    mPxPhysics = PxCreatePhysics(PX_PHYSICS_VERSION, *mPxFoundation, PxTolerancesScale(), true);

    if (mPxPhysics->getPvdConnectionManager())
    {
        mPxPhysics->getVisualDebugger()->setVisualizeConstraints(true);
        mPxPhysics->getVisualDebugger()->setVisualDebuggerFlag(PxVisualDebuggerFlag::eTRANSMIT_CONTACTS, true);
        mPxPhysics->getVisualDebugger()->setVisualDebuggerFlag(PxVisualDebuggerFlag::eTRANSMIT_SCENEQUERIES, true);
        physx::PxVisualDebuggerConnection* conn = PxVisualDebuggerExt::createConnection(mPxPhysics->getPvdConnectionManager(), "127.0.0.1", 5425, 10);
    }

    PxSceneDesc sceneDesc(mPxPhysics->getTolerancesScale());
    sceneDesc.gravity = PxVec3(0.0f, -9.81f, 0.0f);
    mPxDispatcher = PxDefaultCpuDispatcherCreate(2);
    sceneDesc.cpuDispatcher = mPxDispatcher;
    sceneDesc.filterShader = PxDefaultSimulationFilterShader;
    mPxScene = mPxPhysics->createScene(sceneDesc);

    mPxCooking = PxCreateCooking(PX_PHYSICS_VERSION, *mPxFoundation, PxCookingParams(PxTolerancesScale()));

    mPxMaterial = mPxPhysics->createMaterial(0.5f, 0.5f, 0.6f);

}

physx::PxVehicleDrive4W* Demo::PhysicsLogicSystem::createPxVehicle4W()
{
    PxInitVehicleSDK(*mPxPhysics);
    PxVehicleSetBasisVectors(PxVec3(0, 1, 0), PxVec3(0, 0, 1));
    PxVehicleSetUpdateMode(PxVehicleUpdateMode::eVELOCITY_CHANGE);

    //Create the batched scene queries for the suspension raycasts.
    mVehicleQueryData = VehicleSceneQueryData::allocate(1, PX_MAX_NB_WHEELS, 1, *mPxAllocator);
    gBatchQuery = VehicleSceneQueryData::setUpBatchedSceneQuery(0, *mVehicleQueryData, mPxScene);

    //Create the friction table for each combination of tire and surface type.
    mPxFrictionPairs = createFrictionPairs(mPxMaterial);

    //Create a plane to drive on.
    PxRigidStatic* ground = createDrivablePlane(mPxMaterial, mPxPhysics);
    mPxScene->addActor(*ground);

    //Create a vehicle that will drive on the plane.
    VehicleDesc vehicleDesc = initVehicleDesc();
    mVehicle = createVehicle4W(vehicleDesc, mPxPhysics, mPxCooking);
    PxTransform startTransform(PxVec3(0, (vehicleDesc.chassisDims.y*0.5f + vehicleDesc.wheelRadius + 1.0f), 0), PxQuat(PxIdentity));
    mVehicle->getRigidDynamicActor()->setGlobalPose(startTransform);
    mPxScene->addActor(*mVehicle->getRigidDynamicActor());

    //Set the vehicle to rest in first gear.
    //Set the vehicle to use auto-gears.
    mVehicle->setToRestState();
    mVehicle->mDriveDynData.forceGearChange(PxVehicleGearsData::eFIRST);
    mVehicle->mDriveDynData.setUseAutoGears(true);

    gVehicleInputData.setDigitalAccel(1.0f);
    return mVehicle;
}
void PhysicsLogicSystem::destroyPhysicsWorld()
{

}

void PhysicsLogicSystem::update(float timeSinceLast)
{
    BaseSystem::update(timeSinceLast);
    if (mPxScene)
    {
        //Update the control inputs for the vehicle.
        if (mVehicle)
        {
            PxVehicleDrive4WSmoothDigitalRawInputsAndSetAnalogInputs(gKeySmoothingData, gSteerVsForwardSpeedTable, gVehicleInputData, timeSinceLast, gIsVehicleInAir, *mVehicle);

            //Raycasts.
            PxVehicleWheels* vehicles[1] = { mVehicle };
            PxRaycastQueryResult* raycastResults = mVehicleQueryData->getRaycastQueryResultBuffer(0);
            const PxU32 raycastResultsSize = mVehicleQueryData->getRaycastQueryResultBufferSize();
            PxVehicleSuspensionRaycasts(gBatchQuery, 1, vehicles, raycastResultsSize, raycastResults);

            //Vehicle update.
            const PxVec3 grav = mPxScene->getGravity();
            PxWheelQueryResult wheelQueryResults[PX_MAX_NB_WHEELS];
            PxVehicleWheelQueryResult vehicleQueryResults[1] = { { wheelQueryResults, mVehicle->mWheelsSimData.getNbWheels() } };
            PxVehicleUpdates(timeSinceLast, grav, *mPxFrictionPairs, 1, vehicles, vehicleQueryResults);
            gIsVehicleInAir = mVehicle->getRigidDynamicActor()->isSleeping() ? false : PxVehicleIsInAir(vehicleQueryResults[0]);
        }

        mPxScene->simulate(timeSinceLast);//1.0f / 60.0f);
        mPxScene->fetchResults(true);
    }
}

void Demo::PhysicsLogicSystem::updateDrive(float step)
{
    mVehicle->getRigidDynamicActor()->wakeUp();
}


VehicleDesc Demo::PhysicsLogicSystem::initVehicleDesc()
{
    //Set up the chassis mass, dimensions, moment of inertia, and center of mass offset.
    //The moment of inertia is just the moment of inertia of a cuboid but modified for easier steering.
    //Center of mass offset is 0.65m above the base of the chassis and 0.25m towards the front.
    const PxF32 chassisMass = 1500.0f;
    const PxVec3 chassisDims(2.5f, 2.0f, 5.0f);
    const PxVec3 chassisMOI
        ((chassisDims.y*chassisDims.y + chassisDims.z*chassisDims.z)*chassisMass / 12.0f,
        (chassisDims.x*chassisDims.x + chassisDims.z*chassisDims.z)*0.8f*chassisMass / 12.0f,
        (chassisDims.x*chassisDims.x + chassisDims.y*chassisDims.y)*chassisMass / 12.0f);
    const PxVec3 chassisCMOffset(0.0f, -chassisDims.y*0.5f + 0.65f, 0.25f);

    //Set up the wheel mass, radius, width, moment of inertia, and number of wheels.
    //Moment of inertia is just the moment of inertia of a cylinder.
    const PxF32 wheelMass = 20.0f;
    const PxF32 wheelRadius = 0.5f;
    const PxF32 wheelWidth = 0.4f;
    const PxF32 wheelMOI = 0.5f*wheelMass*wheelRadius*wheelRadius;
    const PxU32 nbWheels = 4;

    VehicleDesc vehicleDesc;
    vehicleDesc.chassisMass = chassisMass;
    vehicleDesc.chassisDims = chassisDims;
    vehicleDesc.chassisMOI = chassisMOI;
    vehicleDesc.chassisCMOffset = chassisCMOffset;
    vehicleDesc.chassisMaterial = mPxMaterial;
    vehicleDesc.wheelMass = wheelMass;
    vehicleDesc.wheelRadius = wheelRadius;
    vehicleDesc.wheelWidth = wheelWidth;
    vehicleDesc.wheelMOI = wheelMOI;
    vehicleDesc.numWheels = nbWheels;
    vehicleDesc.wheelMaterial = mPxMaterial;
    return vehicleDesc;
}

void Demo::ErrCallback::reportError(PxErrorCode::Enum code, const char* message, const char* file, int line)
{
    std::stringstream ss;
    ss << file << ":" << line << ":" << message;
    Ogre::LogManager::getSingleton().getDefaultLog()->logMessage(ss.str());
}