// State
class State                { Eigen::Vector positions, velocities; }

// Basic interfaces
class MassInterface        { addMass(state, Matrix* M)=0; };
class DeformationInterface { addF(state, Vector* F)=0; addDamping(state, Matrix* D)=0; addStiffness(state, Matrix* K)=0; };

// Basic elements deriving from the interfaces
class MassElement         : public MassInterface{ double mass; addMass(state, Matrix* M); }
class LinearSpringElement : public DeformationInterface{ double stiffness, damping, restLength; addF(state, Vector* F); addDamping(state, Matrix* D); addStiffness(state, Matrix* K); };
class RigidElement        : public MassInterface { Shape* shape; addMass(state, Matrix* M); }
class FemElement          : public MassInterface,
                            public DeformationInterface { addMass(state, Matrix* M); addF(state, Vector* F); addDamping(state, Matrix* D); addStiffness(state, Matrix* K); };
// Concrete elements for FEM:
class FemElement1D : public FemElement {};
class FemElement2D : public FemElement {};
class FemElement3D : public FemElement {};


class ExternalForce : public DeformationInterface; // Just for semantic purpose...but it is really the same interface !
class Gravity         : public ExternalForce { addF(state, Vector* F){mg};           addStiffness(state, Matrix *K){}; addDamping(state, Matrix *D){}; }
class RigidTorque     : public ExternalForce { addF(state, Vector* F){w.cross(I.w)}; addStiffness(state, Matrix *K){}; addDamping(state, Matrix *D){}; }
class RayleighDamping : public ExternalForce { addF(state, Vector* F){-cv};          addStiffness(state, Matrix *K){}; addDamping(state, Matrix *D){c.Identity()}; }
class VTC             : public ExternalForce // ???? The behavior could be simply about grabbing the latest input pose, but the force calculation itself should be only done on request.
                                             // also, this architecture would be more suited for the VTC as the forces could be recomputed in the loop with an updated state.
                                             // in our current implementation, the force/derivative are set once for the full time step.

template<...> class Representation : public SurgSim::Framework::Representation, public OdeEquation<...>
{
  vector<MassInterface>        massInterfaces;
  vector<DeformationInterface> deformationInterfaces;
  vector<ExternalForces>       externalForces;
  vector<int>                  boundaryConditions;
  OdeSolver<...>               odeSolver;
  Matrix M, D, K;
  Vector F;
  
  update()
  {
    odeSolver.solve()
  }
  
  void applyBoundaryConditions(Vector *v) { apply bc to v } // For future use by the OdeSolver, boundary conditions should be set in the solver on specific structures
  void applyBoundaryConditions(Matrix *v) { apply bc to m } // For future use by the OdeSolver, boundary conditions should be set in the solver on specific structures
  
  Vector& computeF(state)
  {
    for all deformation in deformationInterfaces
      deformation.addF(state, &F);
    for all externalF in externalForces
      externalF.addF(state, &F);
    for all bc in boundaryConditions // This is what we have now, applying the bc when building each entity (f, M, D, K)
      apply bc                       // but, theoritically the OdeSolver should request this on a special structure (could be MDK assembled)
                                     // this is why i added the 2 methods applyBoundaryConditions above, to introduce this concept in this architecture.
                                     // This would be a much proper handling of the boundary conditions !
  }

  Matrix& computeM(state)
  {
    for all mass in massInterfaces
      mass.addMass(state, &M);
    for all bc in boundaryConditions // This is what we have now, applying the bc when building each entity (f, M, D, K)
      apply bc                       // but, theoritically the OdeSolver should request this on a special structure (could be MDK assembled)
                                     // this is why i added the 2 methods applyBoundaryConditions above, to introduce this concept in this architecture.
                                     // This would be a much proper handling of the boundary conditions !
  }

  Matrix& computeK(state)
  {
    for all deformation in deformationInterfaces
      deformation.addStiffness(state, &K);
    for all externalF in externalForces
      externalF.addStiffness(state, &K);
    for all bc in boundaryConditions // This is what we have now, applying the bc when building each entity (f, M, D, K)
      apply bc                       // but, theoritically the OdeSolver should request this on a special structure (could be MDK assembled)
                                     // this is why i added the 2 methods applyBoundaryConditions above, to introduce this concept in this architecture.
                                     // This would be a much proper handling of the boundary conditions !
  }
}

class MassSpring1dRep<Mdiagonal, D,K band> : public Representation {...}
class MassSpring2dRep<Mdiagonal, D,K sparse> : public Representation {...}
class MassSpring3dRep<Mdiagonal, D,K sparse> : public Representation {...}

class Fem1DRep<Tri-diagonalblock matrices> : public Representation {...add FemElement1d...}
class Fem2DRep<sparse matrices> : public Representation {...add FemElement2d...}
class Fem3DRep<sparse matrices> : public Representation {...add FemElement3d...}

class RigidRep<6x6 matrix> : public Representation
{
  virtual update() override
  {
    Representation::update();
    // update the quaternion properly, the rotation matrix and the global inertia matrix
  }
}


// ###############
// NOTE
// DeformationInterface and ExternalForce could be combined and renamed into a unique class ForceInterface;
//   then we could mix them in the Representation in the same vector, or keep them separate to have internal forces and external forces separate.