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

#include <iDynTree/Direction.h>

#include <iDynTree/EigenHelpers.h>

#include <Eigen/Dense>

#include <cstdio>
#include <sstream>

namespace iDynTree
{
Direction::Direction(double x, double y, double z)
{
    this->m_data[0] = x;
    this->m_data[1] = y;
    this->m_data[2] = z;

    this->Normalize();
}

Direction::Direction(const double* in_data, const unsigned int in_size)
    : VectorFixSize<3>(in_data, in_size)
{
    this->Normalize();
}

Direction::Direction(const Direction& other)
    : VectorFixSize<int(3)>(other)
{
    this->m_data[0] = other.m_data[0];
    this->m_data[1] = other.m_data[1];
    this->m_data[2] = other.m_data[2];
}

Direction& Direction::operator=(const Direction& other)
{
    this->m_data[0] = other.m_data[0];
    this->m_data[1] = other.m_data[1];
    this->m_data[2] = other.m_data[2];
    return *this;
}

void Direction::setToDefault()
{
    this->m_data[0] = 1.0;
    this->m_data[1] = 0.0;
    this->m_data[2] = 0.0;
}

void Direction::Normalize(double tol)
{
    Eigen::Map<Eigen::Vector3d> vec(this->m_data);
    double nrm = vec.norm();
    if (nrm < tol)
    {
        this->setToDefault();
    } else
    {
        vec.normalize();
    }

    return;
}

bool Direction::isParallel(const Direction& otherDirection, double tolerance) const
{
    // The tolerance should be positive
    assert(tolerance > 0);

    // Compute the difference of the norm
    Eigen::Vector3d diff = toEigen(*this) - toEigen(otherDirection);
    double diffNorm = diff.norm();

    // There are two possibilities for two directions to be parallel
    // either they point in the same direction, or in opposite directions
    if ((diffNorm < tolerance) || (fabs(diffNorm - 2) < tolerance))
    {
        return true;
    } else
    {
        return false;
    }
}

bool Direction::isPerpendicular(const Direction& otherDirection, double tolerance) const
{
    assert(tolerance > 0);

    double fabsDotProduct = fabs(toEigen(*this).dot(toEigen(otherDirection)));

    if (fabsDotProduct > tolerance)
    {
        return false;
    } else
    {
        return true;
    }
}

Direction Direction::reverse() const
{
    return Direction(-this->m_data[0], -this->m_data[1], -this->m_data[2]);
}

std::string Direction::toString() const
{
    std::stringstream ss;

    ss << " x " << this->m_data[0] << " y " << this->m_data[1] << " z " << this->m_data[2];

    return ss.str();
}

std::string Direction::reservedToString() const
{
    return this->toString();
}

Direction Direction::Default()
{
    Direction res;
    res.setToDefault();
    return res;
}

} // namespace iDynTree
