//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program.  If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------


#include "mcapi_utils.h"

Vector3d computeAccFromSpatial(const Vector3d& ddr, const Vector3d& w, const Vector3d& dr)
{
    return ddr + w.cross(dr);
}


std::vector<Vector3d> contacts_from_footrect_center()
{
    // compute feet corners coordinates like from the leg_X_6_joint
    double lx = 0.1;
    double ly = 0.065;
    double lz = 0.107;
    std::vector<Vector3d> contacts; contacts.resize(4);
    contacts[0] = {-lx, -ly, -lz};
    contacts[1] = {-lx,  ly, -lz};
    contacts[2] = { lx, -ly, -lz};
    contacts[3] = { lx,  ly, -lz};
    return contacts;
}


Matrix<double, 6, 1> contact_force_to_wrench(const std::vector<Vector3d>& contacts, 
                                 const Matrix<double, 12, 1>& cf12)
{
    Vector3d f = cf12.segment<3>(0) + cf12.segment<3>(3) + cf12.segment<3>(6) + cf12.segment<3>(9);
    Vector3d tau = contacts[0].cross(cf12.segment<3>(0))
                 + contacts[1].cross(cf12.segment<3>(3))
                 + contacts[2].cross(cf12.segment<3>(6))
                 + contacts[3].cross(cf12.segment<3>(9));
    Matrix<double, 6, 1> wrench; wrench << f, tau;
    return wrench;
}


Matrix3d compute_Iw(pinocchio::Model& model, 
                    pinocchio::Data& data, 
                    VectorXd& q_static, 
                    Vector3d& b_p_bc)
{
    MatrixXd b_Mc = pinocchio::crba(model, data, q_static);  // mass matrix at b frame expressed in b frame
    // MatrixXd b_Mc = crba(model_dist, data_dist, q_static);  // mass matrix at b frame expressed in b frame
    MatrixXd b_I_b = b_Mc.topLeftCorner<6,6>();             // inertia matrix at b frame expressed in b frame 
    pinocchio::SE3 bTc (Eigen::Matrix3d::Identity(), b_p_bc);          // "no free flyer robot -> c frame oriented the same as b"
    pinocchio::SE3 cTb = bTc.inverse();        
    // Ic = cXb^star * Ib * bXc = bXc^T * Ib * bXc 
    // c_I_c block diagonal:
    // [m*Id3, 03;
    //  0,     Iw]
    MatrixXd c_I_c = cTb.toDualActionMatrix() * b_I_b * bTc.toActionMatrix();
    Matrix3d b_Iw  = c_I_c.bottomRightCorner<3,3>();  // meas
    return b_Iw;
}