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

#include "TransformConstraint.h"
#include <iDynTree/Transform.h>

namespace internal
{
namespace kinematics
{

TransformConstraint::TransformConstraint(const std::string& constrainedFrameName,
                                         TransformConstraintType type)
    : m_type(type)
    , m_frameName(constrainedFrameName)
    , m_posWeight(1.0)
    , m_rotWeight(1.0)
    , m_resolutionMode(iDynTree::InverseKinematicsTreatTargetAsConstraintNone)
    , m_isActive(true)
{
}

TransformConstraint TransformConstraint::positionConstraint(const std::string& frameName,
                                                            const iDynTree::Position& position,
                                                            const double posWeight)
{
    TransformConstraint transformConstraint(frameName, TransformConstraintTypePosition);
    transformConstraint.m_transform.setPosition(position);
    transformConstraint.m_posWeight = posWeight;
    return transformConstraint;
}

TransformConstraint TransformConstraint::rotationConstraint(const std::string& frameName,
                                                            const iDynTree::Rotation& rotation,
                                                            const double rotWeight)
{
    TransformConstraint transformConstraint(frameName, TransformConstraintTypeRotation);
    transformConstraint.m_transform.setRotation(rotation);
    transformConstraint.m_rotWeight = rotWeight;
    return transformConstraint;
}

TransformConstraint TransformConstraint::fullTransformConstraint(const std::string& frameName,
                                                                 const iDynTree::Position& position,
                                                                 const iDynTree::Rotation& rotation,
                                                                 const double posWeight,
                                                                 const double rotWeight)
{
    iDynTree::Transform transform(rotation, position);
    return fullTransformConstraint(frameName, transform, posWeight, rotWeight);
}

TransformConstraint
TransformConstraint::fullTransformConstraint(const std::string& frameName,
                                             const iDynTree::Transform& _transform,
                                             const double posWeight,
                                             const double rotWeight)
{
    TransformConstraint transform(frameName, TransformConstraintTypeFullTransform);
    transform.m_transform = _transform;
    transform.m_posWeight = posWeight;
    transform.m_rotWeight = rotWeight;
    return transform;
}

unsigned TransformConstraint::getSize() const
{
    switch (m_type)
    {
    case TransformConstraintTypePosition:
        return 3;
    case TransformConstraintTypeRotation:
        return 4;
    case TransformConstraintTypeFullTransform:
        return 7;
    }
}

TransformConstraint::TransformConstraintType TransformConstraint::getType() const
{
    return m_type;
}
bool TransformConstraint::hasPositionConstraint() const
{
    return m_type == TransformConstraintTypePosition
           || m_type == TransformConstraintTypeFullTransform;
}
bool TransformConstraint::hasRotationConstraint() const
{
    return m_type == TransformConstraintTypeRotation
           || m_type == TransformConstraintTypeFullTransform;
}

const iDynTree::Position& TransformConstraint::getPosition() const
{
    return m_transform.getPosition();
}
void TransformConstraint::setPosition(const iDynTree::Position& newPos)
{
    m_transform.setPosition(newPos);
}
const iDynTree::Rotation& TransformConstraint::getRotation() const
{
    return m_transform.getRotation();
}
void TransformConstraint::setRotation(const iDynTree::Rotation& newRot)
{
    m_transform.setRotation(newRot);
}
const iDynTree::Transform& TransformConstraint::getTransform() const
{
    return m_transform;
}
const std::string& TransformConstraint::getFrameName() const
{
    return m_frameName;
}
const double TransformConstraint::getPositionWeight() const
{
    return m_posWeight;
}
void TransformConstraint::setPositionWeight(const double newPosWeight)
{
    if (newPosWeight >= 0.0)
        m_posWeight = newPosWeight;
}
const double TransformConstraint::getRotationWeight() const
{
    return m_rotWeight;
}
void TransformConstraint::setRotationWeight(const double newRotWeight)
{
    if (newRotWeight >= 0.0)
        m_rotWeight = newRotWeight;
}

void TransformConstraint::setTargetResolutionMode(
    iDynTree::InverseKinematicsTreatTargetAsConstraint mode)
{
    m_resolutionMode = mode;
}
iDynTree::InverseKinematicsTreatTargetAsConstraint TransformConstraint::targetResolutionMode() const
{
    return m_resolutionMode;
}

void TransformConstraint::setActive(const bool isActive)
{
    m_isActive = isActive;
}

bool TransformConstraint::isActive() const
{
    return m_isActive;
}

} // namespace kinematics
} // namespace internal
