// SPDX-License-Identifier: LGPL-2.1-or-later
/****************************************************************************
 *                                                                          *
 *   Copyright (c) 2023 Ondsel <development@ondsel.com>                     *
 *                                                                          *
 *   This file is part of FreeCAD.                                          *
 *                                                                          *
 *   FreeCAD 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 2.1 of the   *
 *   License, or (at your option) any later version.                        *
 *                                                                          *
 *   FreeCAD 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 FreeCAD. If not, see                                *
 *   <https://www.gnu.org/licenses/>.                                       *
 *                                                                          *
 ***************************************************************************/

#include <BRepAdaptor_Curve.hxx>
#include <BRepAdaptor_Surface.hxx>
#include <TopExp_Explorer.hxx>
#include <TopoDS.hxx>
#include <TopoDS_Face.hxx>
#include <gp_Circ.hxx>
#include <gp_Cylinder.hxx>
#include <gp_Sphere.hxx>


#include <App/Application.h>
#include <App/Datums.h>
#include <App/Document.h>
#include <App/DocumentObject.h>
#include <App/PropertyStandard.h>
#include <App/Link.h>

#include <Base/Placement.h>
#include <Base/Tools.h>
#include <Base/Interpreter.h>

#include <Mod/Part/App/DatumFeature.h>
#include <Mod/Part/App/PartFeature.h>
#include <Mod/PartDesign/App/Body.h>

#include "AssemblyUtils.h"
#include "AssemblyObject.h"
#include "AssemblyLink.h"

#include "JointGroup.h"


namespace PartApp = Part;

FC_LOG_LEVEL_INIT("Assembly", true, true, true)

