// This code is part of my Bachelor's thesis.
// It is a C++ implementation of a cloth simulation using the backward Euler method (or implicit Euler method)
// for numerical integration. It required calculating the derivatives of every force in the simulation and storing
// them in a big matrix. I used dispersed matrices to improve the efficiency of the code since it avoided thousands
// of unnecessary operations.

PhysicsManager::SimulationInfo PhysicsManager::StepImplicit(float h, SimulationInfo previousState, int iterations)
{
	Eigen::VectorXd x = previousState.x;// Node positions
	Eigen::VectorXd v = previousState.v;// Node velocities

	for (int iter = 0; iter < iterations; iter++)
	{

		Eigen::VectorXd f = Eigen::VectorXd::Constant(m_numDoFs, 0.0);//Forces

		SpMat dFdx(m_numDoFs, m_numDoFs);
		SpMat dFdv(m_numDoFs, m_numDoFs);
		std::vector derivPos = std::vector();
		std::vector derivVel = std::vector();

		SpMat M(m_numDoFs, m_numDoFs);
		std::vector masses = std::vector();

		std::vector fixedIndices(m_numDoFs);

		for (int i = 0; i < SimObjects.size(); i++)
		{
			SimObjects[i]->SetPosition(&x);
			SimObjects[i]->SetVelocity(&v);

			SimObjects[i]->GetMass(&masses);
			SimObjects[i]->GetFixedIndices(&fixedIndices);
		}

		//FORCES
		for (int i = 0; i < SimObjects.size(); i++)
		{
			// Fill f with forces from every node and spring.
			SimObjects[i]->GetForce(&f, Colliders); 

			// Fill derivPos and derivVel with the corresponing derivatives.
			SimObjects[i]->GetForceJacobian(&derivPos, &derivVel, Colliders);
		}

		dFdx.setFromTriplets(derivPos.begin(), derivPos.end(), [](const double& a, const double& b) { return a + b; });
		dFdv.setFromTriplets(derivVel.begin(), derivVel.end(), [](const double& a, const double& b) { return a + b; });
		M.setFromTriplets(masses.begin(), masses.end());

		//MATRIX OPERATIONS

		SpMat firstPart = M + (-h) * dFdv;

		SpMat A = firstPart + (-h * h) * dFdx;
		Eigen::VectorXd b;

		if (iter == 0) {
			b = firstPart * v + h * f;
		}
		else {
			b = (M * previousState.v) + (h * f) - (h * dFdv * v) - (h * h * dFdx * x);
		}

		//FIXING SELECTED VERTICES

		for (int i = 0; i < A.outerSize(); i++) {
			for (SpMat::InnerIterator j(A, i); j; ++j) {
				if (fixedIndices[i] || fixedIndices[j.row()]){
                    if (i == j.row())
                        j.valueRef() = 1;
                    else
                        j.valueRef() = 0;
                }
			}
		}

		for (size_t i = 0; i < m_numDoFs; i++)
		{
			if (fixedIndices[i]) {
				b[i] = 0;
			}
		}

		//SOLVING

		Eigen::ConjugateGradient, Eigen::Lower | Eigen::Upper> cg;//UnitLower UnitUpper
		cg.setTolerance(tolerance);
		cg.compute(A);

		v = cg.solveWithGuess(b, v);
		x = previousState.x + h * v;
	}

	SimulationInfo newState;
	newState.x = x;
	newState.v = v;

	return newState;
}