text: // This file is a part of the OpenSurgSim project.
  // Copyright 2013, SimQuest Solutions Inc.
  //
  // Licensed under the Apache License, Version 2.0 (the "License");
  // you may not use this file except in compliance with the License.
  // You may obtain a copy of the License at
  //
  //     http://www.apache.org/licenses/LICENSE-2.0
  //
  // Unless required by applicable law or agreed to in writing, software
  // distributed under the License is distributed on an "AS IS" BASIS,
  // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  // See the License for the specific language governing permissions and
  // limitations under the License.
  
  /// \file
  /// Tests for the class OdeSolverEulerExplicitModified.
  ///
  
  #include <memory>
  
  #include <gtest/gtest.h>
  
  #include "SurgSim/Math/OdeSolverEulerExplicitModified.h"
  #include "SurgSim/Math/OdeSolverLinearEulerExplicitModified.h"
  #include "SurgSim/Math/UnitTests/MockObject.h"
  
  namespace SurgSim
  {
  
  namespace Math
  {
  
  namespace
  {
  template<class T>
  void doConstructorTest()
  {
  	MassPoint m;
  	ASSERT_NO_THROW({T solver(&m);});
  }
  };
  
  TEST(OdeSolverEulerExplicitModified, ConstructorTest)
  {
  	{
  		SCOPED_TRACE("EulerExplicitModified");
  		doConstructorTest<OdeSolverEulerExplicitModified>();
  	}
  	{
  		SCOPED_TRACE("LinearEulerExplicitModified");
  		doConstructorTest<OdeSolverLinearEulerExplicitModified>();
  	}
  }
  
  namespace
  {
  template<class T>
  void doSolveTest(bool computeCompliance)
  {
  	// Test 2 iterations because Linear solvers have a different algorithm on the 1st pass from the following passes.
  
  	{
  		MassPoint m;
  		MassPointState defaultState, state0, state1, state2;
  		auto solver = std::make_shared<T>(&m);
  		m.setOdeSolver(solver);
  
  		// ma = mg <=> a = g
  		// v(1) = g.dt + v(0)
  		// x(1) = v(1).dt + x(0)
  		ASSERT_NO_THROW({solver->solve(1e-3, state0, &state1, computeCompliance);});
  		EXPECT_EQ(defaultState, state0);
  		EXPECT_NE(defaultState, state1);
  		EXPECT_TRUE(state1.getVelocities().isApprox(m.m_gravity * 1e-3 + state0.getVelocities()));
  		EXPECT_TRUE(state1.getPositions().isApprox(state1.getVelocities() * 1e-3 + state0.getPositions()));
  
  		// v(2) = g.dt + v(1)
  		// x(2) = v(2).dt + x(1)
  		ASSERT_NO_THROW({solver->solve(1e-3, state1, &state2, computeCompliance);});
  		EXPECT_NE(defaultState, state1);
  		EXPECT_NE(defaultState, state2);
  		EXPECT_NE(state2, state1);
  		EXPECT_TRUE(state2.getVelocities().isApprox(m.m_gravity * 1e-3 + state1.getVelocities()));
  		EXPECT_TRUE(state2.getPositions().isApprox(state2.getVelocities() * 1e-3 + state1.getPositions()));
  	}
  
  	{
  		MassPoint m(0.1);
  		MassPointState defaultState, state0, state1, state2;
  		auto solver = std::make_shared<T>(&m);
  		m.setOdeSolver(solver);
  
  		// ma = mg - c.v <=> a = g - c/m.v
  		// v(1) = (g - c/m.v).dt + v(0)
  		// x(1) = v(1).dt + x(0)
  		ASSERT_NO_THROW({solver->solve(1e-3, state0, &state1, computeCompliance);});
  		EXPECT_EQ(defaultState, state0);
  		EXPECT_NE(defaultState, state1);
  		Vector3d acceleration0 = m.m_gravity - 0.1 * state0.getVelocities() / m.m_mass;
  		EXPECT_TRUE(state1.getVelocities().isApprox(acceleration0 * 1e-3 + state0.getVelocities()));
  		EXPECT_TRUE(state1.getPositions().isApprox(state1.getVelocities() * 1e-3  + state0.getPositions()));
  
  		// v(2) = (g - c/m.v).dt + v(1)
  		// x(2) = v(2).dt + x(1)
  		ASSERT_NO_THROW({solver->solve(1e-3, state1, &state2, computeCompliance);});
  		EXPECT_NE(defaultState, state1);
  		EXPECT_NE(defaultState, state2);
  		EXPECT_NE(state1, state2);
  		Vector3d acceleration1 = m.m_gravity - 0.1 * state1.getVelocities() / m.m_mass;
  		EXPECT_TRUE(state2.getVelocities().isApprox(acceleration1 * 1e-3 + state1.getVelocities()));
  		EXPECT_TRUE(state2.getPositions().isApprox(state2.getVelocities() * 1e-3 + state1.getPositions()));
  	}
  }
  };
  
  TEST(OdeSolverEulerExplicitModified, SolveTest)
  {
  	{
  		SCOPED_TRACE("EulerExplicitModified computing the compliance matrix");
  		doSolveTest<OdeSolverEulerExplicitModified>(true);
  	}
  	{
  		SCOPED_TRACE("EulerExplicitModified not computing the compliance matrix");
  		doSolveTest<OdeSolverEulerExplicitModified>(false);
  	}
  
  	{
  		SCOPED_TRACE("LinearEulerExplicitModified computing the compliance matrix");
  		doSolveTest<OdeSolverLinearEulerExplicitModified>(true);
  	}
  	{
  		SCOPED_TRACE("LinearEulerExplicitModified not computing the compliance matrix");
  		doSolveTest<OdeSolverLinearEulerExplicitModified>(false);
  	}
  }
  
  namespace
  {
  template <class T>
  void doComputeMatricesTest()
  {
  	MassPoint m;
  	T solver(&m);
  	MassPointState state;
  	double dt = 1e-3;
  
  	m.updateFMDK(state, SurgSim::Math::ODEEQUATIONUPDATE_M);
  	Matrix expectedSystemMatrix = m.getM() / dt;
  	EXPECT_NO_THROW(solver.computeMatrices(dt, state));
  	EXPECT_TRUE(solver.getSystemMatrix().isApprox(expectedSystemMatrix));
  	EXPECT_TRUE(solver.getComplianceMatrix().isApprox(expectedSystemMatrix.inverse()));
  }
  };
  
  TEST(OdeSolverEulerExplicitModified, ComputeMatricesTest)
  {
  	{
  		SCOPED_TRACE("EulerExplicitModified");
  		doComputeMatricesTest<OdeSolverEulerExplicitModified>();
  	}
  
  	{
  		SCOPED_TRACE("LinearEulerExplicitModified");
  		doComputeMatricesTest<OdeSolverLinearEulerExplicitModified>();
  	}
  }
  
  }; // Math
  
  }; // SurgSim
  