// ======================================= Utils ======================================
namespace Assembly
{

const char* distanceTypeName(DistanceType dt)
{
    switch (dt) {
        case DistanceType::PointPoint:       return "PointPoint";
        case DistanceType::LineLine:         return "LineLine";
        case DistanceType::LineCircle:       return "LineCircle";
        case DistanceType::CircleCircle:     return "CircleCircle";
        case DistanceType::PlanePlane:       return "PlanePlane";
        case DistanceType::PlaneCylinder:    return "PlaneCylinder";
        case DistanceType::PlaneSphere:      return "PlaneSphere";
        case DistanceType::PlaneCone:        return "PlaneCone";
        case DistanceType::PlaneTorus:       return "PlaneTorus";
        case DistanceType::CylinderCylinder: return "CylinderCylinder";
        case DistanceType::CylinderSphere:   return "CylinderSphere";
        case DistanceType::CylinderCone:     return "CylinderCone";
        case DistanceType::CylinderTorus:    return "CylinderTorus";
        case DistanceType::ConeCone:         return "ConeCone";
        case DistanceType::ConeTorus:        return "ConeTorus";
        case DistanceType::ConeSphere:       return "ConeSphere";
        case DistanceType::TorusTorus:       return "TorusTorus";
        case DistanceType::TorusSphere:      return "TorusSphere";
        case DistanceType::SphereSphere:     return "SphereSphere";
        case DistanceType::PointPlane:       return "PointPlane";
        case DistanceType::PointCylinder:    return "PointCylinder";
        case DistanceType::PointSphere:      return "PointSphere";
        case DistanceType::PointCone:        return "PointCone";
        case DistanceType::PointTorus:       return "PointTorus";
        case DistanceType::LinePlane:        return "LinePlane";
        case DistanceType::LineCylinder:     return "LineCylinder";
        case DistanceType::LineSphere:       return "LineSphere";
        case DistanceType::LineCone:         return "LineCone";
        case DistanceType::LineTorus:        return "LineTorus";
        case DistanceType::CurvePlane:       return "CurvePlane";
        case DistanceType::CurveCylinder:    return "CurveCylinder";
        case DistanceType::CurveSphere:      return "CurveSphere";
        case DistanceType::CurveCone:        return "CurveCone";
        case DistanceType::CurveTorus:       return "CurveTorus";
        case DistanceType::PointLine:        return "PointLine";
        case DistanceType::PointCurve:       return "PointCurve";
        case DistanceType::Other:            return "Other";
    }
    return "Unknown";
}

void swapJCS(const App::DocumentObject* joint)
{
    if (!joint) {
        return;
    }

    auto pPlc1 = joint->getPropertyByName<App::PropertyPlacement>("Placement1");
    auto pPlc2 = joint->getPropertyByName<App::PropertyPlacement>("Placement2");
    if (pPlc1 && pPlc2) {
        const auto temp = pPlc1->getValue();
        pPlc1->setValue(pPlc2->getValue());
        pPlc2->setValue(temp);
    }
    auto pRef1 = joint->getPropertyByName<App::PropertyXLinkSub>("Reference1");
    auto pRef2 = joint->getPropertyByName<App::PropertyXLinkSub>("Reference2");
    if (pRef1 && pRef2) {
        auto temp = pRef1->getValue();
        auto subs1 = pRef1->getSubValues();
        auto subs2 = pRef2->getSubValues();
        pRef1->setValue(pRef2->getValue());
        pRef1->setSubValues(std::move(subs2));
        pRef2->setValue(temp);
        pRef2->setSubValues(std::move(subs1));
    }
}

bool isEdgeType(const App::DocumentObject* obj, const std::string& elName, const GeomAbs_CurveType type)
{
    auto* base = dynamic_cast<const PartApp::Feature*>(obj);
    if (!base) {
        return false;
    }

    const auto& TopShape = base->Shape.getShape();

    // Check for valid face types
    const auto edge = TopoDS::Edge(TopShape.getSubShape(elName.c_str()));
    BRepAdaptor_Curve sf(edge);

    return sf.GetType() == type;
}

bool isFaceType(const App::DocumentObject* obj, const std::string& elName, const GeomAbs_SurfaceType type)
{
    auto* base = dynamic_cast<const PartApp::Feature*>(obj);
    if (!base) {
        return false;
    }

    const auto TopShape = base->Shape.getShape();

    // Check for valid face types
    const auto face = TopoDS::Face(TopShape.getSubShape(elName.c_str()));
    BRepAdaptor_Surface sf(face);

    return sf.GetType() == type;
}

double getFaceRadius(const App::DocumentObject* obj, const std::string& elt)
{
    auto* base = dynamic_cast<const PartApp::Feature*>(obj);
    if (!base) {
        return 0.0;
    }

    const PartApp::TopoShape& TopShape = base->Shape.getShape();

    // Check for valid face types
    TopoDS_Face face = TopoDS::Face(TopShape.getSubShape(elt.c_str()));
    BRepAdaptor_Surface sf(face);

    const auto type = sf.GetType();
    return type == GeomAbs_Cylinder ? sf.Cylinder().Radius()
        : type == GeomAbs_Sphere    ? sf.Sphere().Radius()
                                    : 0.0;
}

double getEdgeRadius(const App::DocumentObject* obj, const std::string& elt)
{
    auto* base = dynamic_cast<const PartApp::Feature*>(obj);
    if (!base) {
        return 0.0;
    }

    const auto& TopShape = base->Shape.getShape();

    // Check for valid face types
    const auto edge = TopoDS::Edge(TopShape.getSubShape(elt.c_str()));
    BRepAdaptor_Curve sf(edge);

    return sf.GetType() == GeomAbs_Circle ? sf.Circle().Radius() : 0.0;
}

/// Determine whether \a obj represents a planar datum when referenced with an
/// empty element type (bare sub-name ending with ".").
///
/// Covers three independent class hierarchies:
///   1. App::Plane  (origin planes, Part::DatumPlane)
///   2. Part::Datum (PartDesign::Plane — not derived from App::Plane)
///   3. Any Part::Feature whose whole-object shape is a single planar face
///      (e.g. Part::Plane primitive referenced without an element)
static bool isDatumPlane(const App::DocumentObject* obj)
{
    if (!obj) {
        return false;
    }

    // Origin planes and Part::DatumPlane (both inherit App::Plane).
    if (obj->isDerivedFrom<App::Plane>()) {
        return true;
    }

    // PartDesign datum objects inherit Part::Datum but NOT App::Plane.
    // Part::Datum is also the base for PartDesign::Line and PartDesign::Point,
    // so inspect the shape to confirm it is actually planar.
    if (obj->isDerivedFrom<PartApp::Datum>()) {
        auto* feat = static_cast<const PartApp::Feature*>(obj);
        const auto& shape = feat->Shape.getShape().getShape();
        if (shape.IsNull()) {
            return false;
        }
        TopExp_Explorer ex(shape, TopAbs_FACE);
        if (ex.More()) {
            BRepAdaptor_Surface sf(TopoDS::Face(ex.Current()));
            return sf.GetType() == GeomAbs_Plane;
        }
        return false;
    }

    // Fallback for any Part::Feature (e.g. Part::Plane primitive) referenced
    // bare — if its shape is a single planar face, treat it as a datum plane.
    if (auto* feat = dynamic_cast<const PartApp::Feature*>(obj)) {
        const auto& shape = feat->Shape.getShape().getShape();
        if (shape.IsNull()) {
            return false;
        }
        TopExp_Explorer ex(shape, TopAbs_FACE);
        if (!ex.More()) {
            return false;
        }
        BRepAdaptor_Surface sf(TopoDS::Face(ex.Current()));
        if (sf.GetType() != GeomAbs_Plane) {
            return false;
        }
        ex.Next();
        // Only treat as datum if there is exactly one face — a multi-face
        // solid referenced bare is ambiguous and should not be classified.
        return !ex.More();
    }

    return false;
}

/// Same idea for datum lines (App::Line, PartDesign::Line, etc.).
static bool isDatumLine(const App::DocumentObject* obj)
{
    if (!obj) {
        return false;
    }

    if (obj->isDerivedFrom<App::Line>()) {
        return true;
    }

    if (obj->isDerivedFrom<PartApp::Datum>()) {
        auto* feat = static_cast<const PartApp::Feature*>(obj);
        const auto& shape = feat->Shape.getShape().getShape();
        if (shape.IsNull()) {
            return false;
        }
        TopExp_Explorer ex(shape, TopAbs_EDGE);
        if (ex.More()) {
            BRepAdaptor_Curve cv(TopoDS::Edge(ex.Current()));
            return cv.GetType() == GeomAbs_Line;
        }
        return false;
    }

    return false;
}

/// Same idea for datum points (App::Point, PartDesign::Point, etc.).
static bool isDatumPoint(const App::DocumentObject* obj)
{
    if (!obj) {
        return false;
    }

    if (obj->isDerivedFrom<App::Point>()) {
        return true;
    }

    if (obj->isDerivedFrom<PartApp::Datum>()) {
        auto* feat = static_cast<const PartApp::Feature*>(obj);
        const auto& shape = feat->Shape.getShape().getShape();
        if (shape.IsNull()) {
            return false;
        }
        // A datum point has a vertex but no edges or faces.
        TopExp_Explorer exE(shape, TopAbs_EDGE);
        TopExp_Explorer exV(shape, TopAbs_VERTEX);
        return !exE.More() && exV.More();
    }

    return false;
}

DistanceType getDistanceType(App::DocumentObject* joint)
{
    if (!joint) {
        return DistanceType::Other;
    }

    const auto type1 = getElementTypeFromProp(joint, "Reference1");
    const auto type2 = getElementTypeFromProp(joint, "Reference2");
    auto elt1 = getElementFromProp(joint, "Reference1");
    auto elt2 = getElementFromProp(joint, "Reference2");
    auto* obj1 = getLinkedObjFromRef(joint, "Reference1");
    auto* obj2 = getLinkedObjFromRef(joint, "Reference2");

    // Datum objects referenced bare have empty element types (sub-name
    // ends with ".").  PartDesign datums referenced through a body can
    // also produce non-standard element types like "Plane" (from a
    // sub-name such as "Body.DatumPlane.Plane" — Part::Datum::getSubObject
    // ignores the trailing element, but splitSubName still extracts it).
    //
    // Detect these before the main geometry chain, which only handles
    // the standard Face/Edge/Vertex element types.
    //
    // isDatumPlane/Line/Point cover all three independent hierarchies:
    //   - App::Plane / App::Line / App::Point  (origin datums)
    //   - Part::Datum subclasses               (PartDesign datums)
    //   - Part::Feature with single-face shape  (Part::Plane primitive, bare ref)
    auto isNonGeomElement = [](const std::string& t) {
        return t != "Face" && t != "Edge" && t != "Vertex";
    };
    const bool datumPlane1 = isNonGeomElement(type1) && isDatumPlane(obj1);
    const bool datumPlane2 = isNonGeomElement(type2) && isDatumPlane(obj2);
    const bool datumLine1  = isNonGeomElement(type1) && !datumPlane1 && isDatumLine(obj1);
    const bool datumLine2  = isNonGeomElement(type2) && !datumPlane2 && isDatumLine(obj2);
    const bool datumPoint1 = isNonGeomElement(type1) && !datumPlane1 && !datumLine1 && isDatumPoint(obj1);
    const bool datumPoint2 = isNonGeomElement(type2) && !datumPlane2 && !datumLine2 && isDatumPoint(obj2);
    const bool datum1 = datumPlane1 || datumLine1 || datumPoint1;
    const bool datum2 = datumPlane2 || datumLine2 || datumPoint2;

    if (datum1 || datum2) {
        // Map each datum side to a synthetic element type so the same
        // classification logic applies regardless of which hierarchy
        // the object comes from.
        auto syntheticType = [](bool isPlane, bool isLine, bool isPoint,
                                const std::string& elemType) -> std::string {
            if (isPlane)  return "Face";
            if (isLine)   return "Edge";
            if (isPoint)  return "Vertex";
            return elemType;  // non-datum side keeps its real type
        };

        const std::string syn1 = syntheticType(datumPlane1, datumLine1, datumPoint1, type1);
        const std::string syn2 = syntheticType(datumPlane2, datumLine2, datumPoint2, type2);

        // Both sides are datum planes.
        if (datumPlane1 && datumPlane2) {
            FC_LOG("Assembly : getDistanceType('" << joint->getFullName()
                   << "') — datum+datum → PlanePlane");
            return DistanceType::PlanePlane;
        }

        // One side is a datum plane, the other has a real element type
        // (or is another datum kind).
        // For PointPlane/LinePlane, the solver's PointInPlaneConstraint
        // reads the plane normal from marker_j (Reference2).  Unlike
        // real Face+Vertex joints (where both Placements carry the
        // face normal from findPlacement), datum planes only carry
        // their normal through computeMarkerTransform.  So the datum
        // plane must end up on Reference2 for the normal to reach marker_j.
        //
        // For PlanePlane the convention matches the existing Face+Face
        // path (plane on Reference1).
        if (datumPlane1 || datumPlane2) {
            const auto& otherSyn = datumPlane1 ? syn2 : syn1;

            if (otherSyn == "Vertex" || otherSyn == "Edge") {
                // Datum plane must be on Reference2 (j side).
                if (datumPlane1) {
                    swapJCS(joint);  // move datum from Ref1 → Ref2
                }
                DistanceType result = (otherSyn == "Vertex")
                    ? DistanceType::PointPlane : DistanceType::LinePlane;
                FC_LOG("Assembly : getDistanceType('" << joint->getFullName()
                       << "') — datum+" << otherSyn << " → "
                       << distanceTypeName(result)
                       << (datumPlane1 ? " (swapped)" : ""));
                return result;
            }

            // Face + datum plane or datum plane + datum plane → PlanePlane.
            // No swap needed: PlanarConstraint is symmetric (uses both
            // z_i and z_j), and preserving the original Reference order
            // keeps the initial Placement values consistent so the solver
            // stays in the correct orientation branch.
            FC_LOG("Assembly : getDistanceType('" << joint->getFullName()
                   << "') — datum+" << otherSyn << " → PlanePlane");
            return DistanceType::PlanePlane;
        }

        // Datum line or datum point paired with a real element type.
        // Map to the appropriate pair using synthetic types and fall
        // through to the main geometry chain below.  The synthetic
        // types ("Edge", "Vertex") will match the existing if-else
        // branches — but those branches call isEdgeType/isFaceType on
        // the object, which requires a real sub-element name.  For
        // datum lines/points the element is empty, so we classify
        // directly here.
        if (datumLine1 && datumLine2) {
            FC_LOG("Assembly : getDistanceType('" << joint->getFullName()
                   << "') — datumLine+datumLine → LineLine");
            return DistanceType::LineLine;
        }
        if (datumPoint1 && datumPoint2) {
            FC_LOG("Assembly : getDistanceType('" << joint->getFullName()
                   << "') — datumPoint+datumPoint → PointPoint");
            return DistanceType::PointPoint;
        }
        if ((datumLine1 && datumPoint2) || (datumPoint1 && datumLine2)) {
            if (datumPoint1) {
                swapJCS(joint);  // line first
            }
            FC_LOG("Assembly : getDistanceType('" << joint->getFullName()
                   << "') — datumLine+datumPoint → PointLine");
            return DistanceType::PointLine;
        }

        // One datum line/point + one real element type.
        if (datumLine1 || datumLine2) {
            const auto& otherSyn = datumLine1 ? syn2 : syn1;
            if (otherSyn == "Face") {
                // Line + Face — need line on Reference2 (edge side).
                if (datumLine1) {
                    swapJCS(joint);
                }
                // We don't know the face type without inspecting the shape,
                // but LinePlane is the most common and safest classification.
                FC_LOG("Assembly : getDistanceType('" << joint->getFullName()
                       << "') — datumLine+Face → LinePlane");
                return DistanceType::LinePlane;
            }
            if (otherSyn == "Vertex") {
                if (datumLine2) {
                    swapJCS(joint);  // line first
                }
                FC_LOG("Assembly : getDistanceType('" << joint->getFullName()
                       << "') — datumLine+Vertex → PointLine");
                return DistanceType::PointLine;
            }
            if (otherSyn == "Edge") {
                FC_LOG("Assembly : getDistanceType('" << joint->getFullName()
                       << "') — datumLine+Edge → LineLine");
                return DistanceType::LineLine;
            }
        }
        if (datumPoint1 || datumPoint2) {
            const auto& otherSyn = datumPoint1 ? syn2 : syn1;
            if (otherSyn == "Face") {
                // Point + Face — face first, point second.
                if (!datumPoint2) {
                    swapJCS(joint);  // put face on Ref1
                }
                FC_LOG("Assembly : getDistanceType('" << joint->getFullName()
                       << "') — datumPoint+Face → PointPlane");
                return DistanceType::PointPlane;
            }
            if (otherSyn == "Edge") {
                // Edge first, point second.
                if (datumPoint1) {
                    swapJCS(joint);
                }
                FC_LOG("Assembly : getDistanceType('" << joint->getFullName()
                       << "') — datumPoint+Edge → PointLine");
                return DistanceType::PointLine;
            }
            if (otherSyn == "Vertex") {
                FC_LOG("Assembly : getDistanceType('" << joint->getFullName()
                       << "') — datumPoint+Vertex → PointPoint");
                return DistanceType::PointPoint;
            }
        }

        // If we get here, it's an unrecognized datum combination.
        FC_WARN("Assembly : getDistanceType('" << joint->getFullName()
                << "') — unrecognized datum combination (syn1="
                << syn1 << ", syn2=" << syn2 << ")");
    }

    if (type1 == "Vertex" && type2 == "Vertex") {
        return DistanceType::PointPoint;
    }
    else if (type1 == "Edge" && type2 == "Edge") {
        if (isEdgeType(obj1, elt1, GeomAbs_Line) || isEdgeType(obj2, elt2, GeomAbs_Line)) {
            if (!isEdgeType(obj1, elt1, GeomAbs_Line)) {
                swapJCS(joint);  // make sure that line is first if not 2 lines.
                std::swap(elt1, elt2);
                std::swap(obj1, obj2);
            }

            if (isEdgeType(obj2, elt2, GeomAbs_Line)) {
                return DistanceType::LineLine;
            }
            else if (isEdgeType(obj2, elt2, GeomAbs_Circle)) {
                return DistanceType::LineCircle;
            }
            // TODO : other cases Ellipse, parabola, hyperbola...
        }

        else if (isEdgeType(obj1, elt1, GeomAbs_Circle) || isEdgeType(obj2, elt2, GeomAbs_Circle)) {
            if (!isEdgeType(obj1, elt1, GeomAbs_Circle)) {
                swapJCS(joint);  // make sure that circle is first if not 2 lines.
                std::swap(elt1, elt2);
                std::swap(obj1, obj2);
            }

            if (isEdgeType(obj2, elt2, GeomAbs_Circle)) {
                return DistanceType::CircleCircle;
            }
            // TODO : other cases Ellipse, parabola, hyperbola...
        }
    }
    else if (type1 == "Face" && type2 == "Face") {
        if (isFaceType(obj1, elt1, GeomAbs_Plane) || isFaceType(obj2, elt2, GeomAbs_Plane)) {
            if (!isFaceType(obj1, elt1, GeomAbs_Plane)) {
                swapJCS(joint);  // make sure plane is first if its not 2 planes.
                std::swap(elt1, elt2);
                std::swap(obj1, obj2);
            }

            if (isFaceType(obj2, elt2, GeomAbs_Plane)) {
                return DistanceType::PlanePlane;
            }
            else if (isFaceType(obj2, elt2, GeomAbs_Cylinder)) {
                return DistanceType::PlaneCylinder;
            }
            else if (isFaceType(obj2, elt2, GeomAbs_Sphere)) {
                return DistanceType::PlaneSphere;
            }
            else if (isFaceType(obj2, elt2, GeomAbs_Cone)) {
                return DistanceType::PlaneCone;
            }
            else if (isFaceType(obj2, elt2, GeomAbs_Torus)) {
                return DistanceType::PlaneTorus;
            }
        }

        else if (isFaceType(obj1, elt1, GeomAbs_Cylinder)
                 || isFaceType(obj2, elt2, GeomAbs_Cylinder)) {
            if (!isFaceType(obj1, elt1, GeomAbs_Cylinder)) {
                swapJCS(joint);  // make sure cylinder is first if its not 2 cylinders.
                std::swap(elt1, elt2);
                std::swap(obj1, obj2);
            }

            if (isFaceType(obj2, elt2, GeomAbs_Cylinder)) {
                return DistanceType::CylinderCylinder;
            }
            else if (isFaceType(obj2, elt2, GeomAbs_Sphere)) {
                return DistanceType::CylinderSphere;
            }
            else if (isFaceType(obj2, elt2, GeomAbs_Cone)) {
                return DistanceType::CylinderCone;
            }
            else if (isFaceType(obj2, elt2, GeomAbs_Torus)) {
                return DistanceType::CylinderTorus;
            }
        }

        else if (isFaceType(obj1, elt1, GeomAbs_Cone) || isFaceType(obj2, elt2, GeomAbs_Cone)) {
            if (!isFaceType(obj1, elt1, GeomAbs_Cone)) {
                swapJCS(joint);  // make sure cone is first if its not 2 cones.
                std::swap(elt1, elt2);
                std::swap(obj1, obj2);
            }

            if (isFaceType(obj2, elt2, GeomAbs_Cone)) {
                return DistanceType::ConeCone;
            }
            else if (isFaceType(obj2, elt2, GeomAbs_Torus)) {
                return DistanceType::ConeTorus;
            }
            else if (isFaceType(obj2, elt2, GeomAbs_Sphere)) {
                return DistanceType::ConeSphere;
            }
        }

        else if (isFaceType(obj1, elt1, GeomAbs_Torus) || isFaceType(obj2, elt2, GeomAbs_Torus)) {
            if (!isFaceType(obj1, elt1, GeomAbs_Torus)) {
                swapJCS(joint);  // make sure torus is first if its not 2 torus.
                std::swap(elt1, elt2);
                std::swap(obj1, obj2);
            }

            if (isFaceType(obj2, elt2, GeomAbs_Torus)) {
                return DistanceType::TorusTorus;
            }
            else if (isFaceType(obj2, elt2, GeomAbs_Sphere)) {
                return DistanceType::TorusSphere;
            }
        }

        else if (isFaceType(obj1, elt1, GeomAbs_Sphere) || isFaceType(obj2, elt2, GeomAbs_Sphere)) {
            if (!isFaceType(obj1, elt1, GeomAbs_Sphere)) {
                swapJCS(joint);  // make sure sphere is first if its not 2 spheres.
                std::swap(elt1, elt2);
                std::swap(obj1, obj2);
            }

            if (isFaceType(obj2, elt2, GeomAbs_Sphere)) {
                return DistanceType::SphereSphere;
            }
        }
    }
    else if ((type1 == "Vertex" && type2 == "Face") || (type1 == "Face" && type2 == "Vertex")) {
        if (type1 == "Vertex") {  // Make sure face is the first.
            swapJCS(joint);
            std::swap(elt1, elt2);
            std::swap(obj1, obj2);
        }
        if (isFaceType(obj1, elt1, GeomAbs_Plane)) {
            return DistanceType::PointPlane;
        }
        else if (isFaceType(obj1, elt1, GeomAbs_Cylinder)) {
            return DistanceType::PointCylinder;
        }
        else if (isFaceType(obj1, elt1, GeomAbs_Sphere)) {
            return DistanceType::PointSphere;
        }
        else if (isFaceType(obj1, elt1, GeomAbs_Cone)) {
            return DistanceType::PointCone;
        }
        else if (isFaceType(obj1, elt1, GeomAbs_Torus)) {
            return DistanceType::PointTorus;
        }
    }
    else if ((type1 == "Edge" && type2 == "Face") || (type1 == "Face" && type2 == "Edge")) {
        if (type1 == "Edge") {  // Make sure face is the first.
            swapJCS(joint);
            std::swap(elt1, elt2);
            std::swap(obj1, obj2);
        }
        if (isEdgeType(obj2, elt2, GeomAbs_Line)) {
            if (isFaceType(obj1, elt1, GeomAbs_Plane)) {
                return DistanceType::LinePlane;
            }
            else if (isFaceType(obj1, elt1, GeomAbs_Cylinder)) {
                return DistanceType::LineCylinder;
            }
            else if (isFaceType(obj1, elt1, GeomAbs_Sphere)) {
                return DistanceType::LineSphere;
            }
            else if (isFaceType(obj1, elt1, GeomAbs_Cone)) {
                return DistanceType::LineCone;
            }
            else if (isFaceType(obj1, elt1, GeomAbs_Torus)) {
                return DistanceType::LineTorus;
            }
        }
        else {
            // For other curves we consider them as planes for now. Can be refined later.
            if (isFaceType(obj1, elt1, GeomAbs_Plane)) {
                return DistanceType::CurvePlane;
            }
            else if (isFaceType(obj1, elt1, GeomAbs_Cylinder)) {
                return DistanceType::CurveCylinder;
            }
            else if (isFaceType(obj1, elt1, GeomAbs_Sphere)) {
                return DistanceType::CurveSphere;
            }
            else if (isFaceType(obj1, elt1, GeomAbs_Cone)) {
                return DistanceType::CurveCone;
            }
            else if (isFaceType(obj1, elt1, GeomAbs_Torus)) {
                return DistanceType::CurveTorus;
            }
        }
    }
    else if ((type1 == "Vertex" && type2 == "Edge") || (type1 == "Edge" && type2 == "Vertex")) {
        if (type1 == "Vertex") {  // Make sure edge is the first.
            swapJCS(joint);
            std::swap(elt1, elt2);
            std::swap(obj1, obj2);
        }
        if (isEdgeType(obj1, elt1, GeomAbs_Line)) {  // Point on line joint.
            return DistanceType::PointLine;
        }
        else {
            // For other curves we do a point in plane-of-the-curve.
            // Maybe it would be best tangent / distance to the conic? For arcs and
            // circles we could use ASMTRevSphJoint. But is it better than pointInPlane?
            return DistanceType::PointCurve;
        }
    }
    return DistanceType::Other;
}

JointGroup* getJointGroup(const App::Part* part)
{
    if (!part) {
        return nullptr;
    }

    const auto* doc = part->getDocument();

    const auto jointGroups = doc->getObjectsOfType(JointGroup::getClassTypeId());
    if (jointGroups.empty()) {
        return nullptr;
    }
    for (auto jointGroup : jointGroups) {
        if (part->hasObject(jointGroup)) {
            return freecad_cast<JointGroup*>(jointGroup);
        }
    }
    return nullptr;
}

void setJointActivated(const App::DocumentObject* joint, bool val)
{
    if (!joint) {
        return;
    }

    if (auto propSuppressed = joint->getPropertyByName<App::PropertyBool>("Suppressed")) {
        propSuppressed->setValue(!val);
    }
}

bool getJointActivated(const App::DocumentObject* joint)
{
    if (!joint) {
        return false;
    }

    if (const auto propActivated = joint->getPropertyByName<App::PropertyBool>("Suppressed")) {
        return !propActivated->getValue();
    }
    return false;
}

double getJointDistance(const App::DocumentObject* joint, const char* propertyName)
{
    if (!joint) {
        return 0.0;
    }

    const auto* prop = joint->getPropertyByName<App::PropertyFloat>(propertyName);
    if (!prop) {
        return 0.0;
    }

    return prop->getValue();
}

double getJointAngle(const App::DocumentObject* joint)
{
    return getJointDistance(joint, "Angle");
}

double getJointDistance(const App::DocumentObject* joint)
{
    return getJointDistance(joint, "Distance");
}

double getJointDistance2(const App::DocumentObject* joint)
{
    return getJointDistance(joint, "Distance2");
}

JointType getJointType(const App::DocumentObject* joint)
{
    if (!joint) {
        return JointType::Fixed;
    }

    const auto* prop = joint->getPropertyByName<App::PropertyEnumeration>("JointType");
    if (!prop) {
        return JointType::Fixed;
    }

    return static_cast<JointType>(prop->getValue());
}

std::vector<std::string> getSubAsList(const App::PropertyXLinkSub* prop)
{
    if (!prop) {
        return {};
    }

    const auto subs = prop->getSubValues();
    if (subs.empty()) {
        return {};
    }

    return Base::Tools::splitSubName(subs[0]);
}

std::vector<std::string> getSubAsList(const App::DocumentObject* obj, const char* pName)
{
    if (!obj) {
        return {};
    }
    return getSubAsList(obj->getPropertyByName<App::PropertyXLinkSub>(pName));
}

std::string getElementFromProp(const App::DocumentObject* obj, const char* pName)
{
    if (!obj) {
        return "";
    }

    const auto names = getSubAsList(obj, pName);
    if (names.empty()) {
        return "";
    }

    return names.back();
}

std::string getElementTypeFromProp(const App::DocumentObject* obj, const char* propName)
{
    // The prop is going to be something like 'Edge14' or 'Face7'. We need 'Edge' or 'Face'
    std::string elementType;
    for (const char ch : getElementFromProp(obj, propName)) {
        if (std::isalpha(ch)) {
            elementType += ch;
        }
    }
    return elementType;
}

App::DocumentObject* getObjFromProp(const App::DocumentObject* joint, const char* pName)
{
    if (!joint) {
        return {};
    }

    const auto* propObj = joint->getPropertyByName<App::PropertyLink>(pName);
    if (!propObj) {
        return {};
    }

    return propObj->getValue();
}

App::DocumentObject* getObjFromRef(App::DocumentObject* comp, const std::string& sub)
{
    if (!comp) {
        return nullptr;
    }

    const auto* doc = comp->getDocument();
    auto names = Base::Tools::splitSubName(sub);
    names.insert(names.begin(), comp->getNameInDocument());

    if (names.size() <= 2) {
        return comp;
    }

    // Lambda function to check if the typeId is a BodySubObject
    const auto isBodySubObject = [](App::DocumentObject* obj) -> bool {
        // PartDesign::Point + Line + Plane + CoordinateSystem
        // getViewProviderName instead of isDerivedFrom to avoid dependency on sketcher
        const auto isDerivedFromVpSketch
            = strcmp(obj->getViewProviderName(), "SketcherGui::ViewProviderSketch") == 0;
        return isDerivedFromVpSketch || obj->isDerivedFrom<PartApp::Datum>()
            || obj->isDerivedFrom<App::DatumElement>()
            || obj->isDerivedFrom<App::LocalCoordinateSystem>();
    };

    // Helper function to handle PartDesign::Body objects
    const auto handlePartDesignBody =
        [&](App::DocumentObject* obj,
            std::vector<std::string>::const_iterator it) -> App::DocumentObject* {
        auto nextIt = std::next(it);
        if (nextIt != names.end()) {
            for (auto* obji : obj->getOutList()) {
                if (*nextIt == obji->getNameInDocument() && isBodySubObject(obji)) {
                    // if obji is a LCS then perhaps we need to resolve one more level
                    if (auto* lcs = freecad_cast<App::LocalCoordinateSystem*>(obji)) {
                        nextIt = std::next(nextIt);
                        if (nextIt != names.end()) {
                            for (auto* objj : lcs->baseObjects()) {
                                if (*nextIt == objj->getNameInDocument()
                                    && objj->isDerivedFrom<App::DatumElement>()) {
                                    return objj;
                                }
                            }
                        }
                    }
                    return obji;
                }
            }
        }
        return obj;
    };


    for (auto it = names.begin(); it != names.end(); ++it) {
        App::DocumentObject* obj = doc->getObject(it->c_str());
        if (!obj) {
            return nullptr;
        }

        if (obj->isDerivedFrom<App::DocumentObjectGroup>()) {
            continue;
        }

        // The last but one name should be the selected
        if (std::next(it) == std::prev(names.end())) {
            return obj;
        }

        if (obj->isDerivedFrom<App::Part>() || obj->isLinkGroup()) {
            continue;
        }
        else if (obj->isDerivedFrom<App::LocalCoordinateSystem>()) {
            // Resolve LCS → child datum element (e.g. Origin → XY_Plane)
            auto nextIt = std::next(it);
            if (nextIt != names.end()) {
                for (auto* child : obj->getOutList()) {
                    if (child->getNameInDocument() == *nextIt
                        && child->isDerivedFrom<App::DatumElement>()) {
                        return child;
                    }
                }
            }
            return obj;
        }
        else if (obj->isDerivedFrom<PartDesign::Body>()) {
            return handlePartDesignBody(obj, it);
        }
        else if (obj->isDerivedFrom<PartApp::Feature>()) {
            // Primitive, fastener, gear, etc.
            return obj;
        }
        else if (obj->isLink()) {
            App::DocumentObject* linked_obj = obj->getLinkedObject();
            if (linked_obj->isDerivedFrom<PartDesign::Body>()) {
                auto* retObj = handlePartDesignBody(linked_obj, it);
                return retObj == linked_obj ? obj : retObj;
            }
            else if (linked_obj->isDerivedFrom<PartApp::Feature>()) {
                return obj;
            }
            else {
                doc = linked_obj->getDocument();
                continue;
            }
        }
    }

    return nullptr;
}

App::DocumentObject* getObjFromRef(const App::PropertyXLinkSub* prop)
{
    if (!prop) {
        return nullptr;
    }

    App::DocumentObject* obj = prop->getValue();
    if (!obj) {
        return nullptr;
    }

    const std::vector<std::string> subs = prop->getSubValues();
    if (subs.empty()) {
        return nullptr;
    }

    return getObjFromRef(obj, subs[0]);
}

App::DocumentObject* getObjFromJointRef(const App::DocumentObject* joint, const char* pName)
{
    if (!joint) {
        return nullptr;
    }

    const auto* prop = joint->getPropertyByName<App::PropertyXLinkSub>(pName);
    return getObjFromRef(prop);
}

App::DocumentObject* getLinkedObjFromRef(const App::DocumentObject* joint, const char* pObj)
{
    if (!joint) {
        return nullptr;
    }

    if (const auto* obj = getObjFromJointRef(joint, pObj)) {
        return obj->getLinkedObject(true);
    }
    return nullptr;
}

App::DocumentObject* getMovingPartFromSel(
    const AssemblyObject* assemblyObject,
    App::DocumentObject* obj,
    const std::string& sub
)
{
    if (!obj) {
        return nullptr;
    }

    auto* doc = obj->getDocument();

    auto names = Base::Tools::splitSubName(sub);
    names.insert(names.begin(), obj->getNameInDocument());

    bool assemblyPassed = false;

    for (const auto& objName : names) {
        obj = doc->getObject(objName.c_str());
        if (!obj) {
            continue;
        }

        if (obj->isLink()) {  // update the document if necessary for next object
            doc = obj->getLinkedObject()->getDocument();
        }

        if (obj == assemblyObject) {
            // We make sure we pass the assembly for cases like part.assembly.part.body
            assemblyPassed = true;
            continue;
        }
        if (!assemblyPassed) {
            continue;
        }

        if (obj->isDerivedFrom<App::DocumentObjectGroup>()) {
            continue;  // we ignore groups.
        }

        if (obj->isLinkGroup()) {
            continue;
        }

        // We ignore dynamic sub-assemblies.
        if (obj->isDerivedFrom<Assembly::AssemblyLink>()) {
            const auto* pRigid = obj->getPropertyByName<App::PropertyBool>("Rigid");
            if (pRigid && !pRigid->getValue()) {
                continue;
            }
        }

        return obj;
    }
    return nullptr;
}

App::DocumentObject* getMovingPartFromRef(App::PropertyXLinkSub* prop)
{
    if (!prop) {
        return nullptr;
    }

    return prop->getValue();
}

App::DocumentObject* getMovingPartFromRef(App::DocumentObject* joint, const char* pName)
{
    if (!joint) {
        return nullptr;
    }

    auto* prop = joint->getPropertyByName<App::PropertyXLinkSub>(pName);
    return getMovingPartFromRef(prop);
}

void syncPlacements(App::DocumentObject* src, App::DocumentObject* to)
{
    auto* plcPropSource = dynamic_cast<App::PropertyPlacement*>(src->getPropertyByName("Placement"));
    auto* plcPropLink = dynamic_cast<App::PropertyPlacement*>(to->getPropertyByName("Placement"));

    if (plcPropSource && plcPropLink) {
        if (!plcPropSource->getValue().isSame(plcPropLink->getValue())) {
            plcPropLink->setValue(plcPropSource->getValue());
        }
    }
}
namespace
{
// Helper function to perform the recursive traversal. Kept in an anonymous
// namespace as it's an implementation detail of getAssemblyComponents.
void collectComponentsRecursively(
    const std::vector<App::DocumentObject*>& objects,
    std::vector<App::DocumentObject*>& results
)
{
    for (auto* obj : objects) {
        if (!obj) {
            continue;
        }

        if (auto* asmLink = freecad_cast<Assembly::AssemblyLink*>(obj)) {
            // If the sub-assembly is rigid, treat it as a single movable part.
            // If it's flexible, we need to check its individual components.
            if (asmLink->isRigid()) {
                results.push_back(asmLink);
            }
            else {
                collectComponentsRecursively(asmLink->Group.getValues(), results);
            }
            continue;
        }
        else if (obj->isLinkGroup()) {
            auto* linkGroup = static_cast<App::Link*>(obj);
            for (auto* elt : linkGroup->ElementList.getValues()) {
                results.push_back(elt);
            }
            continue;
        }
        else if (auto* group = freecad_cast<App::DocumentObjectGroup*>(obj)) {
            collectComponentsRecursively(group->Group.getValues(), results);
            continue;
        }
        else if (auto* link = freecad_cast<App::Link*>(obj)) {
            obj = link->getLinkedObject();
            if (obj->isDerivedFrom<App::GeoFeature>()
                && !obj->isDerivedFrom<App::LocalCoordinateSystem>()) {
                results.push_back(link);
            }
        }

        else if (obj->isDerivedFrom<App::GeoFeature>()
                 && !obj->isDerivedFrom<App::LocalCoordinateSystem>()) {
            results.push_back(obj);
        }
    }
}
}  // namespace

std::vector<App::DocumentObject*> getAssemblyComponents(const AssemblyObject* assembly)
{
    if (!assembly) {
        return {};
    }

    std::vector<App::DocumentObject*> components;
    collectComponentsRecursively(assembly->Group.getValues(), components);
    return components;
}

}  // namespace Assembly