#include "twentieth.hpp"

Physics::Physics(irr::IrrlichtDevice *device, irr::video::IVideoDriver *driver, irr::scene::ISceneManager *sceneManager){                   
    newtonDevice = device;
    newtonDriver = driver;
    newtonSceneManager = sceneManager;
    
    newtonWorld = NewtonCreate(); // 2.0
    newtonNode = 0;
}

Physics::~Physics(){
    NewtonDestroy(newtonWorld);
}

void Physics::Cube(irr::core::vector3df position, irr::core::vector3df taille, float masse){
    
    irr::scene::IMesh* cubeMesh = newtonSceneManager->getMesh("data/smallcube.3ds");
    newtonNode = newtonSceneManager->addMeshSceneNode(cubeMesh);
    newtonNode->setMaterialTexture(0, newtonDriver->getTexture("data/crate.jpg"));
    newtonNode->setMaterialFlag(irr::video::EMF_LIGHTING, false);
    
    NewtonCollision* collision;
    
    collision = NewtonCreateBox(newtonWorld, taille.X, taille.Y, taille.Z, 0, NULL); // 2.00
    
    newtonBody = NewtonCreateBody(newtonWorld, collision, NULL); // 2.00
    NewtonReleaseCollision(newtonWorld, collision);  
    
    NewtonBodySetUserData(newtonBody,reinterpret_cast<void*>(newtonNode));
    
    if(masse == 0.0){newtonNode->setPosition(position);}
    
    if(masse != 0.0){
        irr::core::vector3df inertie;
        inertie.X = (masse/12)*(pow(taille.Y,2)+pow(taille.Z,2));
        inertie.Y = (masse/12)*(pow(taille.X,2)+pow(taille.Z,2));
        inertie.Z = (masse/12)*(pow(taille.X,2)+pow(taille.Y,2));
        NewtonBodySetMassMatrix (newtonBody, masse, inertie.X, inertie.Y, inertie.Z);
        
        NewtonBodySetTransformCallback(newtonBody, SetMeshTransformEvent);
	    NewtonBodySetForceAndTorqueCallback(newtonBody, ApplyForceAndTorqueEvent);
    }
    
    irr::core::matrix4 mat;
    mat.setTranslation(position);
    NewtonBodySetMatrix(newtonBody, mat.pointer());
}

void Physics::Update(){
   
    if (newtonDevice->getTimer()->getTime() > lasttick + 10)
    {   
        lasttick = newtonDevice->getTimer()->getTime();
        NewtonUpdate(newtonWorld, 0.01f);
    }
}

// CALLBACKS -------------------------------------------------------------------

// 2.00

void Physics::SetMeshTransformEvent(const NewtonBody* newtonBody, const float* matrix, int)
{
	irr::core::matrix4 mat;
	memcpy(mat.pointer(), matrix, sizeof(float)*16);

	irr::scene::ISceneNode *newtonNode = (irr::scene::ISceneNode *)NewtonBodyGetUserData(newtonBody);
	if (newtonNode)
	{
		newtonNode->setPosition(mat.getTranslation());
		newtonNode->setRotation(mat.getRotationDegrees());
	}
}

void Physics::ApplyForceAndTorqueEvent(const NewtonBody* newtonBody, float, int)
{ 
    float masse; 
    float inertieX; 
    float inertieY; 
    float inertieZ; 
    float force[3]; 
    float torque[3]; 
    
    NewtonBodyGetMassMatrix (newtonBody, &masse, &inertieX, &inertieY, &inertieZ); 
    
    force[0] = 0.0f; 
    force[1] = -9.81 * masse; 
    force[2] = 0.0f; 
    
    torque[0] = 0.0f; 
    torque[1] = 0.0f; 
    torque[2] = 0.0f; 
    
    NewtonBodyAddForce(newtonBody, force); 
    NewtonBodyAddTorque(newtonBody, torque); 
}

// -----------------------------------------------------------------------------