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

#include <iDynTree/ConvexHullHelpers.h>

#include <iDynTree/Axis.h>
#include <iDynTree/EigenHelpers.h>

#include <Eigen/Core>

#include <algorithm>

namespace iDynTree
{
Polygon::Polygon()
    : m_vertices(0)
{
}

size_t Polygon::getNrOfVertices() const
{
    return m_vertices.size();
}

void Polygon::setNrOfVertices(size_t size)
{
    m_vertices.resize(size);
    return;
}

bool Polygon::isValid() const
{
    return (m_vertices.size() >= 3);
}

Polygon Polygon::applyTransform(const Transform& newFrame_X_oldFrame) const
{
    Polygon ret;

    assert(this->isValid());

    ret.setNrOfVertices(this->getNrOfVertices());

    for (size_t i = 0; i < this->getNrOfVertices(); i++)
    {
        ret(i) = newFrame_X_oldFrame * this->operator()(i);
    }

    return ret;
}

Position& Polygon::operator()(const size_t idx)
{
    return m_vertices[idx];
}

const Position& Polygon::operator()(const size_t idx) const
{
    return m_vertices[idx];
}

Polygon Polygon::XYRectangleFromOffsets(const double front,
                                        const double back,
                                        const double left,
                                        const double right)
{
    Polygon ret;
    ret.m_vertices.push_back(iDynTree::Position(front, left, 0.0));
    ret.m_vertices.push_back(iDynTree::Position(-back, left, 0.0));
    ret.m_vertices.push_back(iDynTree::Position(-back, -right, 0.0));
    ret.m_vertices.push_back(iDynTree::Position(front, -right, 0.0));

    return ret;
}

Polygon2D::Polygon2D()
    : m_vertices(0)
{
}

bool Polygon2D::isValid() const
{
    return (m_vertices.size() >= 3);
}

size_t Polygon2D::getNrOfVertices() const
{
    return m_vertices.size();
}

void Polygon2D::setNrOfVertices(size_t size)
{
    m_vertices.resize(size);
    return;
}

Vector2& Polygon2D::operator()(const size_t idx)
{
    return m_vertices[idx];
}

const Vector2& Polygon2D::operator()(const size_t idx) const
{
    return m_vertices[idx];
}

double monotono_chain_cross(Vector2 o, Vector2 a, Vector2 b)
{
    return ((a(0) - o(0)) * (b(1) - o(1)) - (a(1) - o(1)) * (b(0) - o(0)));
}

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

bool ConvexHullProjectionConstraint::isActive()
{
    return m_isActive;
}

size_t ConvexHullProjectionConstraint::getNrOfConstraints()
{
    return A.rows();
}

bool ConvexHullProjectionConstraint::buildConvexHull(
    const iDynTree::Direction xAxisOfPlaneInWorld,
    const iDynTree::Direction yAxisOfPlaneInWorld,
    const iDynTree::Position originOfPlaneInWorld,
    const std::vector<Polygon>& supportPolygonsExpressedInSupportFrame,
    const std::vector<Transform>& absoluteFrame_X_supportFrame)
{
    o = originOfPlaneInWorld;
    toEigen(P).block<1, 3>(0, 0) = toEigen(xAxisOfPlaneInWorld);
    toEigen(P).block<1, 3>(1, 0) = toEigen(yAxisOfPlaneInWorld);

    // Transform the polygons in the absolute frame
    std::vector<Polygon> supportPolygonsExpressedInAbsoluteFrame;

    supportPolygonsExpressedInAbsoluteFrame.resize(supportPolygonsExpressedInSupportFrame.size());

    for (size_t i = 0; i < supportPolygonsExpressedInSupportFrame.size(); i++)
    {
        supportPolygonsExpressedInAbsoluteFrame[i]
            = supportPolygonsExpressedInSupportFrame[i].applyTransform(
                absoluteFrame_X_supportFrame[i]);
    }

    // Project the polygons on the projection plane
    std::vector<Vector2> projectedPoints;

    for (size_t poly = 0; poly < supportPolygonsExpressedInAbsoluteFrame.size(); poly++)
    {
        for (size_t vertex = 0;
             vertex < supportPolygonsExpressedInAbsoluteFrame[poly].getNrOfVertices();
             vertex++)
        {
            Vector2 newProjectedPoint
                = project(supportPolygonsExpressedInAbsoluteFrame[poly](vertex));
            // toEigen(newProjectedPoint) =
            // toEigen(P)*toEigen(supportPolygonsExpressedInAbsoluteFrame[poly](vertex)-o);
            projectedPoints.push_back(newProjectedPoint);
        }
    }

    if (projectedPoints.size() <= 2)
    {
        return false;
    }

    // Compute the convex hull using the Monotone Chain

    // Order the projected points with a lexographical ordering
    struct
    {
        bool operator()(Vector2 a, Vector2 b)
        {
            return (a(0) < b(0)) || ((a(0) == b(0)) && (a(1) < b(1)));
        }
    } vector2lexographical;

    std::sort(projectedPoints.begin(), projectedPoints.end(), vector2lexographical);

    // Reset the convex hull (it will be properly resized after running the algorithm
    projectedConvexHull.m_vertices.resize(2 * projectedPoints.size());
    size_t k = 0;

    // Build the upper hull
    for (int i = 0; i < projectedPoints.size(); ++i)
    {
        while (k >= 2
               && monotono_chain_cross(projectedConvexHull.m_vertices[k - 2],
                                       projectedConvexHull.m_vertices[k - 1],
                                       projectedPoints[i])
                      <= 0)
        {
            k = k - 1;
        }

        projectedConvexHull.m_vertices[k] = projectedPoints[i];
        k = k + 1;
    }

    // Build the lower hull
    for (int i = projectedPoints.size() - 2, t = k + 1; i >= 0; i--)
    {
        while (k >= t
               && monotono_chain_cross(projectedConvexHull.m_vertices[k - 2],
                                       projectedConvexHull.m_vertices[k - 1],
                                       projectedPoints[i])
                      <= 0)
        {
            k = k - 1;
        }

        projectedConvexHull.m_vertices[k] = projectedPoints[i];
        k = k + 1;
    }

    projectedConvexHull.m_vertices.resize(k - 1);

    // Build the constraint matrix
    buildConstraintMatrix();

    // Update AtimesP matrix
    AtimesP.resize(A.rows(), P.cols());

    toEigen(AtimesP) = toEigen(A) * toEigen(P);

    return true;
}

void ConvexHullProjectionConstraint::buildConstraintMatrix()
{
    // The rows of the A matrix and of the b vector depends on the number of vertices in the convex
    // hull
    A.resize(projectedConvexHull.getNrOfVertices(), 2);
    b.resize(projectedConvexHull.getNrOfVertices());

    // We assume that the vertices in projectedConvexHull are expressed in counter-clockwise order:
    // this is ensured by the structure of the Monotone Chain algorithm

    // Iterate on all line segments one by one
    for (size_t i = 0; i < projectedConvexHull.getNrOfVertices(); i++)
    {
        Vector2 p0 = projectedConvexHull(i);
        Vector2 p1;

        if (i != projectedConvexHull.getNrOfVertices() - 1)
        {
            p1 = projectedConvexHull(i + 1);
        } else
        {
            // if we are using the last vertix, we build the line connecting it with the first
            // vertex
            p1 = projectedConvexHull(0);
        }

        toEigen(A).block<1, 2>(i, 0) = Eigen::Vector2d(p1(1) - p0(1), p0(0) - p1(0));
        b(i) = p0(0) * p1(1) - p1(0) * p0(1);
    }

    return;
}

Vector2 ConvexHullProjectionConstraint::project(Position& pos3dInAbsoluteFrame)
{
    iDynTree::Vector2 projected;
    toEigen(projected) = toEigen(P) * toEigen(pos3dInAbsoluteFrame - o);
    return projected;
}

double distanceBetweentTwoPoints(const Vector2& firstPoint, const Vector2& secondPoint)
{
    return (toEigen(firstPoint) - toEigen(secondPoint)).norm();
}

double distanceBetweenPointAndSegment(const Vector2& point,
                                      const Vector2& segmentFirstPoint,
                                      const Vector2& segmentLastPoint)
{
    double segmentLengthSquared
        = (toEigen(segmentLastPoint) - toEigen(segmentFirstPoint)).squaredNorm();

    // First case: the segment is actually a point
    if (segmentLengthSquared == 0.0)
    {
        return distanceBetweentTwoPoints(point, segmentFirstPoint);
    }

    // The segment can be written as the set of segment points sp that can be written as:
    // sp = segmentFirstPoint + t*( segmentLastPoint - segmentFirstPoint )
    // for t \in [0, 1]
    // To find the segment point closest to the point, we can just find the t of the projection
    // of the point in the segment, and clamp it to [0, 1]
    double tProjection = ((toEigen(point) - toEigen(segmentFirstPoint))
                              .dot(toEigen(segmentLastPoint) - toEigen(segmentFirstPoint)))
                         / segmentLengthSquared;

    double tClosestPoint = std::max(0.0, std::min(1.0, tProjection));

    Vector2 closestPoint;
    toEigen(closestPoint)
        = toEigen(segmentFirstPoint)
          + tClosestPoint * (toEigen(segmentLastPoint) - toEigen(segmentFirstPoint));

    return distanceBetweentTwoPoints(point, closestPoint);
}

double ConvexHullProjectionConstraint::computeMargin(const Vector2& posIn2D)
{
    bool isInside = true;

    // First check if the point is inside the convex hull or not
    iDynTree::VectorDynSize Ax(A.rows());

    toEigen(Ax) = toEigen(A) * toEigen(posIn2D);

    // If even one of the constraint is violated, then the point is outside the convex hull
    for (int i = 0; i < Ax.size(); i++)
    {
        if (!(Ax(i) <= b(i)))
        {
            isInside = false;
            break;
        }
    }

    // Then, compute the distance between each segment of the convex hull and the point :
    // the minimum one is the distance of the point from the convex hull

    // We start from the last segment that do not follow the segment[i],segment[i+1] structure
    double distanceWithoutSign
        = distanceBetweenPointAndSegment(posIn2D,
                                         projectedConvexHull(projectedConvexHull.getNrOfVertices()
                                                             - 1),
                                         projectedConvexHull(0));
    // Find the minimum distance
    for (int i = 0; i < projectedConvexHull.getNrOfVertices() - 1; i++)
    {
        double candidateDistance = distanceBetweenPointAndSegment(posIn2D,
                                                                  projectedConvexHull(i),
                                                                  projectedConvexHull(i + 1));

        if (candidateDistance < distanceWithoutSign)
        {
            distanceWithoutSign = candidateDistance;
        }
    }

    double margin;
    // If the point is inside the convex hull, we return the distance, otherwise the negated
    // distance
    if (isInside)
    {
        margin = distanceWithoutSign;
    } else
    {
        margin = -distanceWithoutSign;
    }

    return margin;
}

void ConvexHullProjectionConstraint::setProjectionAlongDirection(const Vector3& direction)
{
    Vector3 xProjection, yProjection;

    // define the projection for the x-component
    xProjection.setVal(0, 1.0);
    xProjection.setVal(1, 0.0);
    xProjection.setVal(2, -direction.getVal(0) / direction.getVal(2));

    // define the projection for the y-component
    yProjection.setVal(0, 0.0);
    yProjection.setVal(1, 1.0);
    yProjection.setVal(2, -direction.getVal(1) / direction.getVal(2));

    // fill the projection matrix
    toEigen(Pdirection).block<1, 3>(0, 0) = toEigen(xProjection);
    toEigen(Pdirection).block<1, 3>(1, 0) = toEigen(yProjection);
}

Vector2 ConvexHullProjectionConstraint::projectAlongDirection(
    const iDynTree::Position& posIn3dInAbsoluteFrame)
{
    iDynTree::Vector2 projected;
    toEigen(projected) = toEigen(Pdirection) * toEigen(posIn3dInAbsoluteFrame - o);
    return projected;
}

} // namespace iDynTree
