/**

 * @file

 * @authors Nahuel Villa, Maximilien Naveau

 * @copyright Copyright (c) 2022 LAAS-CNRS, TOWARD, PAL_ROBOTICS

 *            License BSD-2

 *

 * @brief Class to perform inverse geometry on a biped robot.

 */

#ifndef DYNACOM_DYNACOM

#define DYNACOM_DYNACOM

// clang-format off

#include <Eigen/Dense>

#include "pinocchio/fwd.hpp"

#include "pinocchio/multibody/data.hpp"

#include "pinocchio/multibody/model.hpp"

#include "pinocchio/spatial/se3.hpp"

#include <map>

#include <memory>

#include "dynacom/contact6d.hpp"

// clang-format on

// @TODO: add abstract class contact and new implementation of it contactPoint.

// @TODO: implement method copy of contacts to be able to duplicate them easily.

// @TODO: In the getters uni_A, fria_A, uni_b, fri_b return only the active

// block.

// @TODO: change internaly the system of names by ids.

namespace dynacom {

struct DynaCoMSettings {

 public:

  /**

   * @brief This must contain either a valid path to the urdf file or the

   * content of this file in a string.

   */

  std::string urdf = "";

  friend std::ostream &operator<<(std::ostream &out,

                                  const DynaCoMSettings &obj) {

    out << "DynaCoMSettings:\n";

    out << "    urdf: " << obj.urdf << std::endl;

    return out;

  }

  friend bool operator==(const DynaCoMSettings &lhs,

                         const DynaCoMSettings &rhs) {

    bool test = true;

    test &= lhs.urdf == rhs.urdf;

    return test;

  }

};

class DynaCoM {

 public:

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

 private:

  DynaCoMSettings settings_;

  Eigen::Matrix2d S_;

  pinocchio::Model model_;

  pinocchio::Data data_;

  double mass_;

  double inf_ = std::numeric_limits<double>::infinity();

  // Lists of contacts

  // std::vector< std::shared_ptr<Contact6D> > known_contact6ds_;

  std::map<std::string, std::shared_ptr<Contact6D>> known_contact6ds_;

  std::vector<std::string> active_contact6ds_;

  // QP Matrices

  Eigen::MatrixXd unilaterality_A_;

  Eigen::VectorXd unilaterality_b_;

  Eigen::MatrixXd friction_A_;

  Eigen::VectorXd friction_b_;

  Eigen::VectorXd regularization_A_;

  Eigen::VectorXd regularization_b_;

  Eigen::Matrix<double, 6, -1> newton_euler_A_;

  Eigen::Matrix<double, 6, 1> newton_euler_b_;

  // variables for QP problem with proxsuite formulation

  //  Eigen::MatrixXd H_, , A_;

  //  Eigen::VectorXd g_, u_, l_;

  //  Eigen::Matrix<double, 6, 1> b_;

  // variables for QP problem with eiquadprog formulation

  Eigen::MatrixXd G_;            // matrix for quadratic cost

  Eigen::VectorXd g0_;           // linear part of the cost function

  Eigen::MatrixXd CE_, CI_, C_;  // constraints matrix

  Eigen::VectorXd ce0_, ci0_;    // constraints vector

  // variables for QP problem

  Eigen::VectorXd F_;

  Eigen::VectorXi activeSet_;

  size_t activeSetSize_;

  size_t cols_, uni_rows_, fri_rows_;

  // active sizes:

  size_t uni_i_, fri_i_, j_;

  // Internal variables:

  Eigen::Vector3d groundCoMForce_, groundCoMTorque_, CoPTorque_, weight_;

  Eigen::Vector3d acom_;

  Eigen::Vector2d cop_;

  Eigen::Vector3d dL_;

  Eigen::Vector3d L_;

  Eigen::Vector2d n_;

  std::vector<std::string>::iterator activeID_;

  std::map<std::string, std::shared_ptr<Contact6D>>::iterator knownID_;

  // for the CoP wrench

  pinocchio::SE3 oMso_, soMs_;

  Eigen::Matrix<double, 6, 6> Sz_, oXso_, soXs_;

  void addSizes(const std::shared_ptr<Contact6D> &contact);

  void removeSizes(const std::shared_ptr<Contact6D> &contact);

  void resizeMatrices();

  void buildMatrices(const Eigen::Vector3d &groundCoMForce,

                     const Eigen::Vector3d &groundCoMTorque,

                     const Eigen::Vector3d &CoM);

  void solveQP();

  void distribute();

  const Eigen::Matrix<double, 6, 6> toWorldCoPWrench(pinocchio::SE3 pose);

 public:

  DynaCoM();

  DynaCoM(const DynaCoMSettings settings);

  void initialize(const DynaCoMSettings settings);

  void computeDynamics(const Eigen::VectorXd &posture,

                       const Eigen::VectorXd &velocity,

                       const Eigen::VectorXd &acceleration,

                       const Eigen::Matrix<double, 6, 1> &externalWrench =

                           Eigen::Matrix<double, 6, 1>::Zero(),

                       bool flatHorizontalGround = true);

  void computeNL(const double &w, const Eigen::VectorXd &posture,

                 const Eigen::VectorXd &velocity,

                 const Eigen::VectorXd &acceleration,

                 const Eigen::Matrix<double, 6, 1> &externalWrench =

                     Eigen::Matrix<double, 6, 1>::Zero(),

                 bool flatHorizontalGround = true);

  void computeNL(const double &w);

  void addContact6d(const std::shared_ptr<Contact6D> &contact,

                    const std::string &name, const bool active = true);

  void removeContact6d(const std::string &name);

  void activateContact6d(const std::string &name);

  void deactivateContact6d(const std::string &name);

  void distributeForce(const Eigen::Vector3d &groundCoMForce,

                       const Eigen::Vector3d &groundCoMTorque,

                       const Eigen::Vector3d &CoM);

  // GETTERS

  /// @brief Please call computeDynamics first.

  const Eigen::Vector3d &getAMVariation() { return dL_; }

  }

  }

  }

  }

};

}  // namespace dynacom

#endif  // DYNACOM_DYNACOM