// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT)
// SPDX-License-Identifier: BSD-3-Clause

#include <iDynTree/GeomVector3.h>

#include <iDynTree/Rotation.h>

#include <Eigen/Dense>

namespace iDynTree
{
typedef Eigen::Matrix<double, 3, 1> Vector3d;
typedef Eigen::Matrix<double, 3, 3, Eigen::RowMajor> Matrix3dRowMajor;

/**
 * GeomVector3 Method definitions ============================================
 */

// constructors
GeomVector3::GeomVector3(const double* in_data, const unsigned int in_size)
    : Vector3(in_data, in_size)
{
}

GeomVector3::GeomVector3(const double x, const double y, const double z)
{
    this->m_data[0] = x;
    this->m_data[1] = y;
    this->m_data[2] = z;
}

GeomVector3::GeomVector3(const Vector3 other)
{
    this->m_data[0] = other(0);
    this->m_data[1] = other(1);
    this->m_data[2] = other(2);
}

// Geometric operations
GeomVector3 GeomVector3::changeCoordFrame(const Rotation& newCoordFrame) const
{
    GeomVector3 result;
    Eigen::Map<const Vector3d> thisMap(this->data());
    Eigen::Map<const Matrix3dRowMajor> rotMap(newCoordFrame.data());
    Eigen::Map<Vector3d> resultMap(result.data());

    resultMap = rotMap * thisMap;

    return result;
}

GeomVector3 GeomVector3::compose(const GeomVector3& op1, const GeomVector3& op2) const
{
    GeomVector3 result;
    Eigen::Map<const Vector3d> op1Data(op1.data());
    Eigen::Map<const Vector3d> op2Data(op2.data());
    Eigen::Map<Vector3d> resultData(result.data());

    resultData = op1Data + op2Data;

    return result;
}

GeomVector3 GeomVector3::inverse(const GeomVector3& op) const
{
    GeomVector3 result;

    Eigen::Map<const Vector3d> opData(op.data());
    Eigen::Map<Vector3d> resultData(result.data());

    resultData = -opData;

    return result;
}

// dot product
double GeomVector3::dot(const GeomVector3& other) const
{
    Eigen::Map<const Vector3d> otherData(other.data());
    Eigen::Map<const Vector3d> thisData(this->data());

    return thisData.dot(otherData);
}

// overloaded operators
GeomVector3 GeomVector3::operator+(const GeomVector3& other) const
{
    return compose(*this, other);
}

GeomVector3 GeomVector3::operator-(const GeomVector3& other) const
{
    return compose(*this, inverse(other));
}

GeomVector3 GeomVector3::operator-() const
{
    return inverse(*this);
}

Rotation GeomVector3::exp() const
{
    Rotation ret;
    Eigen::Map<const Eigen::Vector3d> thisVec(this->data());
    Eigen::AngleAxisd aa(thisVec.norm(), thisVec.normalized());

    Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor>>(ret.data()) = aa.toRotationMatrix();

    return ret;
}

GeomVector3 GeomVector3::cross(const GeomVector3& other) const
{
    GeomVector3 result;
    Eigen::Map<const Eigen::Vector3d> thisData(this->data());
    Eigen::Map<const Eigen::Vector3d> otherData(other.data());
    Eigen::Map<Eigen::Vector3d> resultData(result.data());

    resultData = thisData.cross(otherData);

    return result;
}

} // namespace iDynTree
