/** @file
 * Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
 * @author Daniel CHAPPUIS
 * @author Edouard DUPIN
 * @copyright 2010-2016, Daniel Chappuis
 * @copyright 2017, Edouard DUPIN
 * @license MPL v2.0 (see license file)
 */
#include <ephysics/engine/ConstraintSolver.hpp>
#include <ephysics/engine/Profiler.hpp>

using namespace ephysics;

ConstraintSolver::ConstraintSolver( etk::Map<RigidBody*, int> mapBodyToVelocityIndex):
  this.mapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex),
  this.isWarmStartingActive(true),
  this.raintSolverData(mapBodyToVelocityIndex) {
	
}

void ConstraintSolver::initializeForIsland(float dt, Island* island) {
	PROFILE("ConstraintSolver::initializeForIsland()");
	assert(island != null);
	assert(island->getNbBodies() > 0);
	assert(island->getNbJoints() > 0);
	// Set the current time step
	this.timeStep = dt;
	// Initialize the raint solver data used to initialize and solve the raints
	this.raintSolverData.timeStep = this.timeStep;
	this.raintSolverData.isWarmStartingActive = this.isWarmStartingActive;
	// For each joint of the island
	Joint** joints = island->getJoints();
	for (int iii=0; iii<island->getNbJoints(); ++iii) {
		// Initialize the raint before solving it
		joints[iii]->initBeforeSolve(this.raintSolverData);
		// Warm-start the raint if warm-starting is enabled
		if (this.isWarmStartingActive) {
			joints[iii]->warmstart(this.raintSolverData);
		}
	}
}

void ConstraintSolver::solveVelocityConstraints(Island* island) {
	PROFILE("ConstraintSolver::solveVelocityConstraints()");
	assert(island != null);
	assert(island->getNbJoints() > 0);
	// For each joint of the island
	Joint** joints = island->getJoints();
	for (int iii=0; iii<island->getNbJoints(); ++iii) {
		joints[iii]->solveVelocityConstraint(this.raintSolverData);
	}
}

void ConstraintSolver::solvePositionConstraints(Island* island) {
	PROFILE("ConstraintSolver::solvePositionConstraints()");
	assert(island != null);
	assert(island->getNbJoints() > 0);
	Joint** joints = island->getJoints();
	for (int iii=0; iii < island->getNbJoints(); ++iii) {
		joints[iii]->solvePositionConstraint(this.raintSolverData);
	}
}

void ConstraintSolver::setConstrainedVelocitiesArrays(vec3* rainedLinearVelocities,
                                                      vec3* rainedAngularVelocities) {
	assert(rainedLinearVelocities != null);
	assert(rainedAngularVelocities != null);
	this.raintSolverData.linearVelocities = rainedLinearVelocities;
	this.raintSolverData.angularVelocities = rainedAngularVelocities;
}

void ConstraintSolver::setConstrainedPositionsArrays(vec3* rainedPositions,
                                                     etk::Quaternion* rainedOrientations) {
	assert(rainedPositions != null);
	assert(rainedOrientations != null);
	this.raintSolverData.positions = rainedPositions;
	this.raintSolverData.orientations = rainedOrientations;
}