#ifndef CNOID_BODY_FORWARD_DYNAMICS_CBM_H
#define CNOID_BODY_FORWARD_DYNAMICS_CBM_H

#include "ForwardDynamics.h"
#include "exportdecl.h"

namespace cnoid {

class DyLink;
class ForceSensorDevice;

/**
   The ForwardDynamicsCBM class calculates the forward dynamics using
   the motion equation based on the generalized mass matrix.
   The class is mainly used for a body that has high-gain mode joints.
   If all the joints of a body are the torque mode, the ForwardDynamicsABM,
   which uses the articulated body method, is more efficient.
*/
class CNOID_EXPORT ForwardDynamicsCBM : public ForwardDynamics
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

    ForwardDynamicsCBM(DySubBody* subBody);
    ~ForwardDynamicsCBM();

    virtual void initialize();
    virtual void calcNextState();
    virtual void refreshState();

    void complementHighGainModeCommandValues();

    void initializeAccelSolver();
    void sumExternalForces();
    void solveUnknownAccels();
    void solveUnknownAccels(const Vector3& fext, const Vector3& tauext);
    bool solveUnknownAccels(
        DyLink* link, const Vector3& fext, const Vector3& tauext, const Vector3& rootfext, const Vector3& roottauext);

private:
        
    /*
      Elements of the motion equation
		   
      |     |     |   | dv         |   | b1 |   | 0  |   | totalfext      |
      | M11 | M12 |   | dw         |   |    |   | 0  |   | totaltauext    |
      |     |     | * |ddq (unkown)| + |    | + | d1 | = | u (known)      |
      |-----+-----|   |------------|   |----|   |----|   |----------------|
      | M21 | M22 |   | given ddq  |   | b2 |   | d2 |   | u2             |
		
                   |fext  |
      d1 = trans(s)|      |
                   |tauext|

    */
		
    MatrixXd M11;
    MatrixXd M12;
    MatrixXd b1;
    VectorXd d1;
    VectorXd c1;

    std::vector<DyLink*> torqueModeJoints;
    std::vector<DyLink*> highGainModeJoints;

    //int rootDof; // dof of dv and dw (0 or 6)
    int unknown_rootDof;
    int given_rootDof;

    bool isNoUnknownAccelMode;

    VectorXd qGiven;
    VectorXd dqGiven;
    VectorXd ddqGiven;
    Vector3 pGiven;
    Matrix3 RGiven;
    Vector3 voGiven;
    Vector3 wGiven;

    bool accelSolverInitialized;
    bool ddqGivenCopied;

    VectorXd qGivenPrev;
    VectorXd dqGivenPrev;
    Vector3 pGivenPrev;
    Matrix3 RGivenPrev;
    Vector3 voGivenPrev;
    Vector3 wGivenPrev;

    Vector3 fextTotal;
    Vector3 tauextTotal;

    Vector3 root_w_x_v;

    // buffers for the unit vector method
    VectorXd ddqorg;
    VectorXd uorg;
    Vector3 dvoorg;
    Vector3 dworg;

    // Buffers for the Runge Kutta Method
    Isometry3 T0;
    Vector3 vo0;
    Vector3 w0;
    std::vector<double> q0;
    std::vector<double> dq0;
		
    Vector3 vo;
    Vector3 w;
    Vector3 dvo;
    Vector3 dw;
    std::vector<double> dq;
    std::vector<double> ddq;

    virtual void initializeSensors();

    void calcMotionWithSemiImplicitEulerMethod();
    void calcMotionWithRungeKuttaMethod();
    void integrateRungeKuttaOneStep(double r, double dt);
    void preserveHighGainModeJointState();
    void calcPositionAndVelocityFK();
    void calcMassMatrix();
    void setColumnOfMassMatrix(MatrixXd& M, int column);
    void calcInverseDynamics(DyLink* link, Vector3& out_f, Vector3& out_tau, bool isSubBodyRoot);
    void calcd1(DyLink* link, Vector3& out_f, Vector3& out_tau, bool isSubBodyRoot);
    inline void calcAccelFKandForceSensorValues();
    void calcAccelFKandForceSensorValues(DyLink* link, Vector3& out_f, Vector3& out_tau, bool isSubBodyRoot);
    void updateForceSensors();
};

}

#endif
