// SPDX-License-Identifier: LGPL-2.1-or-later

/***************************************************************************
 *   Copyright (c) 2021 Abdullah Tahiri <abdullah.tahiri.yo@gmail.com>     *
 *                                                                         *
 *   This file is part of the FreeCAD CAx development system.              *
 *                                                                         *
 *   This library is free software; you can redistribute it and/or         *
 *   modify it under the terms of the GNU Library General Public           *
 *   License as published by the Free Software Foundation; either          *
 *   version 2 of the License, or (at your option) any later version.      *
 *                                                                         *
 *   This library  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 Library General Public License for more details.                  *
 *                                                                         *
 *   You should have received a copy of the GNU Library General Public     *
 *   License along with this library; see the file COPYING.LIB. If not,    *
 *   write to the Free Software Foundation, Inc., 59 Temple Place,         *
 *   Suite 330, Boston, MA  02111-1307, USA                                *
 *                                                                         *
 ***************************************************************************/

#include <FCConfig.h>

#include <QPainter>
#include <QRegularExpression>
#include <limits>
#include <memory>
#include <map>
#include <stack>
#include <cmath>

#include <Inventor/SbImage.h>
#include <Inventor/SbVec3f.h>
#include <Inventor/SoPickedPoint.h>
#include <Inventor/nodes/SoAnnotation.h>
#include <Inventor/nodes/SoDepthBuffer.h>
#include <Inventor/nodes/SoDrawStyle.h>
#include <Inventor/nodes/SoGroup.h>
#include <Inventor/nodes/SoImage.h>
#include <Inventor/nodes/SoInfo.h>
#include <Inventor/nodes/SoMaterial.h>
#include <Inventor/nodes/SoMaterialBinding.h>
#include <Inventor/nodes/SoPickStyle.h>
#include <Inventor/nodes/SoSeparator.h>
#include <Inventor/nodes/SoTranslation.h>

#include <Base/Converter.h>
#include <Base/Exception.h>
#include <Base/Tools.h>
#include <Base/UnitsApi.h>
#include <Base/Vector3D.h>
#include <Gui/BitmapFactory.h>
#include <Gui/Inventor/SmSwitchboard.h>
#include <Gui/SoDatumLabel.h>
#include <Gui/Tools.h>
#include <Gui/Utilities.h>
#include <Mod/Part/App/Geometry.h>
#include <Mod/Sketcher/App/Constraint.h>
#include <Mod/Sketcher/App/GeoEnum.h>
#include <Mod/Sketcher/App/GeoList.h>
#include <Mod/Sketcher/App/GeometryFacade.h>
#include <Mod/Sketcher/App/SolverGeometryExtension.h>

#include "EditModeConstraintCoinManager.h"
#include "SoZoomTranslation.h"
#include "Utils.h"
#include "ViewProviderSketch.h"
#include "ViewProviderSketchCoinAttorney.h"


using namespace Gui;
using namespace SketcherGui;
using namespace Sketcher;

//**************************** EditModeConstraintCoinManager class ******************************

EditModeConstraintCoinManager::EditModeConstraintCoinManager(
    ViewProviderSketch& vp,
    DrawingParameters& drawingParams,
    GeometryLayerParameters& geometryLayerParams,
    ConstraintParameters& constraintParams,
    EditModeScenegraphNodes& editModeScenegraph,
    CoinMapping& coinMap
)
    : viewProvider(vp)
    , drawingParameters(drawingParams)
    , geometryLayerParameters(geometryLayerParams)
    , constraintParameters(constraintParams)
    , editModeScenegraphNodes(editModeScenegraph)
    , coinMapping(coinMap)
{}

EditModeConstraintCoinManager::~EditModeConstraintCoinManager()
{}

void EditModeConstraintCoinManager::updateVirtualSpace()
{
    const std::vector<Sketcher::Constraint*>& constrlist
        = ViewProviderSketchCoinAttorney::getConstraints(viewProvider);

    bool isshownvirtualspace = ViewProviderSketchCoinAttorney::isShownVirtualSpace(viewProvider);

    if (constrlist.size() == vConstrType.size()) {

        editModeScenegraphNodes.constrGroup->enable.setNum(constrlist.size());

        SbBool* sws = editModeScenegraphNodes.constrGroup->enable.startEditing();

        for (size_t i = 0; i < constrlist.size(); i++) {
            sws[i] = !(constrlist[i]->isInVirtualSpace != isshownvirtualspace)
                && constrlist[i]->isVisible;  // XOR of constraint mode and VP mode
        }


        editModeScenegraphNodes.constrGroup->enable.finishEditing();
    }
}

void EditModeConstraintCoinManager::processConstraints(const GeoListFacade& geolistfacade)
{
    using std::numbers::pi;

    const auto& constrlist = ViewProviderSketchCoinAttorney::getConstraints(viewProvider);

    auto zConstrH = ViewProviderSketchCoinAttorney::getViewOrientationFactor(viewProvider)
        * drawingParameters.zConstr;
    // After an undo/redo it can happen that we have an empty geometry list but a non-empty
    // constraint list In this case just ignore the constraints. (See bug #0000421)
    if (geolistfacade.geomlist.size() <= 2 && !constrlist.empty()) {
        rebuildConstraintNodes(geolistfacade);
        return;
    }

    int extGeoCount = geolistfacade.getExternalCount();
    int intGeoCount = geolistfacade.getInternalCount();

    // reset point if the constraint type has changed
Restart:
    // check if a new constraint arrived
    if (constrlist.size() != vConstrType.size()) {
        rebuildConstraintNodes(geolistfacade);
    }

    assert(int(constrlist.size()) == editModeScenegraphNodes.constrGroup->getNumChildren());
    assert(int(vConstrType.size()) == editModeScenegraphNodes.constrGroup->getNumChildren());

    // update the virtual space
    updateVirtualSpace();

    auto getNormal =
        [](const GeoListFacade& geolistfacade, const int geoid, const Base::Vector3d& pointoncurve) {
            auto geom = geolistfacade.getGeometryFromGeoId(geoid);
            auto curve = dynamic_cast<const Part::GeomCurve*>(geom);

            auto line = dynamic_cast<const Part::GeomLineSegment*>(curve);

            if (line) {
                Base::Vector3d linedir = line->getEndPoint() - line->getStartPoint();
                return Base::Vector3d(-linedir.y, linedir.x, 0);
            }
            else {
                Base::Vector3d normal;
                try {
                    if (!(curve && curve->normalAt(pointoncurve, normal))) {
                        normal = Base::Vector3d(1, 0, 0);
                    }
                }
                catch (const Base::CADKernelError&) {
                    normal = Base::Vector3d(1, 0, 0);
                }

                return normal;
            }
        };

    // go through the constraints and update the position
    int i = 0;
    for (std::vector<Sketcher::Constraint*>::const_iterator it = constrlist.begin();
         it != constrlist.end();
         ++it, i++) {
        // check if the type has changed
        if ((*it)->Type != vConstrType[i]) {
            // clearing the type vector will force a rebuild of the visual nodes
            vConstrType.clear();
            // TODO: The 'goto' here is unsafe as it can happen that we cause an endless loop (see
            // bug #0001956).
            goto Restart;
        }
        try {  // because calculateNormalAtPoint, used in there, can throw
            // root separator for this constraint
            SoSeparator* sep = static_cast<SoSeparator*>(
                editModeScenegraphNodes.constrGroup->getChild(i)
            );
            const Constraint* Constr = *it;

            if (Constr->First < -extGeoCount || Constr->First >= intGeoCount
                || (Constr->Second != GeoEnum::GeoUndef
                    && (Constr->Second < -extGeoCount || Constr->Second >= intGeoCount))
                || (Constr->Third != GeoEnum::GeoUndef
                    && (Constr->Third < -extGeoCount || Constr->Third >= intGeoCount))) {
                // Constraint can refer to non-existent geometry during undo/redo
                continue;
            }

            // distinguish different constraint types to build up
            switch (Constr->Type) {
                case Block:
                case Horizontal:  // write the new position of the Horizontal constraint Same as
                                  // vertical position.
                case Vertical:    // write the new position of the Vertical constraint
                {
                    assert(Constr->First >= -extGeoCount && Constr->First < intGeoCount);
                    bool alignment = Constr->Type != Block && Constr->Second != GeoEnum::GeoUndef;

                    // get the geometry
                    const Part::Geometry* geo = geolistfacade.getGeometryFromGeoId(Constr->First);

                    if (!alignment) {
                        // Vertical & Horiz can only be a GeomLineSegment, but Blocked can be
                        // anything.
                        Base::Vector3d midpos;
                        Base::Vector3d dir;
                        Base::Vector3d norm;

                        if (geo->is<Part::GeomLineSegment>()) {
                            const Part::GeomLineSegment* lineSeg
                                = static_cast<const Part::GeomLineSegment*>(geo);

                            // calculate the half distance between the start and endpoint
                            midpos = ((lineSeg->getEndPoint() + lineSeg->getStartPoint()) / 2);

                            // Get a set of vectors perpendicular and tangential to these
                            dir = (lineSeg->getEndPoint() - lineSeg->getStartPoint()).Normalize();

                            norm = Base::Vector3d(-dir.y, dir.x, 0);
                        }
                        else if (geo->is<Part::GeomBSplineCurve>()) {
                            const Part::GeomBSplineCurve* bsp
                                = static_cast<const Part::GeomBSplineCurve*>(geo);
                            midpos = Base::Vector3d(0, 0, 0);

                            std::vector<Base::Vector3d> poles = bsp->getPoles();

                            // Move center of gravity towards start not to collide with bspline
                            // degree information.
                            double ws = 1.0 / poles.size();
                            double w = 1.0;

                            for (std::vector<Base::Vector3d>::iterator it = poles.begin();
                                 it != poles.end();
                                 ++it) {
                                midpos += w * (*it);
                                w -= ws;
                            }

                            midpos /= poles.size();

                            dir = (bsp->getEndPoint() - bsp->getStartPoint()).Normalize();
                            norm = Base::Vector3d(-dir.y, dir.x, 0);
                        }
                        else {
                            double ra = 0, rb = 0;
                            double angle,        // rotation of object as a whole
                                angleplus = 0.;  // arc angle (t parameter for ellipses)

                            if (geo->is<Part::GeomCircle>()) {
                                const Part::GeomCircle* circle
                                    = static_cast<const Part::GeomCircle*>(geo);
                                ra = circle->getRadius();
                                angle = pi / 4;
                                midpos = circle->getCenter();
                            }
                            else if (geo->is<Part::GeomArcOfCircle>()) {
                                const Part::GeomArcOfCircle* arc
                                    = static_cast<const Part::GeomArcOfCircle*>(geo);
                                ra = arc->getRadius();
                                double startangle, endangle;
                                arc->getRange(startangle, endangle, /*emulateCCW=*/true);
                                angle = (startangle + endangle) / 2;
                                midpos = arc->getCenter();
                            }
                            else if (geo->is<Part::GeomEllipse>()) {
                                const Part::GeomEllipse* ellipse
                                    = static_cast<const Part::GeomEllipse*>(geo);
                                ra = ellipse->getMajorRadius();
                                rb = ellipse->getMinorRadius();
                                Base::Vector3d majdir = ellipse->getMajorAxisDir();
                                angle = atan2(majdir.y, majdir.x);
                                angleplus = pi / 4;
                                midpos = ellipse->getCenter();
                            }
                            else if (geo->is<Part::GeomArcOfEllipse>()) {
                                const Part::GeomArcOfEllipse* aoe
                                    = static_cast<const Part::GeomArcOfEllipse*>(geo);
                                ra = aoe->getMajorRadius();
                                rb = aoe->getMinorRadius();
                                double startangle, endangle;
                                aoe->getRange(startangle, endangle, /*emulateCCW=*/true);
                                Base::Vector3d majdir = aoe->getMajorAxisDir();
                                angle = atan2(majdir.y, majdir.x);
                                angleplus = (startangle + endangle) / 2;
                                midpos = aoe->getCenter();
                            }
                            else if (geo->is<Part::GeomArcOfHyperbola>()) {
                                const Part::GeomArcOfHyperbola* aoh
                                    = static_cast<const Part::GeomArcOfHyperbola*>(geo);
                                ra = aoh->getMajorRadius();
                                rb = aoh->getMinorRadius();
                                double startangle, endangle;
                                aoh->getRange(startangle, endangle, /*emulateCCW=*/true);
                                Base::Vector3d majdir = aoh->getMajorAxisDir();
                                angle = atan2(majdir.y, majdir.x);
                                angleplus = (startangle + endangle) / 2;
                                midpos = aoh->getCenter();
                            }
                            else if (geo->is<Part::GeomArcOfParabola>()) {
                                const Part::GeomArcOfParabola* aop
                                    = static_cast<const Part::GeomArcOfParabola*>(geo);
                                ra = aop->getFocal();
                                double startangle, endangle;
                                aop->getRange(startangle, endangle, /*emulateCCW=*/true);
                                Base::Vector3d majdir = -aop->getXAxisDir();
                                angle = atan2(majdir.y, majdir.x);
                                angleplus = (startangle + endangle) / 2;
                                midpos = aop->getFocus();
                            }
                            else {
                                break;
                            }

                            if (geo->is<Part::GeomEllipse>() || geo->is<Part::GeomArcOfEllipse>()
                                || geo->is<Part::GeomArcOfHyperbola>()) {

                                Base::Vector3d majDir, minDir, rvec;
                                majDir = Base::Vector3d(
                                    cos(angle),
                                    sin(angle),
                                    0
                                );  // direction of major axis of ellipse
                                minDir = Base::Vector3d(
                                    -majDir.y,
                                    majDir.x,
                                    0
                                );  // direction of minor axis of ellipse
                                rvec = (ra * cos(angleplus)) * majDir
                                    + (rb * sin(angleplus)) * minDir;
                                midpos += rvec;
                                rvec.Normalize();
                                norm = rvec;
                                dir = Base::Vector3d(
                                    -rvec.y,
                                    rvec.x,
                                    0
                                );  // DeepSOIC: I'm not sure what dir is supposed to mean.
                            }
                            else {
                                norm = Base::Vector3d(cos(angle), sin(angle), 0);
                                dir = Base::Vector3d(-norm.y, norm.x, 0);
                                midpos += ra * norm;
                            }
                        }

                        Base::Vector3d relpos = seekConstraintPosition(
                            midpos,
                            norm,
                            dir,
                            2.5,
                            editModeScenegraphNodes.constrGroup->getChild(i)
                        );

                        auto translation = static_cast<SoZoomTranslation*>(sep->getChild(
                            static_cast<int>(ConstraintNodePosition::FirstTranslationIndex)
                        ));

                        translation->abPos = SbVec3f(midpos.x, midpos.y, zConstrH);  // Absolute
                                                                                     // Reference

                        // Reference Position that is scaled according to zoom
                        translation->translation = SbVec3f(relpos.x, relpos.y, 0);
                    }
                    else {
                        assert(Constr->Second >= -extGeoCount && Constr->Second < intGeoCount);
                        assert(
                            Constr->FirstPos != Sketcher::PointPos::none
                            && Constr->SecondPos != Sketcher::PointPos::none
                        );

                        Base::Vector3d midpos1, dir1, norm1;
                        Base::Vector3d midpos2, dir2, norm2;

                        midpos1 = geolistfacade.getPoint(Constr->First, Constr->FirstPos);
                        midpos2 = geolistfacade.getPoint(Constr->Second, Constr->SecondPos);

                        dir1 = (midpos2 - midpos1).Normalize();
                        dir2 = -dir1;
                        norm1 = Base::Vector3d(-dir1.y, dir1.x, 0.);
                        norm2 = norm1;

                        Base::Vector3d relpos1 = seekConstraintPosition(
                            midpos1,
                            norm1,
                            dir1,
                            4.0,
                            editModeScenegraphNodes.constrGroup->getChild(i)
                        );

                        auto translation = static_cast<SoZoomTranslation*>(sep->getChild(
                            static_cast<int>(ConstraintNodePosition::FirstTranslationIndex)
                        ));

                        translation->abPos = SbVec3f(midpos1.x, midpos1.y, zConstrH);
                        translation->translation = SbVec3f(relpos1.x, relpos1.y, 0);

                        Base::Vector3d relpos2 = seekConstraintPosition(
                            midpos2,
                            norm2,
                            dir2,
                            4.0,
                            editModeScenegraphNodes.constrGroup->getChild(i)
                        );

                        Base::Vector3d secondPos = midpos2 - midpos1;

                        translation = static_cast<SoZoomTranslation*>(sep->getChild(
                            static_cast<int>(ConstraintNodePosition::SecondTranslationIndex)
                        ));

                        translation->abPos = SbVec3f(secondPos.x, secondPos.y, zConstrH);
                        translation->translation
                            = SbVec3f(relpos2.x - relpos1.x, relpos2.y - relpos1.y, 0);
                    }
                } break;
                case Perpendicular: {
                    assert(Constr->First >= -extGeoCount && Constr->First < intGeoCount);
                    assert(Constr->Second >= -extGeoCount && Constr->Second < intGeoCount);
                    // get the geometry
                    const Part::Geometry* geo1 = geolistfacade.getGeometryFromGeoId(Constr->First);
                    const Part::Geometry* geo2 = geolistfacade.getGeometryFromGeoId(Constr->Second);
                    Base::Vector3d midpos1, dir1, norm1;
                    Base::Vector3d midpos2, dir2, norm2;
                    bool twoIcons = false;  // a very local flag. It's set to true to indicate that
                                            // the second dir+norm are valid and should be used

                    if (Constr->Third != GeoEnum::GeoUndef ||            // perpty via point
                        Constr->FirstPos != Sketcher::PointPos::none) {  // endpoint-to-curve or
                                                                         // endpoint-to-endpoint perpty

                        int ptGeoId;
                        Sketcher::PointPos ptPosId;
                        do {  // dummy loop to use break =) Maybe goto?
                            ptGeoId = Constr->First;
                            ptPosId = Constr->FirstPos;
                            if (ptPosId != Sketcher::PointPos::none) {
                                break;
                            }
                            ptGeoId = Constr->Second;
                            ptPosId = Constr->SecondPos;
                            if (ptPosId != Sketcher::PointPos::none) {
                                break;
                            }
                            ptGeoId = Constr->Third;
                            ptPosId = Constr->ThirdPos;
                            if (ptPosId != Sketcher::PointPos::none) {
                                break;
                            }
                            assert(0);  // no point found!
                        } while (false);

                        midpos1 = geolistfacade.getPoint(ptGeoId, ptPosId);

                        norm1 = getNormal(geolistfacade, Constr->Second, midpos1);

                        // TODO: Check the method above. This was the old one making use of the
                        // solver.
                        // norm1 = getSolvedSketch().calculateNormalAtPoint(Constr->Second,
                        // midpos1.x, midpos1.y);

                        norm1.Normalize();
                        dir1 = norm1;
                        dir1.RotateZ(-pi / 2.0);
                    }
                    else if (Constr->FirstPos == Sketcher::PointPos::none) {

                        if (geo1->is<Part::GeomLineSegment>()) {
                            const Part::GeomLineSegment* lineSeg1
                                = static_cast<const Part::GeomLineSegment*>(geo1);
                            midpos1 = ((lineSeg1->getEndPoint() + lineSeg1->getStartPoint()) / 2);
                            dir1 = (lineSeg1->getEndPoint() - lineSeg1->getStartPoint()).Normalize();
                            norm1 = Base::Vector3d(-dir1.y, dir1.x, 0.);
                        }
                        else if (geo1->is<Part::GeomArcOfCircle>()) {
                            const Part::GeomArcOfCircle* arc
                                = static_cast<const Part::GeomArcOfCircle*>(geo1);
                            double startangle, endangle, midangle;
                            arc->getRange(startangle, endangle, /*emulateCCW=*/true);
                            midangle = (startangle + endangle) / 2;
                            norm1 = Base::Vector3d(cos(midangle), sin(midangle), 0);
                            dir1 = Base::Vector3d(-norm1.y, norm1.x, 0);
                            midpos1 = arc->getCenter() + arc->getRadius() * norm1;
                        }
                        else if (geo1->is<Part::GeomCircle>()) {
                            const Part::GeomCircle* circle = static_cast<const Part::GeomCircle*>(geo1);
                            norm1 = Base::Vector3d(cos(pi / 4), sin(pi / 4), 0);
                            dir1 = Base::Vector3d(-norm1.y, norm1.x, 0);
                            midpos1 = circle->getCenter() + circle->getRadius() * norm1;
                        }
                        else {
                            break;
                        }

                        if (geo2->is<Part::GeomLineSegment>()) {
                            const Part::GeomLineSegment* lineSeg2
                                = static_cast<const Part::GeomLineSegment*>(geo2);
                            midpos2 = ((lineSeg2->getEndPoint() + lineSeg2->getStartPoint()) / 2);
                            dir2 = (lineSeg2->getEndPoint() - lineSeg2->getStartPoint()).Normalize();
                            norm2 = Base::Vector3d(-dir2.y, dir2.x, 0.);
                        }
                        else if (geo2->is<Part::GeomArcOfCircle>()) {
                            const Part::GeomArcOfCircle* arc
                                = static_cast<const Part::GeomArcOfCircle*>(geo2);
                            double startangle, endangle, midangle;
                            arc->getRange(startangle, endangle, /*emulateCCW=*/true);
                            midangle = (startangle + endangle) / 2;
                            norm2 = Base::Vector3d(cos(midangle), sin(midangle), 0);
                            dir2 = Base::Vector3d(-norm2.y, norm2.x, 0);
                            midpos2 = arc->getCenter() + arc->getRadius() * norm2;
                        }
                        else if (geo2->is<Part::GeomCircle>()) {
                            const Part::GeomCircle* circle = static_cast<const Part::GeomCircle*>(geo2);
                            norm2 = Base::Vector3d(cos(pi / 4), sin(pi / 4), 0);
                            dir2 = Base::Vector3d(-norm2.y, norm2.x, 0);
                            midpos2 = circle->getCenter() + circle->getRadius() * norm2;
                        }
                        else {
                            break;
                        }
                        twoIcons = true;
                    }

                    Base::Vector3d relpos1 = seekConstraintPosition(
                        midpos1,
                        norm1,
                        dir1,
                        4.0,
                        editModeScenegraphNodes.constrGroup->getChild(i)
                    );

                    auto translation = static_cast<SoZoomTranslation*>(
                        sep->getChild(static_cast<int>(ConstraintNodePosition::FirstTranslationIndex))
                    );

                    translation->abPos = SbVec3f(midpos1.x, midpos1.y, zConstrH);
                    translation->translation = SbVec3f(relpos1.x, relpos1.y, 0);

                    if (twoIcons) {
                        Base::Vector3d relpos2 = seekConstraintPosition(
                            midpos2,
                            norm2,
                            dir2,
                            4.0,
                            editModeScenegraphNodes.constrGroup->getChild(i)
                        );

                        Base::Vector3d secondPos = midpos2 - midpos1;
                        auto translation = static_cast<SoZoomTranslation*>(sep->getChild(
                            static_cast<int>(ConstraintNodePosition::SecondTranslationIndex)
                        ));
                        translation->abPos = SbVec3f(secondPos.x, secondPos.y, zConstrH);
                        translation->translation
                            = SbVec3f(relpos2.x - relpos1.x, relpos2.y - relpos1.y, 0);
                    }

                } break;
                case Parallel:
                case Equal: {
                    assert(Constr->First >= -extGeoCount && Constr->First < intGeoCount);
                    assert(Constr->Second >= -extGeoCount && Constr->Second < intGeoCount);
                    // get the geometry
                    const Part::Geometry* geo1 = geolistfacade.getGeometryFromGeoId(Constr->First);
                    const Part::Geometry* geo2 = geolistfacade.getGeometryFromGeoId(Constr->Second);

                    Base::Vector3d midpos1, dir1, norm1;
                    Base::Vector3d midpos2, dir2, norm2;
                    if (!geo1->is<Part::GeomLineSegment>() || !geo2->is<Part::GeomLineSegment>()) {
                        if (Constr->Type == Equal) {
                            double r1a = 0, r1b = 0, r2a = 0, r2b = 0;
                            double angle1, angle1plus = 0., angle2,
                                           angle2plus = 0.;  // angle1 = rotation of object as a
                                                             // whole; angle1plus = arc angle (t
                                                             // parameter for ellipses).
                            if (geo1->is<Part::GeomCircle>()) {
                                const Part::GeomCircle* circle
                                    = static_cast<const Part::GeomCircle*>(geo1);
                                r1a = circle->getRadius();
                                angle1 = pi / 4;
                                midpos1 = circle->getCenter();
                            }
                            else if (geo1->is<Part::GeomArcOfCircle>()) {
                                const Part::GeomArcOfCircle* arc
                                    = static_cast<const Part::GeomArcOfCircle*>(geo1);
                                r1a = arc->getRadius();
                                double startangle, endangle;
                                arc->getRange(startangle, endangle, /*emulateCCW=*/true);
                                angle1 = (startangle + endangle) / 2;
                                midpos1 = arc->getCenter();
                            }
                            else if (geo1->is<Part::GeomEllipse>()) {
                                const Part::GeomEllipse* ellipse
                                    = static_cast<const Part::GeomEllipse*>(geo1);
                                r1a = ellipse->getMajorRadius();
                                r1b = ellipse->getMinorRadius();
                                Base::Vector3d majdir = ellipse->getMajorAxisDir();
                                angle1 = atan2(majdir.y, majdir.x);
                                angle1plus = pi / 4;
                                midpos1 = ellipse->getCenter();
                            }
                            else if (geo1->is<Part::GeomArcOfEllipse>()) {
                                const Part::GeomArcOfEllipse* aoe
                                    = static_cast<const Part::GeomArcOfEllipse*>(geo1);
                                r1a = aoe->getMajorRadius();
                                r1b = aoe->getMinorRadius();
                                double startangle, endangle;
                                aoe->getRange(startangle, endangle, /*emulateCCW=*/true);
                                Base::Vector3d majdir = aoe->getMajorAxisDir();
                                angle1 = atan2(majdir.y, majdir.x);
                                angle1plus = (startangle + endangle) / 2;
                                midpos1 = aoe->getCenter();
                            }
                            else if (geo1->is<Part::GeomArcOfHyperbola>()) {
                                const Part::GeomArcOfHyperbola* aoh
                                    = static_cast<const Part::GeomArcOfHyperbola*>(geo1);
                                r1a = aoh->getMajorRadius();
                                r1b = aoh->getMinorRadius();
                                double startangle, endangle;
                                aoh->getRange(startangle, endangle, /*emulateCCW=*/true);
                                Base::Vector3d majdir = aoh->getMajorAxisDir();
                                angle1 = atan2(majdir.y, majdir.x);
                                angle1plus = (startangle + endangle) / 2;
                                midpos1 = aoh->getCenter();
                            }
                            else if (geo1->is<Part::GeomArcOfParabola>()) {
                                const Part::GeomArcOfParabola* aop
                                    = static_cast<const Part::GeomArcOfParabola*>(geo1);
                                r1a = aop->getFocal();
                                double startangle, endangle;
                                aop->getRange(startangle, endangle, /*emulateCCW=*/true);
                                Base::Vector3d majdir = -aop->getXAxisDir();
                                angle1 = atan2(majdir.y, majdir.x);
                                angle1plus = (startangle + endangle) / 2;
                                midpos1 = aop->getFocus();
                            }
                            else {
                                break;
                            }

                            if (geo2->is<Part::GeomCircle>()) {
                                const Part::GeomCircle* circle
                                    = static_cast<const Part::GeomCircle*>(geo2);
                                r2a = circle->getRadius();
                                angle2 = pi / 4;
                                midpos2 = circle->getCenter();
                            }
                            else if (geo2->is<Part::GeomArcOfCircle>()) {
                                const Part::GeomArcOfCircle* arc
                                    = static_cast<const Part::GeomArcOfCircle*>(geo2);
                                r2a = arc->getRadius();
                                double startangle, endangle;
                                arc->getRange(startangle, endangle, /*emulateCCW=*/true);
                                angle2 = (startangle + endangle) / 2;
                                midpos2 = arc->getCenter();
                            }
                            else if (geo2->is<Part::GeomEllipse>()) {
                                const Part::GeomEllipse* ellipse
                                    = static_cast<const Part::GeomEllipse*>(geo2);
                                r2a = ellipse->getMajorRadius();
                                r2b = ellipse->getMinorRadius();
                                Base::Vector3d majdir = ellipse->getMajorAxisDir();
                                angle2 = atan2(majdir.y, majdir.x);
                                angle2plus = pi / 4;
                                midpos2 = ellipse->getCenter();
                            }
                            else if (geo2->is<Part::GeomArcOfEllipse>()) {
                                const Part::GeomArcOfEllipse* aoe
                                    = static_cast<const Part::GeomArcOfEllipse*>(geo2);
                                r2a = aoe->getMajorRadius();
                                r2b = aoe->getMinorRadius();
                                double startangle, endangle;
                                aoe->getRange(startangle, endangle, /*emulateCCW=*/true);
                                Base::Vector3d majdir = aoe->getMajorAxisDir();
                                angle2 = atan2(majdir.y, majdir.x);
                                angle2plus = (startangle + endangle) / 2;
                                midpos2 = aoe->getCenter();
                            }
                            else if (geo2->is<Part::GeomArcOfHyperbola>()) {
                                const Part::GeomArcOfHyperbola* aoh
                                    = static_cast<const Part::GeomArcOfHyperbola*>(geo2);
                                r2a = aoh->getMajorRadius();
                                r2b = aoh->getMinorRadius();
                                double startangle, endangle;
                                aoh->getRange(startangle, endangle, /*emulateCCW=*/true);
                                Base::Vector3d majdir = aoh->getMajorAxisDir();
                                angle2 = atan2(majdir.y, majdir.x);
                                angle2plus = (startangle + endangle) / 2;
                                midpos2 = aoh->getCenter();
                            }
                            else if (geo2->is<Part::GeomArcOfParabola>()) {
                                const Part::GeomArcOfParabola* aop
                                    = static_cast<const Part::GeomArcOfParabola*>(geo2);
                                r2a = aop->getFocal();
                                double startangle, endangle;
                                aop->getRange(startangle, endangle, /*emulateCCW=*/true);
                                Base::Vector3d majdir = -aop->getXAxisDir();
                                angle2 = atan2(majdir.y, majdir.x);
                                angle2plus = (startangle + endangle) / 2;
                                midpos2 = aop->getFocus();
                            }
                            else {
                                break;
                            }

                            if (geo1->is<Part::GeomEllipse>() || geo1->is<Part::GeomArcOfEllipse>()
                                || geo1->is<Part::GeomArcOfHyperbola>()) {

                                Base::Vector3d majDir, minDir, rvec;
                                majDir = Base::Vector3d(
                                    cos(angle1),
                                    sin(angle1),
                                    0
                                );  // direction of major axis of ellipse
                                minDir = Base::Vector3d(
                                    -majDir.y,
                                    majDir.x,
                                    0
                                );  // direction of minor axis of ellipse
                                rvec = (r1a * cos(angle1plus)) * majDir
                                    + (r1b * sin(angle1plus)) * minDir;
                                midpos1 += rvec;
                                rvec.Normalize();
                                norm1 = rvec;
                                dir1 = Base::Vector3d(
                                    -rvec.y,
                                    rvec.x,
                                    0
                                );  // DeepSOIC: I'm not sure what dir is supposed to mean.
                            }
                            else {
                                norm1 = Base::Vector3d(cos(angle1), sin(angle1), 0);
                                dir1 = Base::Vector3d(-norm1.y, norm1.x, 0);
                                midpos1 += r1a * norm1;
                            }


                            if (geo2->is<Part::GeomEllipse>() || geo2->is<Part::GeomArcOfEllipse>()
                                || geo2->is<Part::GeomArcOfHyperbola>()) {

                                Base::Vector3d majDir, minDir, rvec;
                                majDir = Base::Vector3d(
                                    cos(angle2),
                                    sin(angle2),
                                    0
                                );  // direction of major axis of ellipse
                                minDir = Base::Vector3d(
                                    -majDir.y,
                                    majDir.x,
                                    0
                                );  // direction of minor axis of ellipse
                                rvec = (r2a * cos(angle2plus)) * majDir
                                    + (r2b * sin(angle2plus)) * minDir;
                                midpos2 += rvec;
                                rvec.Normalize();
                                norm2 = rvec;
                                dir2 = Base::Vector3d(-rvec.y, rvec.x, 0);
                            }
                            else {
                                norm2 = Base::Vector3d(cos(angle2), sin(angle2), 0);
                                dir2 = Base::Vector3d(-norm2.y, norm2.x, 0);
                                midpos2 += r2a * norm2;
                            }
                        }
                        else {  // Parallel can only apply to a GeomLineSegment
                            break;
                        }
                    }
                    else {
                        const Part::GeomLineSegment* lineSeg1
                            = static_cast<const Part::GeomLineSegment*>(geo1);
                        const Part::GeomLineSegment* lineSeg2
                            = static_cast<const Part::GeomLineSegment*>(geo2);

                        // calculate the half distance between the start and endpoint
                        midpos1 = ((lineSeg1->getEndPoint() + lineSeg1->getStartPoint()) / 2);
                        midpos2 = ((lineSeg2->getEndPoint() + lineSeg2->getStartPoint()) / 2);
                        // Get a set of vectors perpendicular and tangential to these
                        dir1 = (lineSeg1->getEndPoint() - lineSeg1->getStartPoint()).Normalize();
                        dir2 = (lineSeg2->getEndPoint() - lineSeg2->getStartPoint()).Normalize();
                        norm1 = Base::Vector3d(-dir1.y, dir1.x, 0.);
                        norm2 = Base::Vector3d(-dir2.y, dir2.x, 0.);
                    }

                    Base::Vector3d relpos1 = seekConstraintPosition(
                        midpos1,
                        norm1,
                        dir1,
                        4.0,
                        editModeScenegraphNodes.constrGroup->getChild(i)
                    );
                    Base::Vector3d relpos2 = seekConstraintPosition(
                        midpos2,
                        norm2,
                        dir2,
                        4.0,
                        editModeScenegraphNodes.constrGroup->getChild(i)
                    );

                    auto translation = static_cast<SoZoomTranslation*>(
                        sep->getChild(static_cast<int>(ConstraintNodePosition::FirstTranslationIndex))
                    );

                    translation->abPos = SbVec3f(midpos1.x, midpos1.y, zConstrH);  // Absolute Reference

                    // Reference Position that is scaled according to zoom
                    translation->translation = SbVec3f(relpos1.x, relpos1.y, 0);

                    Base::Vector3d secondPos = midpos2 - midpos1;

                    translation = static_cast<SoZoomTranslation*>(sep->getChild(
                        static_cast<int>(ConstraintNodePosition::SecondTranslationIndex)
                    ));

                    translation->abPos = SbVec3f(secondPos.x, secondPos.y, zConstrH);  // Absolute
                                                                                       // Reference

                    // Reference Position that is scaled according to zoom
                    translation->translation = SbVec3f(relpos2.x - relpos1.x, relpos2.y - relpos1.y, 0);

                } break;
                case Distance:
                case DistanceX:
                case DistanceY: {
                    assert(Constr->First >= -extGeoCount && Constr->First < intGeoCount);

                    double helperStartAngle1 = 0.;  // for arc helpers
                    double helperStartAngle2 = 0.;
                    double helperRange1 = 0.;
                    double helperRange2 = 0.;
                    double radius1 = 0.;
                    double radius2 = 0.;
                    Base::Vector3d center1(0., 0., 0.);
                    Base::Vector3d center2(0., 0., 0.);

                    int numPoints = 2;

                    // pnt1 will be initialized to (0,0,0) if only one point is given
                    auto pnt1 = geolistfacade.getPoint(Constr->First, Constr->FirstPos);

                    Base::Vector3d pnt2(0., 0., 0.);

                    if (Constr->SecondPos != Sketcher::PointPos::none) {
                        // point to point distance
                        pnt2 = geolistfacade.getPoint(Constr->Second, Constr->SecondPos);
                    }
                    else if (Constr->Second != GeoEnum::GeoUndef) {
                        auto geo1 = geolistfacade.getGeometryFromGeoId(Constr->First);
                        auto geo2 = geolistfacade.getGeometryFromGeoId(Constr->Second);
                        if (isLineSegment(*geo2)) {
                            // NOLINTNEXTLINE
                            auto lineSeg = static_cast<const Part::GeomLineSegment*>(geo2);
                            Base::Vector3d l2p1 = lineSeg->getStartPoint();
                            Base::Vector3d l2p2 = lineSeg->getEndPoint();

                            if (Constr->FirstPos != Sketcher::PointPos::none) {
                                // point to line distance
                                // calculate the projection of p1 onto lineSeg
                                pnt2.ProjectToLine(pnt1 - l2p1, l2p2 - l2p1);
                                pnt2 += pnt1;
                            }
                            else if (isCircleOrArc(*geo1)) {
                                // circular to line distance
                                auto [radius, ct] = getRadiusCenterCircleArc(geo1);
                                // project the center on the line (translated to origin)
                                pnt1.ProjectToLine(ct - l2p1, l2p2 - l2p1);
                                Base::Vector3d dir = pnt1;
                                dir.Normalize();
                                pnt1 += ct;
                                pnt2 = ct + dir * radius;
                            }
                        }
                        else if (isCircleOrArc(*geo2)) {
                            if (Constr->FirstPos != Sketcher::PointPos::none) {
                                // point to circular distance
                                auto [rad, ct] = getRadiusCenterCircleArc(geo2);

                                Base::Vector3d v = pnt1 - ct;
                                v = v.Normalize();
                                pnt2 = ct + rad * v;
                            }
                            else if (isCircleOrArc(*geo1)) {
                                // circular to circular distance
                                GetCirclesMinimalDistance(geo1, geo2, pnt1, pnt2);
                            }
                        }
                        else {
                            break;
                        }
                    }
                    else if (Constr->FirstPos != Sketcher::PointPos::none) {
                        // one point distance
                        pnt1 = Base::Vector3d(0., 0., 0.);
                        pnt2 = geolistfacade.getPoint(Constr->First, Constr->FirstPos);
                    }
                    else if (Constr->First != GeoEnum::GeoUndef) {
                        auto geo = geolistfacade.getGeometryFromGeoId(Constr->First);
                        if (isLineSegment(*geo)) {
                            // segment distance
                            auto lineSeg = static_cast<const Part::GeomLineSegment*>(geo);
                            pnt1 = lineSeg->getStartPoint();
                            pnt2 = lineSeg->getEndPoint();
                        }
                        else if (isArcOfCircle(*geo)) {
                            // arc length
                            auto arc = static_cast<const Part::GeomArcOfCircle*>(geo);
                            int index = static_cast<int>(ConstraintNodePosition::DatumLabelIndex);
                            auto* asciiText = static_cast<SoDatumLabel*>(sep->getChild(index));
                            center1 = arc->getCenter();
                            pnt1 = arc->getStartPoint();
                            pnt2 = arc->getEndPoint();

                            double startAngle, endAngle;
                            arc->getRange(startAngle, endAngle, /*emulateCCW=*/false);

                            asciiText->datumtype = SoDatumLabel::ARCLENGTH;
                            asciiText->param1 = Constr->LabelDistance;
                            asciiText->string = SbString(
                                getPresentationString(Constr, "◠ ").toUtf8().constData()
                            );
                            asciiText->strikethrough = !Constr->isActive;

                            asciiText->pnts.setNum(3);
                            SbVec3f* verts = asciiText->pnts.startEditing();
                            verts[0] = SbVec3f(center1.x, center1.y, center1.z);
                            verts[1] = SbVec3f(pnt1.x, pnt1.y, pnt1.z);
                            verts[2] = SbVec3f(pnt2.x, pnt2.y, pnt2.z);
                            asciiText->pnts.finishEditing();
                            break;
                        }
                        else {
                            break;
                        }
                    }
                    else {
                        break;
                    }

                    int index = static_cast<int>(ConstraintNodePosition::DatumLabelIndex);
                    auto* asciiText = static_cast<SoDatumLabel*>(sep->getChild(index));  // NOLINT

                    // Get presentation string (w/o units if option is set)
                    asciiText->string = SbString(getPresentationString(Constr).toUtf8().constData());
                    asciiText->strikethrough = !Constr->isActive;

                    if (Constr->Type == Distance) {
                        asciiText->datumtype = SoDatumLabel::DISTANCE;
                    }
                    else if (Constr->Type == DistanceX) {
                        asciiText->datumtype = SoDatumLabel::DISTANCEX;
                    }
                    else if (Constr->Type == DistanceY) {
                        asciiText->datumtype = SoDatumLabel::DISTANCEY;
                    }

                    // Check if arc helpers are needed
                    if (Constr->Second != GeoEnum::GeoUndef) {
                        auto geo1 = geolistfacade.getGeometryFromGeoId(Constr->First);
                        auto geo2 = geolistfacade.getGeometryFromGeoId(Constr->Second);

                        if (isArcOfCircle(*geo1) && Constr->FirstPos == Sketcher::PointPos::none) {
                            auto arc = static_cast<const Part::GeomArcOfCircle*>(geo1);  // NOLINT
                            radius1 = arc->getRadius();
                            center1 = arc->getCenter();

                            double angle = toVector2d(
                                               isLineSegment(*geo2) ? pnt2 - center1 : pnt1 - center1
                            )
                                               .Angle();
                            double startAngle, endAngle;
                            arc->getRange(startAngle, endAngle, /*emulateCCW=*/true);

                            findHelperAngles(helperStartAngle1, helperRange1, angle, startAngle, endAngle);

                            if (helperRange1 != 0.) {
                                // We override to draw the full helper as it does not look good
                                // otherwise We still use findHelperAngles before to find if helper
                                // is needed.
                                helperStartAngle1 = endAngle;
                                helperRange1 = 2 * pi - (endAngle - startAngle);

                                numPoints++;
                            }
                        }
                        if (isArcOfCircle(*geo2) && Constr->SecondPos == Sketcher::PointPos::none) {
                            auto arc = static_cast<const Part::GeomArcOfCircle*>(geo2);  // NOLINT
                            radius2 = arc->getRadius();
                            center2 = arc->getCenter();

                            double angle = toVector2d(pnt2 - center2).Angle();  // between -pi and pi
                            double startAngle, endAngle;  // between 0 and 2*pi
                            arc->getRange(startAngle, endAngle, /*emulateCCW=*/true);

                            findHelperAngles(helperStartAngle2, helperRange2, angle, startAngle, endAngle);

                            if (helperRange2 != 0.) {
                                helperStartAngle2 = endAngle;
                                helperRange2 = 2 * pi - (endAngle - startAngle);

                                numPoints++;
                            }
                        }
                    }

                    // Assign the Datum Points
                    asciiText->pnts.setNum(numPoints);
                    SbVec3f* verts = asciiText->pnts.startEditing();

                    verts[0] = SbVec3f(pnt1.x, pnt1.y, zConstrH);
                    verts[1] = SbVec3f(pnt2.x, pnt2.y, zConstrH);

                    if (numPoints > 2) {
                        if (helperRange1 != 0.) {
                            verts[2] = SbVec3f(center1.x, center1.y, zConstrH);
                            asciiText->param3 = helperStartAngle1;
                            asciiText->param4 = helperRange1;
                            asciiText->param5 = radius1;
                        }
                        else {
                            verts[2] = SbVec3f(center2.x, center2.y, zConstrH);
                            asciiText->param3 = helperStartAngle2;
                            asciiText->param4 = helperRange2;
                            asciiText->param5 = radius2;
                        }
                        if (numPoints > 3) {
                            verts[3] = SbVec3f(center2.x, center2.y, zConstrH);
                            asciiText->param6 = helperStartAngle2;
                            asciiText->param7 = helperRange2;
                            asciiText->param8 = radius2;
                        }
                        else {
                            asciiText->param6 = 0.;
                            asciiText->param7 = 0.;
                            asciiText->param8 = 0.;
                        }
                    }
                    else {
                        asciiText->param3 = 0.;
                        asciiText->param4 = 0.;
                        asciiText->param5 = 0.;
                    }

                    asciiText->pnts.finishEditing();

                    // Assign the Label Distance
                    asciiText->param1 = Constr->LabelDistance;
                    asciiText->param2 = Constr->LabelPosition;
                } break;
                case PointOnObject:
                case Tangent:
                case SnellsLaw: {
                    assert(Constr->First >= -extGeoCount && Constr->First < intGeoCount);
                    assert(Constr->Second >= -extGeoCount && Constr->Second < intGeoCount);

                    Base::Vector3d pos, relPos;
                    if (
                        Constr->Type == PointOnObject || Constr->Type == SnellsLaw
                        || (Constr->Type == Tangent && Constr->Third != GeoEnum::GeoUndef)
                        ||  // Tangency via point
                        (Constr->Type == Tangent
                         && Constr->FirstPos != Sketcher::PointPos::none)  // endpoint-to-curve or
                                                                           // endpoint-to-endpoint
                                                                           // tangency
                    ) {

                        // find the point of tangency/point that is on object
                        // just any point among first/second/third should be OK
                        int ptGeoId;
                        Sketcher::PointPos ptPosId;
                        do {  // dummy loop to use break =) Maybe goto?
                            ptGeoId = Constr->First;
                            ptPosId = Constr->FirstPos;
                            if (ptPosId != Sketcher::PointPos::none) {
                                break;
                            }
                            ptGeoId = Constr->Second;
                            ptPosId = Constr->SecondPos;
                            if (ptPosId != Sketcher::PointPos::none) {
                                break;
                            }
                            ptGeoId = Constr->Third;
                            ptPosId = Constr->ThirdPos;
                            if (ptPosId != Sketcher::PointPos::none) {
                                break;
                            }
                            assert(0);  // no point found!
                        } while (false);

                        pos = geolistfacade.getPoint(ptGeoId, ptPosId);
                        auto norm = getNormal(geolistfacade, Constr->Second, pos);

                        // TODO: Check substitution
                        // Base::Vector3d norm =
                        // getSolvedSketch().calculateNormalAtPoint(Constr->Second, pos.x, pos.y);
                        norm.Normalize();
                        Base::Vector3d dir = norm;
                        dir.RotateZ(-pi / 2.0);

                        relPos = seekConstraintPosition(
                            pos,
                            norm,
                            dir,
                            2.5,
                            editModeScenegraphNodes.constrGroup->getChild(i)
                        );

                        auto translation = static_cast<SoZoomTranslation*>(sep->getChild(
                            static_cast<int>(ConstraintNodePosition::FirstTranslationIndex)
                        ));

                        translation->abPos = SbVec3f(pos.x, pos.y, zConstrH);  // Absolute Reference
                        translation->translation = SbVec3f(relPos.x, relPos.y, 0);
                    }
                    else if (Constr->Type == Tangent) {
                        // get the geometry
                        const Part::Geometry* geo1 = geolistfacade.getGeometryFromGeoId(Constr->First);
                        const Part::Geometry* geo2 = geolistfacade.getGeometryFromGeoId(Constr->Second);

                        if (geo1->is<Part::GeomLineSegment>() && geo2->is<Part::GeomLineSegment>()) {
                            const Part::GeomLineSegment* lineSeg1
                                = static_cast<const Part::GeomLineSegment*>(geo1);
                            const Part::GeomLineSegment* lineSeg2
                                = static_cast<const Part::GeomLineSegment*>(geo2);
                            // tangency between two lines
                            Base::Vector3d midpos1
                                = ((lineSeg1->getEndPoint() + lineSeg1->getStartPoint()) / 2);
                            Base::Vector3d midpos2
                                = ((lineSeg2->getEndPoint() + lineSeg2->getStartPoint()) / 2);
                            Base::Vector3d dir1
                                = (lineSeg1->getEndPoint() - lineSeg1->getStartPoint()).Normalize();
                            Base::Vector3d dir2
                                = (lineSeg2->getEndPoint() - lineSeg2->getStartPoint()).Normalize();
                            Base::Vector3d norm1 = Base::Vector3d(-dir1.y, dir1.x, 0.f);
                            Base::Vector3d norm2 = Base::Vector3d(-dir2.y, dir2.x, 0.f);

                            Base::Vector3d relpos1 = seekConstraintPosition(
                                midpos1,
                                norm1,
                                dir1,
                                4.0,
                                editModeScenegraphNodes.constrGroup->getChild(i)
                            );
                            Base::Vector3d relpos2 = seekConstraintPosition(
                                midpos2,
                                norm2,
                                dir2,
                                4.0,
                                editModeScenegraphNodes.constrGroup->getChild(i)
                            );

                            auto translation = static_cast<SoZoomTranslation*>(sep->getChild(
                                static_cast<int>(ConstraintNodePosition::FirstTranslationIndex)
                            ));

                            translation->abPos = SbVec3f(midpos1.x, midpos1.y, zConstrH);  // Absolute
                                                                                           // Reference
                            translation->translation = SbVec3f(relpos1.x, relpos1.y, 0);

                            Base::Vector3d secondPos = midpos2 - midpos1;

                            translation = static_cast<SoZoomTranslation*>(sep->getChild(
                                static_cast<int>(ConstraintNodePosition::SecondTranslationIndex)
                            ));

                            translation->abPos = SbVec3f(
                                secondPos.x,
                                secondPos.y,
                                zConstrH
                            );  // Absolute Reference
                            translation->translation
                                = SbVec3f(relpos2.x - relpos1.x, relpos2.y - relpos1.y, 0);

                            break;
                        }
                        else if (geo2->is<Part::GeomLineSegment>()) {
                            std::swap(geo1, geo2);
                        }

                        if (geo1->is<Part::GeomLineSegment>()) {
                            const Part::GeomLineSegment* lineSeg
                                = static_cast<const Part::GeomLineSegment*>(geo1);
                            Base::Vector3d dir
                                = (lineSeg->getEndPoint() - lineSeg->getStartPoint()).Normalize();
                            Base::Vector3d norm(-dir.y, dir.x, 0);
                            if (geo2->is<Part::GeomCircle>()) {
                                const Part::GeomCircle* circle
                                    = static_cast<const Part::GeomCircle*>(geo2);
                                // tangency between a line and a circle
                                float length = (circle->getCenter() - lineSeg->getStartPoint()) * dir;

                                pos = lineSeg->getStartPoint() + dir * length;
                                relPos = norm * 1;  // TODO Huh?
                            }
                            else if (geo2->is<Part::GeomEllipse>()
                                     || geo2->is<Part::GeomArcOfEllipse>()) {

                                Base::Vector3d center;
                                if (geo2->is<Part::GeomEllipse>()) {
                                    const Part::GeomEllipse* ellipse
                                        = static_cast<const Part::GeomEllipse*>(geo2);
                                    center = ellipse->getCenter();
                                }
                                else {
                                    const Part::GeomArcOfEllipse* aoc
                                        = static_cast<const Part::GeomArcOfEllipse*>(geo2);
                                    center = aoc->getCenter();
                                }

                                // tangency between a line and an ellipse
                                float length = (center - lineSeg->getStartPoint()) * dir;

                                pos = lineSeg->getStartPoint() + dir * length;
                                relPos = norm * 1;
                            }
                            else if (geo2->is<Part::GeomArcOfCircle>()) {
                                const Part::GeomArcOfCircle* arc
                                    = static_cast<const Part::GeomArcOfCircle*>(geo2);
                                // tangency between a line and an arc
                                float length = (arc->getCenter() - lineSeg->getStartPoint()) * dir;

                                pos = lineSeg->getStartPoint() + dir * length;
                                relPos = norm * 1;  // TODO Huh?
                            }
                        }

                        if (geo1->is<Part::GeomCircle>() && geo2->is<Part::GeomCircle>()) {
                            const Part::GeomCircle* circle1 = static_cast<const Part::GeomCircle*>(
                                geo1
                            );
                            const Part::GeomCircle* circle2 = static_cast<const Part::GeomCircle*>(
                                geo2
                            );
                            // tangency between two circles
                            Base::Vector3d dir
                                = (circle2->getCenter() - circle1->getCenter()).Normalize();
                            pos = circle1->getCenter() + dir * circle1->getRadius();
                            relPos = dir * 1;
                        }
                        else if (geo2->is<Part::GeomCircle>()) {
                            std::swap(geo1, geo2);
                        }

                        if (geo1->is<Part::GeomCircle>() && geo2->is<Part::GeomArcOfCircle>()) {
                            const Part::GeomCircle* circle = static_cast<const Part::GeomCircle*>(geo1);
                            const Part::GeomArcOfCircle* arc
                                = static_cast<const Part::GeomArcOfCircle*>(geo2);
                            // tangency between a circle and an arc
                            Base::Vector3d dir = (arc->getCenter() - circle->getCenter()).Normalize();
                            pos = circle->getCenter() + dir * circle->getRadius();
                            relPos = dir * 1;
                        }
                        else if (geo1->is<Part::GeomArcOfCircle>()
                                 && geo2->is<Part::GeomArcOfCircle>()) {
                            const Part::GeomArcOfCircle* arc1
                                = static_cast<const Part::GeomArcOfCircle*>(geo1);
                            const Part::GeomArcOfCircle* arc2
                                = static_cast<const Part::GeomArcOfCircle*>(geo2);
                            // tangency between two arcs
                            Base::Vector3d dir = (arc2->getCenter() - arc1->getCenter()).Normalize();
                            pos = arc1->getCenter() + dir * arc1->getRadius();
                            relPos = dir * 1;
                        }
                        auto translation = static_cast<SoZoomTranslation*>(sep->getChild(
                            static_cast<int>(ConstraintNodePosition::FirstTranslationIndex)
                        ));

                        translation->abPos = SbVec3f(pos.x, pos.y, zConstrH);  // Absolute Reference
                        translation->translation = SbVec3f(relPos.x, relPos.y, 0);
                    }
                } break;
                case Symmetric: {
                    assert(Constr->First >= -extGeoCount && Constr->First < intGeoCount);
                    assert(Constr->Second >= -extGeoCount && Constr->Second < intGeoCount);

                    Base::Vector3d pnt1 = geolistfacade.getPoint(Constr->First, Constr->FirstPos);
                    Base::Vector3d pnt2 = geolistfacade.getPoint(Constr->Second, Constr->SecondPos);

                    SbVec3f p1(pnt1.x, pnt1.y, zConstrH);
                    SbVec3f p2(pnt2.x, pnt2.y, zConstrH);
                    SbVec3f dir = (p2 - p1);
                    dir.normalize();
                    SbVec3f norm(-dir[1], dir[0], 0);

                    SoDatumLabel* asciiText = static_cast<SoDatumLabel*>(
                        sep->getChild(static_cast<int>(ConstraintNodePosition::DatumLabelIndex))
                    );
                    asciiText->datumtype = SoDatumLabel::SYMMETRIC;

                    asciiText->pnts.setNum(2);
                    SbVec3f* verts = asciiText->pnts.startEditing();

                    verts[0] = p1;
                    verts[1] = p2;

                    asciiText->pnts.finishEditing();

                    auto translation = static_cast<SoTranslation*>(
                        sep->getChild(static_cast<int>(ConstraintNodePosition::FirstTranslationIndex))
                    );

                    translation->translation = (p1 + p2) / 2;
                } break;
                case Angle: {
                    assert(Constr->First >= -extGeoCount && Constr->First < intGeoCount);
                    assert(
                        (Constr->Second >= -extGeoCount && Constr->Second < intGeoCount)
                        || Constr->Second == GeoEnum::GeoUndef
                    );

                    SbVec3f p0;
                    double distance = Constr->LabelDistance;
                    double startangle, range;
                    double endLineLength1 = 0.0;
                    double endLineLength2 = 0.0;
                    if (Constr->Second != GeoEnum::GeoUndef) {
                        Base::Vector3d dir1, dir2;
                        if (Constr->Third == GeoEnum::GeoUndef) {  // angle between two lines
                            const Part::Geometry* geo1 = geolistfacade.getGeometryFromGeoId(
                                Constr->First
                            );
                            const Part::Geometry* geo2 = geolistfacade.getGeometryFromGeoId(
                                Constr->Second
                            );
                            if (!isLineSegment(*geo1) || !isLineSegment(*geo2)) {
                                break;
                            }
                            auto* line1 = static_cast<const Part::GeomLineSegment*>(geo1);
                            auto* line2 = static_cast<const Part::GeomLineSegment*>(geo2);

                            bool flip1 = (Constr->FirstPos == PointPos::end);
                            bool flip2 = (Constr->SecondPos == PointPos::end);
                            dir1 = (flip1 ? -1. : 1.)
                                * (line1->getEndPoint() - line1->getStartPoint()).Normalize();
                            dir2 = (flip2 ? -1. : 1.)
                                * (line2->getEndPoint() - line2->getStartPoint()).Normalize();
                            Base::Vector3d pnt1 = flip1 ? line1->getEndPoint()
                                                        : line1->getStartPoint();
                            Base::Vector3d pnt2 = flip2 ? line2->getEndPoint()
                                                        : line2->getStartPoint();
                            Base::Vector3d pnt12 = flip1 ? line1->getStartPoint()
                                                         : line1->getEndPoint();
                            Base::Vector3d pnt22 = flip2 ? line2->getStartPoint()
                                                         : line2->getEndPoint();

                            // line-line intersection
                            Base::Vector3d intersection;
                            {
                                double det = dir1.x * dir2.y - dir1.y * dir2.x;
                                if ((det > 0 ? det : -det) < 1e-10) {
                                    // lines are coincident (or parallel) and in this case the
                                    // center of the point pairs with the shortest distance is
                                    // used
                                    Base::Vector3d p1[2], p2[2];
                                    p1[0] = line1->getStartPoint();
                                    p1[1] = line1->getEndPoint();
                                    p2[0] = line2->getStartPoint();
                                    p2[1] = line2->getEndPoint();
                                    double length = std::numeric_limits<double>::max();
                                    for (int i = 0; i <= 1; i++) {
                                        for (int j = 0; j <= 1; j++) {
                                            double tmp = (p2[j] - p1[i]).Length();
                                            if (tmp < length) {
                                                length = tmp;
                                                double x = (p2[j].x + p1[i].x) / 2;
                                                double y = (p2[j].y + p1[i].y) / 2;
                                                intersection = Base::Vector3d(x, y, 0.);
                                            }
                                        }
                                    }
                                }
                                else {
                                    double c1 = dir1.y * pnt1.x - dir1.x * pnt1.y;
                                    double c2 = dir2.y * pnt2.x - dir2.x * pnt2.y;
                                    double x = (dir1.x * c2 - dir2.x * c1) / det;
                                    double y = (dir1.y * c2 - dir2.y * c1) / det;
                                    intersection = Base::Vector3d(x, y, 0.);
                                }
                            }
                            p0.setValue(intersection.x, intersection.y, 0.);

                            range = Constr->getValue();  // WYSIWYG
                            startangle = atan2(dir1.y, dir1.x);
                            Base::Vector3d vl1 = dir1 * 2 * distance - (pnt1 - intersection);
                            Base::Vector3d vl2 = dir2 * 2 * distance - (pnt2 - intersection);
                            Base::Vector3d vl12 = dir1 * 2 * distance - (pnt12 - intersection);
                            Base::Vector3d vl22 = dir2 * 2 * distance - (pnt22 - intersection);

                            endLineLength1 = vl12.Dot(dir1) > 0 ? vl12.Length()
                                : vl1.Dot(dir1) < 0             ? -vl1.Length()
                                                                : 0.0;
                            endLineLength2 = vl22.Dot(dir2) > 0 ? vl22.Length()
                                : vl2.Dot(dir2) < 0             ? -vl2.Length()
                                                                : 0.0;
                        }
                        else {  // angle-via-point
                            Base::Vector3d p = geolistfacade.getPoint(Constr->Third, Constr->ThirdPos);
                            p0 = SbVec3f(p.x, p.y, 0);
                            dir1 = getNormal(geolistfacade, Constr->First, p);
                            // TODO: Check
                            // dir1 = getSolvedSketch().calculateNormalAtPoint(Constr->First,
                            // p.x, p.y);
                            dir1.RotateZ(-pi / 2);  // convert to vector of tangency by rotating
                            dir2 = getNormal(geolistfacade, Constr->Second, p);
                            // TODO: Check
                            // dir2 = getSolvedSketch().calculateNormalAtPoint(Constr->Second,
                            // p.x, p.y);
                            dir2.RotateZ(-pi / 2);

                            startangle = atan2(dir1.y, dir1.x);
                            range = atan2(
                                dir1.x * dir2.y - dir1.y * dir2.x,
                                dir1.x * dir2.x + dir1.y * dir2.y
                            );
                        }
                    }
                    else if (Constr->First != GeoEnum::GeoUndef) {
                        const Part::Geometry* geo = geolistfacade.getGeometryFromGeoId(Constr->First);
                        if (geo->is<Part::GeomLineSegment>()) {
                            auto* lineSeg = static_cast<const Part::GeomLineSegment*>(geo);
                            p0 = Base::convertTo<SbVec3f>(
                                (lineSeg->getEndPoint() + lineSeg->getStartPoint()) / 2
                            );
                            double l1 = 2 * distance
                                - (lineSeg->getEndPoint() - lineSeg->getStartPoint()).Length() / 2;
                            endLineLength1 = 2 * distance;
                            endLineLength2 = l1 > 0. ? l1 : 0.;

                            Base::Vector3d dir = lineSeg->getEndPoint() - lineSeg->getStartPoint();
                            startangle = 0.;
                            range = atan2(dir.y, dir.x);
                        }
                        else if (geo->is<Part::GeomArcOfCircle>()) {
                            auto* arc = static_cast<const Part::GeomArcOfCircle*>(geo);
                            p0 = Base::convertTo<SbVec3f>(arc->getCenter());

                            endLineLength1 = 2 * distance - arc->getRadius();
                            endLineLength2 = endLineLength1;

                            double endangle;
                            arc->getRange(startangle, endangle, /*emulateCCWXY=*/true);
                            range = endangle - startangle;
                        }
                        else {
                            break;
                        }
                    }
                    else {
                        break;
                    }

                    SoDatumLabel* asciiText = static_cast<SoDatumLabel*>(
                        sep->getChild(static_cast<int>(ConstraintNodePosition::DatumLabelIndex))
                    );
                    asciiText->string = SbString(getPresentationString(Constr).toUtf8().constData());
                    asciiText->strikethrough = !Constr->isActive;
                    asciiText->datumtype = SoDatumLabel::ANGLE;
                    asciiText->param1 = distance;
                    asciiText->param2 = startangle;
                    asciiText->param3 = range;
                    asciiText->param4 = endLineLength1;
                    asciiText->param5 = endLineLength2;

                    asciiText->pnts.setNum(2);
                    SbVec3f* verts = asciiText->pnts.startEditing();

                    verts[0] = p0;

                    asciiText->pnts.finishEditing();

                } break;
                case Diameter: {
                    assert(Constr->First >= -extGeoCount && Constr->First < intGeoCount);

                    Base::Vector3d pnt1(0., 0., 0.), pnt2(0., 0., 0.);
                    double startHelperAngle = 0.;
                    double startHelperRange = 0.;
                    double endHelperAngle = 0.;
                    double endHelperRange = 0.;

                    if (Constr->First == GeoEnum::GeoUndef) {
                        break;
                    }

                    const Part::Geometry* geo = geolistfacade.getGeometryFromGeoId(Constr->First);

                    if (geo->is<Part::GeomArcOfCircle>()) {
                        auto* arc = static_cast<const Part::GeomArcOfCircle*>(geo);
                        double radius = arc->getRadius();
                        double angle = (double)Constr->LabelPosition;  // between -pi and pi
                        double startAngle, endAngle;                   // between 0 and 2*pi
                        arc->getRange(startAngle, endAngle, /*emulateCCW=*/true);

                        if (angle == 10) {
                            angle = (startAngle + endAngle) / 2;
                        }

                        findHelperAngles(startHelperAngle, startHelperRange, angle, startAngle, endAngle);

                        findHelperAngles(endHelperAngle, endHelperRange, angle + pi, startAngle, endAngle);

                        Base::Vector3d center = arc->getCenter();
                        pnt1 = center - radius * Base::Vector3d(cos(angle), sin(angle), 0.);
                        pnt2 = center + radius * Base::Vector3d(cos(angle), sin(angle), 0.);
                    }
                    else if (geo->is<Part::GeomCircle>()) {
                        auto* circle = static_cast<const Part::GeomCircle*>(geo);
                        double radius = circle->getRadius();
                        double angle = (double)Constr->LabelPosition;
                        if (angle == 10) {
                            angle = 0;
                        }
                        Base::Vector3d center = circle->getCenter();
                        pnt1 = center - radius * Base::Vector3d(cos(angle), sin(angle), 0.);
                        pnt2 = center + radius * Base::Vector3d(cos(angle), sin(angle), 0.);
                    }
                    else {
                        break;
                    }

                    SbVec3f p1(pnt1.x, pnt1.y, zConstrH);
                    SbVec3f p2(pnt2.x, pnt2.y, zConstrH);

                    SoDatumLabel* asciiText = static_cast<SoDatumLabel*>(
                        sep->getChild(static_cast<int>(ConstraintNodePosition::DatumLabelIndex))
                    );

                    // Get display string with units hidden if so requested
                    asciiText->string = SbString(
                        getPresentationString(Constr, "⌀").toUtf8().constData()
                    );
                    asciiText->strikethrough = !Constr->isActive;

                    asciiText->datumtype = SoDatumLabel::DIAMETER;
                    asciiText->param1 = Constr->LabelDistance;
                    asciiText->param2 = Constr->LabelPosition;
                    asciiText->param3 = static_cast<float>(startHelperAngle);
                    asciiText->param4 = static_cast<float>(startHelperRange);
                    asciiText->param5 = static_cast<float>(endHelperAngle);
                    asciiText->param6 = static_cast<float>(endHelperRange);

                    asciiText->pnts.setNum(2);
                    SbVec3f* verts = asciiText->pnts.startEditing();

                    verts[0] = p1;
                    verts[1] = p2;

                    asciiText->pnts.finishEditing();
                } break;
                case Weight:
                case Radius: {
                    assert(Constr->First >= -extGeoCount && Constr->First < intGeoCount);

                    Base::Vector3d pnt1(0., 0., 0.), pnt2(0., 0., 0.);
                    double helperStartAngle = 0.;
                    double helperRange = 0.;

                    if (Constr->First == GeoEnum::GeoUndef) {
                        break;
                    }
                    const Part::Geometry* geo = geolistfacade.getGeometryFromGeoId(Constr->First);

                    if (geo->is<Part::GeomArcOfCircle>()) {
                        auto* arc = static_cast<const Part::GeomArcOfCircle*>(geo);
                        double radius = arc->getRadius();
                        double angle = (double)Constr->LabelPosition;  // between -pi and pi
                        double startAngle, endAngle;                   // between 0 and 2*pi
                        arc->getRange(startAngle, endAngle, /*emulateCCW=*/true);

                        if (angle == 10) {
                            angle = (startAngle + endAngle) / 2;
                        }

                        findHelperAngles(helperStartAngle, helperRange, angle, startAngle, endAngle);

                        pnt1 = arc->getCenter();
                        pnt2 = pnt1 + radius * Base::Vector3d(cos(angle), sin(angle), 0.);
                    }
                    else if (geo->is<Part::GeomCircle>()) {
                        auto* circle = static_cast<const Part::GeomCircle*>(geo);
                        auto gf = GeometryFacade::getFacade(geo);

                        double radius;

                        radius = circle->getRadius();

                        double angle = (double)Constr->LabelPosition;
                        if (angle == 10) {
                            angle = 0;
                        }
                        pnt1 = circle->getCenter();
                        pnt2 = pnt1 + radius * Base::Vector3d(cos(angle), sin(angle), 0.);
                    }
                    else {
                        break;
                    }

                    SbVec3f p1(pnt1.x, pnt1.y, zConstrH);
                    SbVec3f p2(pnt2.x, pnt2.y, zConstrH);

                    SoDatumLabel* asciiText = static_cast<SoDatumLabel*>(
                        sep->getChild(static_cast<int>(ConstraintNodePosition::DatumLabelIndex))
                    );

                    // Get display string with units hidden if so requested
                    if (Constr->Type == Weight) {
                        asciiText->string = SbString(
                            QString::number(Constr->getValue()).toStdString().c_str()
                        );
                    }
                    else {
                        asciiText->string = SbString(
                            getPresentationString(Constr, "R").toUtf8().constData()
                        );
                        asciiText->strikethrough = !Constr->isActive;
                    }

                    asciiText->datumtype = SoDatumLabel::RADIUS;
                    asciiText->param1 = Constr->LabelDistance;
                    asciiText->param2 = Constr->LabelPosition;
                    asciiText->param3 = helperStartAngle;
                    asciiText->param4 = helperRange;

                    asciiText->pnts.setNum(2);
                    SbVec3f* verts = asciiText->pnts.startEditing();

                    verts[0] = p1;
                    verts[1] = p2;

                    asciiText->pnts.finishEditing();
                } break;
                case Coincident:  // nothing to do for coincident
                case None:
                case InternalAlignment:
                case NumConstraintTypes:
                    break;
            }
        }
        catch (Base::Exception& e) {
            Base::Console().developerError(
                "EditModeConstraintCoinManager",
                "Exception during draw: %s\n",
                e.what()
            );
            e.reportException();
        }
        catch (...) {
            Base::Console().developerError(
                "EditModeConstraintCoinManager",
                "Exception during draw: unknown\n"
            );
        }
    }
}

void EditModeConstraintCoinManager::findHelperAngles(
    double& helperStartAngle,
    double& helperRange,
    double angle,
    double startAngle,
    double endAngle
)
{
    using std::numbers::pi;

    double margin = 0.2;  // about 10deg
    if (angle < 0) {
        angle = angle + 2 * pi;
    }
    // endAngle can be more than 2*pi as its startAngle + arcAngle
    if (endAngle > 2 * pi && angle < endAngle - 2 * pi) {
        angle = angle + 2 * pi;
    }
    if (!(angle > startAngle && angle < endAngle)) {
        if ((angle < startAngle && startAngle - angle < angle + 2 * pi - endAngle)
            || (angle > endAngle && startAngle + 2 * pi - angle < angle - endAngle)) {
            if (angle > startAngle) {
                angle -= 2 * pi;
            }
            helperStartAngle = angle - margin;
            helperRange = startAngle - angle + margin;
        }
        else {
            if (angle < endAngle) {
                angle += 2 * pi;
            }
            helperStartAngle = endAngle;
            helperRange = angle - endAngle + margin;
        }
    }
}

Base::Vector3d EditModeConstraintCoinManager::seekConstraintPosition(
    const Base::Vector3d& origPos,
    const Base::Vector3d& norm,
    const Base::Vector3d& dir,
    float step,
    const SoNode* constraint
)
{
    return norm * 0.5f * step;
}

void EditModeConstraintCoinManager::updateConstraintColor(
    const std::vector<Sketcher::Constraint*>& constraints
)
{
    // Because coincident constraints are selected using the point color, we need to edit the point
    // materials.

    std::vector<int> PtNum;
    std::vector<SbColor*> pcolor;  // point color
    std::vector<std::vector<int>> CurvNum;
    std::vector<std::vector<SbColor*>> color;  // curve color

    for (int l = 0; l < geometryLayerParameters.getCoinLayerCount(); l++) {
        PtNum.push_back(editModeScenegraphNodes.PointsMaterials[l]->diffuseColor.getNum());
        pcolor.push_back(editModeScenegraphNodes.PointsMaterials[l]->diffuseColor.startEditing());
        CurvNum.emplace_back();
        color.emplace_back();
        for (int t = 0; t < geometryLayerParameters.getSubLayerCount(); t++) {
            CurvNum[l].push_back(editModeScenegraphNodes.CurvesMaterials[l][t]->diffuseColor.getNum());
            color[l].push_back(
                editModeScenegraphNodes.CurvesMaterials[l][t]->diffuseColor.startEditing()
            );
        }
    }

    int maxNumberOfConstraints = std::min(
        editModeScenegraphNodes.constrGroup->getNumChildren(),
        static_cast<int>(constraints.size())
    );

    // colors of the constraints
    for (int i = 0; i < maxNumberOfConstraints; i++) {
        SoSeparator* s = static_cast<SoSeparator*>(editModeScenegraphNodes.constrGroup->getChild(i));

        // Check Constraint Type
        Sketcher::Constraint* constraint = constraints[i];
        ConstraintType type = constraint->Type;

        // It may happen that color updates are triggered by programmatic selection changes before a
        // command final update. Then constraints may have been changed and the color will be
        // updated as part
        if (type != vConstrType[i]) {
            break;
        }

        bool hasDatumLabel
            = (type == Sketcher::Angle || type == Sketcher::Radius || type == Sketcher::Diameter
               || type == Sketcher::Weight || type == Sketcher::Symmetric || type == Sketcher::Distance
               || type == Sketcher::DistanceX || type == Sketcher::DistanceY);

        // Non DatumLabel Nodes will have a material excluding coincident
        bool hasMaterial = false;

        SoMaterial* m = nullptr;
        if (!hasDatumLabel && type != Sketcher::Coincident && type != Sketcher::InternalAlignment) {
            int matIndex = static_cast<int>(ConstraintNodePosition::MaterialIndex);
            if (matIndex < s->getNumChildren()) {
                hasMaterial = true;
                m = static_cast<SoMaterial*>(s->getChild(matIndex));
            }
        }

        auto selectpoint = [this, pcolor, PtNum](int geoid, Sketcher::PointPos pos) {
            if (geoid >= 0) {
                auto multifieldIndex = coinMapping.getIndexLayer(geoid, pos);

                if (multifieldIndex != MultiFieldId::Invalid) {
                    int index = multifieldIndex.fieldIndex;
                    int layer = multifieldIndex.layerId;
                    if (layer < static_cast<int>(PtNum.size()) && index >= 0 && index < PtNum[layer]) {
                        pcolor[layer][index] = drawingParameters.SelectColor;
                    }
                }
            }
        };

        auto selectline = [this, color, CurvNum](int geoid) {
            if (geoid >= 0) {
                auto multifieldIndex = coinMapping.getIndexLayer(geoid, Sketcher::PointPos::none);

                if (multifieldIndex != MultiFieldId::Invalid) {
                    int index = multifieldIndex.fieldIndex;
                    int layer = multifieldIndex.layerId;
                    int sublayer = multifieldIndex.geoTypeId;
                    if (layer < static_cast<int>(CurvNum.size())
                        && sublayer < static_cast<int>(CurvNum[layer].size()) && index >= 0
                        && index < CurvNum[layer][sublayer]) {
                        color[layer][sublayer][index] = drawingParameters.SelectColor;
                    }
                }
            }
        };


        if (ViewProviderSketchCoinAttorney::isConstraintSelected(viewProvider, i)) {
            if (hasDatumLabel) {
                SoDatumLabel* l = static_cast<SoDatumLabel*>(
                    s->getChild(static_cast<int>(ConstraintNodePosition::DatumLabelIndex))
                );
                l->textColor = drawingParameters.SelectColor;
            }
            else if (hasMaterial) {
                m->diffuseColor = drawingParameters.SelectColor;
            }
            else if (type == Sketcher::Coincident) {
                selectpoint(constraint->First, constraint->FirstPos);
                selectpoint(constraint->Second, constraint->SecondPos);
            }
            else if (type == Sketcher::InternalAlignment) {
                switch (constraint->AlignmentType) {
                    case EllipseMajorDiameter:
                    case EllipseMinorDiameter:
                    case HyperbolaMajor:
                    case HyperbolaMinor:
                    case ParabolaFocalAxis: {
                        selectline(constraint->First);
                    } break;
                    case EllipseFocus1:
                    case EllipseFocus2:
                    case HyperbolaFocus:
                    case ParabolaFocus:
                    case BSplineControlPoint:
                    case BSplineKnotPoint: {
                        selectpoint(constraint->First, constraint->FirstPos);
                    } break;
                    default:
                        break;
                }
            }
        }
        else if (ViewProviderSketchCoinAttorney::isConstraintPreselected(viewProvider, i)) {
            if (hasDatumLabel) {
                SoDatumLabel* l = static_cast<SoDatumLabel*>(
                    s->getChild(static_cast<int>(ConstraintNodePosition::DatumLabelIndex))
                );
                l->textColor = drawingParameters.PreselectColor;
            }
            else if (hasMaterial) {
                m->diffuseColor = drawingParameters.PreselectColor;
            }
        }
        else {
            if (hasDatumLabel) {
                SoDatumLabel* l = static_cast<SoDatumLabel*>(
                    s->getChild(static_cast<int>(ConstraintNodePosition::DatumLabelIndex))
                );

                l->textColor = constraint->isActive
                    ? ViewProviderSketchCoinAttorney::constraintHasExpression(viewProvider, i)
                        ? drawingParameters.ExprBasedConstrDimColor
                        : (constraint->isDriving ? drawingParameters.ConstrDimColor
                                                 : drawingParameters.NonDrivingConstrDimColor)
                    : drawingParameters.DeactivatedConstrDimColor;
            }
            else if (hasMaterial) {
                m->diffuseColor = constraint->isActive
                    ? (constraint->isDriving ? drawingParameters.ConstrDimColor
                                             : drawingParameters.NonDrivingConstrDimColor)
                    : drawingParameters.DeactivatedConstrDimColor;
            }
        }
    }

    for (int l = 0; l < geometryLayerParameters.getCoinLayerCount(); l++) {
        editModeScenegraphNodes.PointsMaterials[l]->diffuseColor.finishEditing();
        for (int t = 0; t < geometryLayerParameters.getSubLayerCount(); t++) {
            editModeScenegraphNodes.CurvesMaterials[l][t]->diffuseColor.finishEditing();
        }
    }
}

void EditModeConstraintCoinManager::rebuildConstraintNodes()
{
    auto geolistfacade = ViewProviderSketchCoinAttorney::getGeoListFacade(viewProvider);

    rebuildConstraintNodes(geolistfacade);
}

void EditModeConstraintCoinManager::setConstraintSelectability(bool enabled /* = true */)
{
    if (enabled) {
        editModeScenegraphNodes.constrGrpSelect->style.setValue(SoPickStyle::SHAPE);
    }
    else {
        editModeScenegraphNodes.constrGrpSelect->style.setValue(SoPickStyle::UNPICKABLE);
    }
}

void EditModeConstraintCoinManager::rebuildConstraintNodes(const GeoListFacade& geolistfacade)
{
    const std::vector<Sketcher::Constraint*>& constrlist
        = ViewProviderSketchCoinAttorney::getConstraints(viewProvider);

    // clean up
    Gui::coinRemoveAllChildren(editModeScenegraphNodes.constrGroup);

    vConstrType.clear();

    // Get sketch normal
    Base::Vector3d RN(0, 0, 1);

    // move to position of Sketch
    Base::Placement Plz = ViewProviderSketchCoinAttorney::getEditingPlacement(viewProvider);
    Base::Rotation tmp(Plz.getRotation());
    tmp.multVec(RN, RN);
    Plz.setRotation(tmp);

    SbVec3f norm(RN.x, RN.y, RN.z);

    rebuildConstraintNodes(geolistfacade, constrlist, norm);
}

void EditModeConstraintCoinManager::rebuildConstraintNodes(
    const GeoListFacade& geolistfacade,
    const std::vector<Sketcher::Constraint*> constrlist,
    SbVec3f norm
)
{

    for (std::vector<Sketcher::Constraint*>::const_iterator it = constrlist.begin();
         it != constrlist.end();
         ++it) {
        // root separator for one constraint
        SoSeparator* sep = new SoSeparator();
        sep->ref();
        // no caching for frequently-changing data structures
        sep->renderCaching = SoSeparator::OFF;

        // every constrained visual node gets its own material for preselection and selection
        SoMaterial* mat = new SoMaterial;
        mat->ref();
        mat->diffuseColor = (*it)->isActive
            ? ((*it)->isDriving ? drawingParameters.ConstrDimColor
                                : drawingParameters.NonDrivingConstrDimColor)
            : drawingParameters.DeactivatedConstrDimColor;


        // distinguish different constraint types to build up
        switch ((*it)->Type) {
            case Distance:
            case DistanceX:
            case DistanceY:
            case Radius:
            case Diameter:
            case Weight:
            case Angle: {
                SoDatumLabel* text = new SoDatumLabel();
                text->norm.setValue(norm);
                text->string = "";
                text->textColor = (*it)->isActive
                    ? ((*it)->isDriving ? drawingParameters.ConstrDimColor
                                        : drawingParameters.NonDrivingConstrDimColor)
                    : drawingParameters.DeactivatedConstrDimColor;
                text->size.setValue(drawingParameters.labelFontSize);
                text->lineWidth = 2 * drawingParameters.pixelScalingFactor;
                text->useAntialiasing = false;
                sep->addChild(text);
                editModeScenegraphNodes.constrGroup->addChild(sep);
                vConstrType.push_back((*it)->Type);
                // nodes not needed
                sep->unref();
                mat->unref();
                continue;  // jump to next constraint
            } break;
            case Horizontal:
            case Vertical:
            case Block: {
                // #define CONSTRAINT_SEPARATOR_INDEX_MATERIAL_OR_DATUMLABEL 0
                sep->addChild(mat);
                // #define CONSTRAINT_SEPARATOR_INDEX_FIRST_TRANSLATION 1
                sep->addChild(new SoZoomTranslation());
                // #define CONSTRAINT_SEPARATOR_INDEX_FIRST_ICON 2
                sep->addChild(new SoImage());
                // #define CONSTRAINT_SEPARATOR_INDEX_FIRST_CONSTRAINTID 3
                sep->addChild(new SoInfo());
                // #define CONSTRAINT_SEPARATOR_INDEX_SECOND_TRANSLATION 4
                sep->addChild(new SoZoomTranslation());
                // #define CONSTRAINT_SEPARATOR_INDEX_SECOND_ICON 5
                sep->addChild(new SoImage());
                // #define CONSTRAINT_SEPARATOR_INDEX_SECOND_CONSTRAINTID 6
                sep->addChild(new SoInfo());

                // remember the type of this constraint node
                vConstrType.push_back((*it)->Type);
            } break;
            case Coincident:  // no visual for coincident so far
                vConstrType.push_back(Coincident);
                break;
            case Parallel:
            case Perpendicular:
            case Equal: {
                // #define CONSTRAINT_SEPARATOR_INDEX_MATERIAL_OR_DATUMLABEL 0
                sep->addChild(mat);
                // #define CONSTRAINT_SEPARATOR_INDEX_FIRST_TRANSLATION 1
                sep->addChild(new SoZoomTranslation());
                // #define CONSTRAINT_SEPARATOR_INDEX_FIRST_ICON 2
                sep->addChild(new SoImage());
                // #define CONSTRAINT_SEPARATOR_INDEX_FIRST_CONSTRAINTID 3
                sep->addChild(new SoInfo());
                // #define CONSTRAINT_SEPARATOR_INDEX_SECOND_TRANSLATION 4
                sep->addChild(new SoZoomTranslation());
                // #define CONSTRAINT_SEPARATOR_INDEX_SECOND_ICON 5
                sep->addChild(new SoImage());
                // #define CONSTRAINT_SEPARATOR_INDEX_SECOND_CONSTRAINTID 6
                sep->addChild(new SoInfo());

                // remember the type of this constraint node
                vConstrType.push_back((*it)->Type);
            } break;
            case PointOnObject:
            case Tangent:
            case SnellsLaw: {
                // #define CONSTRAINT_SEPARATOR_INDEX_MATERIAL_OR_DATUMLABEL 0
                sep->addChild(mat);
                // #define CONSTRAINT_SEPARATOR_INDEX_FIRST_TRANSLATION 1
                sep->addChild(new SoZoomTranslation());
                // #define CONSTRAINT_SEPARATOR_INDEX_FIRST_ICON 2
                sep->addChild(new SoImage());
                // #define CONSTRAINT_SEPARATOR_INDEX_FIRST_CONSTRAINTID 3
                sep->addChild(new SoInfo());

                if ((*it)->Type == Tangent) {
                    const Part::Geometry* geo1 = geolistfacade.getGeometryFromGeoId((*it)->First);
                    const Part::Geometry* geo2 = geolistfacade.getGeometryFromGeoId((*it)->Second);
                    if (!geo1 || !geo2) {
                        Base::Console().developerWarning(
                            "EditModeConstraintCoinManager",
                            "Tangent constraint references non-existing geometry\n"
                        );
                    }
                    else if (geo1->is<Part::GeomLineSegment>() && geo2->is<Part::GeomLineSegment>()) {
                        // #define CONSTRAINT_SEPARATOR_INDEX_SECOND_TRANSLATION 4
                        sep->addChild(new SoZoomTranslation());
                        // #define CONSTRAINT_SEPARATOR_INDEX_SECOND_ICON 5
                        sep->addChild(new SoImage());
                        // #define CONSTRAINT_SEPARATOR_INDEX_SECOND_CONSTRAINTID 6
                        sep->addChild(new SoInfo());
                    }
                }

                vConstrType.push_back((*it)->Type);
            } break;
            case Symmetric: {
                SoDatumLabel* arrows = new SoDatumLabel();
                arrows->norm.setValue(norm);
                arrows->string = "";
                arrows->textColor = drawingParameters.ConstrDimColor;
                arrows->lineWidth = 2 * drawingParameters.pixelScalingFactor;

                // #define CONSTRAINT_SEPARATOR_INDEX_MATERIAL_OR_DATUMLABEL 0
                sep->addChild(arrows);
                // #define CONSTRAINT_SEPARATOR_INDEX_FIRST_TRANSLATION 1
                sep->addChild(new SoTranslation());
                // #define CONSTRAINT_SEPARATOR_INDEX_FIRST_ICON 2
                sep->addChild(new SoImage());
                // #define CONSTRAINT_SEPARATOR_INDEX_FIRST_CONSTRAINTID 3
                sep->addChild(new SoInfo());

                vConstrType.push_back((*it)->Type);
            } break;
            case InternalAlignment: {
                vConstrType.push_back((*it)->Type);
            } break;
            default:
                vConstrType.push_back((*it)->Type);
        }

        editModeScenegraphNodes.constrGroup->addChild(sep);
        // decrement ref counter again
        sep->unref();
        mat->unref();
    }
}

QString EditModeConstraintCoinManager::getPresentationString(
    const Constraint* constraint,
    std::string prefix
)
{
    /**
     * Hide units if
     *  - user has requested it,
     *  - is being displayed in the base units, -and-
     *  - the schema being used has a clear base unit in the first place.
     *
     * Remove unit string if expected unit string matches actual unit string
     * Example code from: Mod/TechDraw/App/DrawViewDimension.cpp:372
     *
     * Hide the default length unit
     */
    auto fixValueStr = [&](const QString& valueStr, const auto& unitStr) -> std::optional<QString> {
        if (!constraintParameters.bHideUnits || constraint->Type == Sketcher::Angle) {
            return std::nullopt;
        }

        const auto baseUnitStr {Base::UnitsApi::getBasicLengthUnit()};
        if (baseUnitStr.empty() || baseUnitStr != unitStr) {
            return std::nullopt;
        }

        // trailing space or non-dig
        const QRegularExpression rxUnits {QString::fromUtf8(" \\D*$")};
        auto vStr = valueStr;
        vStr.remove(rxUnits);
        return {vStr};
    };

    // Get the current value string including units
    double factor {};
    std::string unitStr;  // the actual unit string
    const auto constrPresValue {constraint->getPresentationValue().getUserString(factor, unitStr)};
    auto valueStr = QString::fromStdString(constrPresValue);

    auto fixedValueStr = fixValueStr(valueStr, unitStr).value_or(valueStr);
    if (!prefix.empty()) {
        fixedValueStr.prepend(QString::fromStdString(prefix));
    }

    if (constraintParameters.bShowDimensionalName && !constraint->Name.empty()) {
        /**
         * Create the representation string from the user defined format string
         * Format options are:
         * %N - the constraint name parameter
         * %V - the value of the dimensional constraint, including any unit characters
         */
        auto sDimFmt {constraintParameters.sDimensionalStringFormat};
        if (!sDimFmt.contains(QLatin1String("%V"))
            && !sDimFmt.contains(QLatin1String("%N"))) {  // using default format "%N = %V"

            fixedValueStr = QString::fromStdString(constraint->Name) + QString::fromLatin1(" = ")
                + fixedValueStr;
        }
        else {
            sDimFmt.replace(QLatin1String("%N"), QString::fromStdString(constraint->Name));
            sDimFmt.replace(QLatin1String("%V"), fixedValueStr);
            fixedValueStr = sDimFmt;
        }
    }

    int constraintIndex = -1;
    const auto& constrlist = ViewProviderSketchCoinAttorney::getConstraints(viewProvider);
    auto it = std::find(constrlist.begin(), constrlist.end(), constraint);
    if (it != constrlist.end()) {
        constraintIndex = std::distance(constrlist.begin(), it);
        if (ViewProviderSketchCoinAttorney::constraintHasExpression(viewProvider, constraintIndex)) {
            fixedValueStr += QStringLiteral(" (ƒ𝑥)");
        }
    }

    if (!constraint->isDriving) {
        fixedValueStr = QStringLiteral("(") + fixedValueStr + QStringLiteral(")");
    }

    return fixedValueStr;
}

std::set<int> EditModeConstraintCoinManager::detectPreselectionConstr(const SoPickedPoint* Point)
{
    std::set<int> constrIndices;
    SoPath* path = Point->getPath();

    // The picked node must be a child of the main constraint group.
    SoNode* tailFather2 = path->getNode(path->getLength() - 3);
    if (tailFather2 != editModeScenegraphNodes.constrGroup) {
        return constrIndices;
    }

    SoNode* tail = path->getTail();  // This is the SoImage or SoDatumLabel node that was picked.
    SoSeparator* sep = static_cast<SoSeparator*>(path->getNode(path->getLength() - 2));

    for (int childIdx = 0; childIdx < sep->getNumChildren(); ++childIdx) {
        if (tail == sep->getChild(childIdx) && dynamic_cast<SoImage*>(tail)) {
            // The SoInfo node with the ID always follows the SoImage node.
            if (childIdx + 1 < sep->getNumChildren()) {
                SoInfo* constrIdsNode = dynamic_cast<SoInfo*>(sep->getChild(childIdx + 1));
                if (!constrIdsNode) {
                    continue;
                }

                QString constrIdsStr = QString::fromLatin1(
                    constrIdsNode->string.getValue().getString()
                );

                if (combinedConstrBoxes.count(constrIdsStr)) {

                    // 1. Get the icon group size in device independent pixels.
                    SbVec3s iconGroupSize = getDisplayedSize(static_cast<SoImage*>(tail));

                    // 2. Get the icon group's absolute world position from its
                    // SoZoomTranslation node.
                    SoZoomTranslation* translation = nullptr;
                    SoNode* firstTransNode = sep->getChild(
                        static_cast<int>(ConstraintNodePosition::FirstTranslationIndex)
                    );
                    if (dynamic_cast<SoZoomTranslation*>(firstTransNode)) {
                        translation = static_cast<SoZoomTranslation*>(firstTransNode);
                    }
                    if (!translation) {
                        continue;
                    }

                    SbVec3f absPos = translation->abPos.getValue();
                    SbVec3f trans = translation->translation.getValue();
                    float scaleFactor = translation->getScaleFactor();

                    // If this is the second icon in a pair, add its relative translation.
                    if (int secondIndex = static_cast<int>(ConstraintNodePosition::SecondIconIndex);
                        secondIndex < sep->getNumChildren()) {
                        SoNode* secondIconNode = sep->getChild(secondIndex);
                        if (tail == secondIconNode) {
                            auto translation2 = static_cast<SoZoomTranslation*>(sep->getChild(
                                static_cast<int>(ConstraintNodePosition::SecondTranslationIndex)
                            ));
                            absPos += translation2->abPos.getValue();
                            trans += translation2->translation.getValue();
                            scaleFactor = translation2->getScaleFactor();
                        }
                    }

                    // 3. Calculate the icon's center in world coordinates.
                    SbVec3f iconGroupWorldPos = absPos + scaleFactor * trans;

                    // 4. Project both the icon's center and the picked point to screen coordinates
                    // (device independent pixels). This is the key: both points are now in the same
                    // coordinate system.
                    SbVec2f iconGroupScreenCenter = ViewProviderSketchCoinAttorney::getScreenCoordinates(
                        viewProvider,
                        SbVec2f(iconGroupWorldPos[0], iconGroupWorldPos[1])
                    );

                    SbVec2f cursorScreenPos = ViewProviderSketchCoinAttorney::getScreenCoordinates(
                        viewProvider,
                        SbVec2f(Point->getPoint()[0], Point->getPoint()[1])
                    );

                    // 5. Calculate cursor position relative to the icon group's top-left corner.
                    //    - QRect/QImage assumes a top-left origin (Y increases downwards).
                    //    - Coin3D screen coordinates have a bottom-left origin (Y increases
                    //    upwards).
                    //    - We must flip the Y-axis for the check.
                    int relativeX = static_cast<int>(
                        cursorScreenPos[0] - iconGroupScreenCenter[0] + iconGroupSize[0] / 2.0f
                    );
                    int relativeY = static_cast<int>(
                        iconGroupScreenCenter[1] - cursorScreenPos[1] + iconGroupSize[1] / 2.0f
                    );

                    // 6. Perform the hit test on each icon in the group.
                    for (const auto& boxInfo : combinedConstrBoxes[constrIdsStr]) {
                        if (boxInfo.first.contains(relativeX, relativeY)) {
                            constrIndices.insert(boxInfo.second.begin(), boxInfo.second.end());
                        }
                    }
                }
                else {
                    // Simple, non-merged icon.
                    QStringList constrIdStrings = constrIdsStr.split(QStringLiteral(","));
                    for (const QString& id : constrIdStrings) {
                        constrIndices.insert(id.toInt());
                    }
                }

                return constrIndices;
            }
        }
    }

    // Handle selection of datum labels (e.g., radius, distance dimensions).
    if (dynamic_cast<SoDatumLabel*>(tail)) {
        for (int i = 0; i < editModeScenegraphNodes.constrGroup->getNumChildren(); ++i) {
            if (editModeScenegraphNodes.constrGroup->getChild(i) == sep) {
                constrIndices.insert(i);
                break;
            }
        }
    }

    return constrIndices;
}

SbVec3s EditModeConstraintCoinManager::getDisplayedSize(const SoImage* iconPtr) const
{
    SbVec3s iconSize = iconPtr->image.getValue().getSize();

    if (iconPtr->width.getValue() != -1) {
        iconSize[0] = iconPtr->width.getValue();
    }
    if (iconPtr->height.getValue() != -1) {
        iconSize[1] = iconPtr->height.getValue();
    }
    return iconSize;
}

// public function that triggers drawing of most constraint icons
void EditModeConstraintCoinManager::drawConstraintIcons()
{
    auto geolistfacade = ViewProviderSketchCoinAttorney::getGeoListFacade(viewProvider);

    drawConstraintIcons(geolistfacade);
}

void EditModeConstraintCoinManager::drawConstraintIcons(const GeoListFacade& geolistfacade)
{
    const std::vector<Sketcher::Constraint*>& constraints
        = ViewProviderSketchCoinAttorney::getConstraints(viewProvider);

    std::vector<constrIconQueueItem> iconQueue;

    int maxNumberOfConstraints = std::min(
        editModeScenegraphNodes.constrGroup->getNumChildren(),
        static_cast<int>(constraints.size())
    );

    for (int constrId = 0; constrId < maxNumberOfConstraints; ++constrId) {
        Sketcher::Constraint* constraint = constraints[constrId];

        // Check if Icon Should be created
        bool multipleIcons = false;

        QString icoType = iconTypeFromConstraint(constraint);
        if (icoType.isEmpty()) {
            continue;
        }

        if (constraint->Type != vConstrType[constrId]) {
            break;
        }

        switch (constraint->Type) {

            case Tangent: {  // second icon is available only for collinear line segments
                const Part::Geometry* geo1 = geolistfacade.getGeometryFromGeoId(constraint->First);
                const Part::Geometry* geo2 = geolistfacade.getGeometryFromGeoId(constraint->Second);
                if (geo1 && geo1->is<Part::GeomLineSegment>() && geo2
                    && geo2->is<Part::GeomLineSegment>()) {
                    multipleIcons = true;
                }
            } break;
            case Horizontal:
            case Vertical: {  // second icon is available only for point alignment
                if (constraint->Second != GeoEnum::GeoUndef
                    && constraint->FirstPos != Sketcher::PointPos::none
                    && constraint->SecondPos != Sketcher::PointPos::none) {
                    multipleIcons = true;
                }
            } break;
            case Parallel:
                multipleIcons = true;
                break;
            case Perpendicular:
                // second icon is available only when there is no common point
                if (constraint->FirstPos == Sketcher::PointPos::none
                    && constraint->Third == GeoEnum::GeoUndef) {
                    multipleIcons = true;
                }
                break;
            case Equal:
                multipleIcons = true;
                break;
            default:
                break;
        }

        // Double-check that we can safely access the Inventor nodes
        if (constrId >= editModeScenegraphNodes.constrGroup->getNumChildren()) {
            Base::Console().developerWarning(
                "EditModeConstraintManager",
                "Can't update constraint icons because view is not in sync with sketch\n"
            );
            break;
        }

        // Find the Constraint Icon SoImage Node
        SoSeparator* sep = static_cast<SoSeparator*>(
            editModeScenegraphNodes.constrGroup->getChild(constrId)
        );
        int numChildren = sep->getNumChildren();

        SbVec3f absPos;
        // Somewhat hacky - we use SoZoomTranslations for most types of icon,
        // but symmetry icons use SoTranslations...
        SoTranslation* translationPtr = static_cast<SoTranslation*>(
            sep->getChild(static_cast<int>(ConstraintNodePosition::FirstTranslationIndex))
        );

        if (dynamic_cast<SoZoomTranslation*>(translationPtr)) {
            absPos = static_cast<SoZoomTranslation*>(translationPtr)->abPos.getValue();
        }
        else {
            absPos = translationPtr->translation.getValue();
        }

        SoImage* coinIconPtr = dynamic_cast<SoImage*>(
            sep->getChild(static_cast<int>(ConstraintNodePosition::FirstIconIndex))
        );
        SoInfo* infoPtr = static_cast<SoInfo*>(
            sep->getChild(static_cast<int>(ConstraintNodePosition::FirstConstraintIdIndex))
        );

        constrIconQueueItem thisIcon;
        thisIcon.type = icoType;
        thisIcon.constraintId = constrId;
        thisIcon.position = absPos;
        thisIcon.destination = coinIconPtr;
        thisIcon.infoPtr = infoPtr;
        thisIcon.visible = (constraint->isInVirtualSpace
                            == ViewProviderSketchCoinAttorney::isShownVirtualSpace(viewProvider))
            && constraint->isVisible;

        if (constraint->Type == Symmetric) {
            Base::Vector3d startingpoint
                = geolistfacade.getPoint(constraint->First, constraint->FirstPos);
            Base::Vector3d endpoint = geolistfacade.getPoint(constraint->Second, constraint->SecondPos);

            SbVec3f pos0(startingpoint.x, startingpoint.y, startingpoint.z);
            SbVec3f pos1(endpoint.x, endpoint.y, endpoint.z);

            thisIcon.iconRotation
                = ViewProviderSketchCoinAttorney::getRotation(viewProvider, pos0, pos1);
        }
        else {
            thisIcon.iconRotation = 0;
        }

        if (multipleIcons) {
            if (constraint->Name.empty()) {
                thisIcon.label = QString::number(constrId + 1);
            }
            else {
                thisIcon.label = QString::fromUtf8(constraint->Name.c_str());
            }
            iconQueue.push_back(thisIcon);

            // Note that the second translation is meant to be applied after the first.
            // So, to get the position of the second icon, we add the two translations together
            //
            // See note ~30 lines up.
            if (numChildren > static_cast<int>(ConstraintNodePosition::SecondConstraintIdIndex)) {
                translationPtr = static_cast<SoTranslation*>(
                    sep->getChild(static_cast<int>(ConstraintNodePosition::SecondTranslationIndex))
                );
                if (dynamic_cast<SoZoomTranslation*>(translationPtr)) {
                    thisIcon.position
                        += static_cast<SoZoomTranslation*>(translationPtr)->abPos.getValue();
                }
                else {
                    thisIcon.position += translationPtr->translation.getValue();
                }

                thisIcon.destination = dynamic_cast<SoImage*>(
                    sep->getChild(static_cast<int>(ConstraintNodePosition::SecondIconIndex))
                );
                thisIcon.infoPtr = static_cast<SoInfo*>(
                    sep->getChild(static_cast<int>(ConstraintNodePosition::SecondConstraintIdIndex))
                );
            }
        }
        else {
            if (constraint->Name.empty()) {
                thisIcon.label = QString();
            }
            else {
                thisIcon.label = QString::fromUtf8(constraint->Name.c_str());
            }
        }

        iconQueue.push_back(thisIcon);
    }

    combineConstraintIcons(iconQueue);
}

void EditModeConstraintCoinManager::combineConstraintIcons(IconQueue iconQueue)
{
    // getScaleFactor gives us a ratio of pixels per some kind of real units
    float scale = ViewProviderSketchCoinAttorney::getScaleFactor(viewProvider);
    float maxDistSquared = pow(scale, 2);

    // There's room for optimisation here; we could reuse the combined icons...
    combinedConstrBoxes.clear();

    // Grid size needs to be slightly larger than the max merge distance to ensure
    // we catch neighbors.
    float gridSize = std::max(1.0f, std::abs(scale) * 1.1f);

    // 2. FILTERING & PREPARATION
    // We create a list of valid indices.
    std::vector<int> validIndices;
    validIndices.reserve(iconQueue.size());

    for (size_t i = 0; i < iconQueue.size(); ++i) {
        if (!iconQueue[i].visible) {
            clearCoinImage(iconQueue[i].destination);
            continue;
        }

        // Symmetric constraints render alone (original logic)
        if (iconQueue[i].type == QStringLiteral("Constraint_Symmetric")) {
            drawTypicalConstraintIcon(iconQueue[i]);
            continue;
        }

        validIndices.push_back(static_cast<int>(i));
    }

    // 3. SPATIAL HASHING (The Grid)
    // Map: Pair(GridX, GridY) -> List of indices in iconQueue
    std::map<std::pair<int, int>, std::vector<int>> grid;

    for (int idx : validIndices) {
        const auto& icon = iconQueue[idx];
        int gx = static_cast<int>(std::floor(icon.position[0] / gridSize));
        int gy = static_cast<int>(std::floor(icon.position[1] / gridSize));
        grid[{gx, gy}].push_back(idx);
    }

    // 4. CLUSTERING (Reversed Iteration)
    std::vector<bool> processed(iconQueue.size(), false);

    for (auto it = validIndices.rbegin(); it != validIndices.rend(); ++it) {
        int startIdx = *it;
        if (processed[startIdx]) {
            continue;
        }

        // Start a new group with the "Anchor"
        IconQueue thisGroup;
        thisGroup.reserve(5);

        // Stack for recursive search (Chain reaction)
        std::stack<int> searchStack;
        searchStack.push(startIdx);
        processed[startIdx] = true;

        while (!searchStack.empty()) {
            int currentIdx = searchStack.top();
            searchStack.pop();
            thisGroup.push_back(iconQueue[currentIdx]);

            // Check neighbors of current icon
            const auto& currentIcon = iconQueue[currentIdx];
            int cx = static_cast<int>(std::floor(currentIcon.position[0] / gridSize));
            int cy = static_cast<int>(std::floor(currentIcon.position[1] / gridSize));

            // Check 3x3 grid neighborhood
            for (int dx = -1; dx <= 1; ++dx) {
                for (int dy = -1; dy <= 1; ++dy) {
                    auto gridIt = grid.find({cx + dx, cy + dy});
                    if (gridIt == grid.end()) {
                        continue;
                    }

                    // Iterate over potential matches in this cell
                    for (int neighborIdx : gridIt->second) {
                        if (processed[neighborIdx]) {
                            continue;
                        }

                        const auto& otherIcon = iconQueue[neighborIdx];

                        float distSq = pow(currentIcon.position[0] - otherIcon.position[0], 2)
                            + pow(currentIcon.position[1] - otherIcon.position[1], 2);

                        if (distSq <= maxDistSquared) {
                            processed[neighborIdx] = true;
                            searchStack.push(neighborIdx);
                        }
                    }
                }
            }
        }

        // 5. DRAW
        if (thisGroup.size() == 1) {
            drawTypicalConstraintIcon(thisGroup[0]);
        }
        else {
            drawMergedConstraintIcons(thisGroup);
        }
    }
}

void EditModeConstraintCoinManager::drawMergedConstraintIcons(IconQueue iconQueue)
{
    for (IconQueue::iterator i = iconQueue.begin(); i != iconQueue.end(); ++i) {
        clearCoinImage(i->destination);
    }

    QImage compositeIcon;
    SoImage* thisDest = iconQueue[0].destination;
    SoInfo* thisInfo = iconQueue[0].infoPtr;

    // Tracks all constraint IDs that are combined into this icon
    QString idString;
    int lastVPad = 0;

    QStringList labels;
    std::vector<int> ids;
    QString thisType;
    QColor iconColor;
    QList<QColor> labelColors;
    int maxColorPriority;
    double iconRotation;

    ConstrIconBBVec boundingBoxes;
    while (!iconQueue.empty()) {
        IconQueue::iterator i = iconQueue.begin();

        labels.clear();
        labels.append(i->label);

        ids.clear();
        ids.push_back(i->constraintId);

        thisType = i->type;
        iconColor = constrColor(i->constraintId);
        labelColors.clear();
        labelColors.append(iconColor);
        iconRotation = i->iconRotation;

        maxColorPriority = constrColorPriority(i->constraintId);

        if (idString.length()) {
            idString.append(QStringLiteral(","));
        }
        idString.append(QString::number(i->constraintId));

        i = iconQueue.erase(i);
        while (i != iconQueue.end()) {
            if (i->type != thisType) {
                ++i;
                continue;
            }

            labels.append(i->label);
            ids.push_back(i->constraintId);
            labelColors.append(constrColor(i->constraintId));

            if (constrColorPriority(i->constraintId) > maxColorPriority) {
                maxColorPriority = constrColorPriority(i->constraintId);
                iconColor = constrColor(i->constraintId);
            }

            idString.append(QStringLiteral(",") + QString::number(i->constraintId));

            i = iconQueue.erase(i);
        }

        // To be inserted into edit->combinedConstBoxes
        std::vector<QRect> boundingBoxesVec;
        int oldHeight = 0;

        // Render the icon here.
        if (compositeIcon.isNull()) {
            compositeIcon = renderConstrIcon(
                thisType,
                iconColor,
                labels,
                labelColors,
                iconRotation,
                &boundingBoxesVec,
                &lastVPad
            );
        }
        else {
            int thisVPad;
            QImage partialIcon = renderConstrIcon(
                thisType,
                iconColor,
                labels,
                labelColors,
                iconRotation,
                &boundingBoxesVec,
                &thisVPad
            );

            // Stack vertically for now.  Down the road, it might make sense
            // to figure out the best orientation automatically.
            oldHeight = compositeIcon.height();

            // This is overkill for the currently used (20 July 2014) font,
            // since it always seems to have the same vertical pad, but this
            // might not always be the case.  The 3 pixel buffer might need
            // to vary depending on font size too...
            oldHeight -= std::max(lastVPad - 3, 0);

            compositeIcon = compositeIcon.copy(
                0,
                0,
                std::max(partialIcon.width(), compositeIcon.width()),
                partialIcon.height() + compositeIcon.height()
            );

            QPainter qp(&compositeIcon);
            qp.drawImage(0, oldHeight, partialIcon);

            lastVPad = thisVPad;
        }

        // Add bounding boxes for the icon we just rendered to boundingBoxes
        std::vector<int>::iterator id = ids.begin();
        std::set<int> nextIds;
        for (std::vector<QRect>::iterator bb = boundingBoxesVec.begin(); bb != boundingBoxesVec.end();
             ++bb) {
            nextIds.clear();

            if (bb == boundingBoxesVec.begin()) {
                // The first bounding box is for the icon at left, so assign
                // all IDs for that type of constraint to the icon.
                for (std::vector<int>::iterator j = ids.begin(); j != ids.end(); ++j) {
                    nextIds.insert(*j);
                }
            }
            else {
                nextIds.insert(*(id++));
            }

            ConstrIconBB newBB(bb->adjusted(0, oldHeight, 0, oldHeight), nextIds);

            boundingBoxes.push_back(newBB);
        }
    }

    combinedConstrBoxes[idString] = boundingBoxes;
    thisInfo->string.setValue(idString.toLatin1().data());
    sendConstraintIconToCoin(compositeIcon, thisDest);
}


/// Note: labels, labelColors, and boundingBoxes are all
/// assumed to be the same length.
QImage EditModeConstraintCoinManager::renderConstrIcon(
    const QString& type,
    const QColor& iconColor,
    const QStringList& labels,
    const QList<QColor>& labelColors,
    double iconRotation,
    std::vector<QRect>* boundingBoxes,
    int* vPad
)
{
    // Constants to help create constraint icons
    QString joinStr = QStringLiteral(", ");

    QPixmap pxMap;
    std::stringstream constraintName;
    constraintName << type.toLatin1().data()
                   << drawingParameters.constraintIconSize;  // allow resizing by embedding size
    if (!Gui::BitmapFactory().findPixmapInCache(constraintName.str().c_str(), pxMap)) {
        pxMap = Gui::BitmapFactory().pixmapFromSvg(
            type.toLatin1().data(),
            QSizeF(drawingParameters.constraintIconSize, drawingParameters.constraintIconSize)
        );
        Gui::BitmapFactory().addPixmapToCache(
            constraintName.str().c_str(),
            pxMap
        );  // Cache for speed, avoiding pixmapFromSvg
    }
    QImage icon = pxMap.toImage();
    // The pixmap was already scaled so we don't need to scale the image
    icon.setDevicePixelRatio(1.0f);

    QFont font = ViewProviderSketchCoinAttorney::getApplicationFont(viewProvider);
    font.setPixelSize(static_cast<int>(1.0 * drawingParameters.constraintIconSize));
    font.setBold(true);
    QFontMetrics qfm = QFontMetrics(font);

    int labelWidth = qfm.boundingRect(labels.join(joinStr)).width();
    // See Qt docs on qRect::bottom() for explanation of the +1
    int pxBelowBase = qfm.boundingRect(labels.join(joinStr)).bottom() + 1;

    if (vPad) {
        *vPad = pxBelowBase;
    }

    QTransform rotation;
    rotation.rotate(iconRotation);

    QImage roticon = icon.transformed(rotation);
    QImage image = roticon.copy(0, 0, roticon.width() + labelWidth, roticon.height() + pxBelowBase);

    // Make a bounding box for the icon
    if (boundingBoxes) {
        boundingBoxes->push_back(QRect(0, 0, roticon.width(), roticon.height()));
    }

    // Render the Icons
    QPainter qp(&image);
    qp.setCompositionMode(QPainter::CompositionMode_SourceIn);
    qp.fillRect(roticon.rect(), iconColor);

    // Render constraint label if necessary
    if (!labels.join(QString()).isEmpty()) {
        qp.setCompositionMode(QPainter::CompositionMode_SourceOver);
        qp.setFont(font);

        int cursorOffset = 0;

        // In Python: "for label, color in zip(labels, labelColors):"
        QStringList::const_iterator labelItr;
        QString labelStr;
        QList<QColor>::const_iterator colorItr;
        QRect labelBB;
        for (labelItr = labels.begin(), colorItr = labelColors.begin();
             labelItr != labels.end() && colorItr != labelColors.end();
             ++labelItr, ++colorItr) {

            qp.setPen(*colorItr);

            if (labelItr + 1 == labels.end()) {  // if this is the last label
                labelStr = *labelItr;
            }
            else {
                labelStr = *labelItr + joinStr;
            }

            // Note: text can sometimes draw to the left of the starting
            //       position, eg italic fonts.  Check QFontMetrics
            //       documentation for more info, but be mindful if the
            //       icon.width() is ever very small (or removed).
            qp.drawText(icon.width() + cursorOffset, icon.height(), labelStr);

            if (boundingBoxes) {
                labelBB = qfm.boundingRect(labelStr);
                labelBB.moveTo(icon.width() + cursorOffset, icon.height() - qfm.height() + pxBelowBase);
                boundingBoxes->push_back(labelBB);
            }

            cursorOffset += Gui::QtTools::horizontalAdvance(qfm, labelStr);
        }
    }

    return image;
}

void EditModeConstraintCoinManager::drawTypicalConstraintIcon(const constrIconQueueItem& i)
{
    QColor color = constrColor(i.constraintId);

    QImage image
        = renderConstrIcon(i.type, color, QStringList(i.label), QList<QColor>() << color, i.iconRotation);

    i.infoPtr->string.setValue(QString::number(i.constraintId).toLatin1().data());
    sendConstraintIconToCoin(image, i.destination);
}

QString EditModeConstraintCoinManager::iconTypeFromConstraint(Constraint* constraint)
{
    /*! TODO: Consider pushing this functionality up into Constraint
     *
     Abdullah: Please, don't. An icon is visualisation information and
     does not belong in App, but in Gui. Rather consider refactoring it
     in a separate class dealing with visualisation of constraints.*/

    switch (constraint->Type) {
        case Horizontal:
            return QStringLiteral("Constraint_Horizontal");
        case Vertical:
            return QStringLiteral("Constraint_Vertical");
        case PointOnObject:
            return QStringLiteral("Constraint_PointOnObject");
        case Tangent:
            return QStringLiteral("Constraint_Tangent");
        case Parallel:
            return QStringLiteral("Constraint_Parallel");
        case Perpendicular:
            return QStringLiteral("Constraint_Perpendicular");
        case Equal:
            return QStringLiteral("Constraint_EqualLength");
        case Symmetric:
            return QStringLiteral("Constraint_Symmetric");
        case SnellsLaw:
            return QStringLiteral("Constraint_SnellsLaw");
        case Block:
            return QStringLiteral("Constraint_Block");
        default:
            return QString();
    }
}

void EditModeConstraintCoinManager::sendConstraintIconToCoin(const QImage& icon, SoImage* soImagePtr)
{
    SoSFImage icondata = SoSFImage();

    Gui::BitmapFactory().convert(icon, icondata);

    SbVec2s iconSize(icon.width(), icon.height());

    int four = 4;
    soImagePtr->image.setValue(iconSize, 4, icondata.getValue(iconSize, four));

    // Set Image Alignment to Center
    soImagePtr->vertAlignment = SoImage::HALF;
    soImagePtr->horAlignment = SoImage::CENTER;
}

void EditModeConstraintCoinManager::clearCoinImage(SoImage* soImagePtr)
{
    soImagePtr->setToDefaults();
}

QColor EditModeConstraintCoinManager::constrColor(int constraintId)
{
    auto toQColor = [](auto sbcolor) -> QColor {
        return QColor((int)(sbcolor[0] * 255.0f), (int)(sbcolor[1] * 255.0f), (int)(sbcolor[2] * 255.0f));
    };

    const auto constraints = ViewProviderSketchCoinAttorney::getConstraints(viewProvider);

    if (ViewProviderSketchCoinAttorney::isConstraintPreselected(viewProvider, constraintId)) {
        return toQColor(drawingParameters.PreselectColor);
    }
    else if (ViewProviderSketchCoinAttorney::isConstraintSelected(viewProvider, constraintId)) {
        return toQColor(drawingParameters.SelectColor);
    }
    else if (!constraints[constraintId]->isActive) {
        return toQColor(drawingParameters.DeactivatedConstrDimColor);
    }
    else if (!constraints[constraintId]->isDriving) {
        return toQColor(drawingParameters.NonDrivingConstrDimColor);
    }
    else {
        return toQColor(drawingParameters.ConstrIcoColor);
    }
}

int EditModeConstraintCoinManager::constrColorPriority(int constraintId)
{
    if (ViewProviderSketchCoinAttorney::isConstraintPreselected(viewProvider, constraintId)) {
        return 3;
    }
    else if (ViewProviderSketchCoinAttorney::isConstraintSelected(viewProvider, constraintId)) {
        return 2;
    }
    else {
        return 1;
    }
}

SoSeparator* EditModeConstraintCoinManager::getConstraintIdSeparator(int i)
{
    return dynamic_cast<SoSeparator*>(editModeScenegraphNodes.constrGroup->getChild(i));
}

void EditModeConstraintCoinManager::createEditModeInventorNodes()
{
    // group node for the Constraint visual +++++++++++++++++++++++++++++++++++
    SoMaterialBinding* MtlBind = new SoMaterialBinding;
    MtlBind->setName("ConstraintMaterialBinding");
    MtlBind->value = SoMaterialBinding::OVERALL;
    editModeScenegraphNodes.EditRoot->addChild(MtlBind);

    // use small line width for the Constraints
    editModeScenegraphNodes.ConstraintDrawStyle = new SoDrawStyle;
    editModeScenegraphNodes.ConstraintDrawStyle->setName("ConstraintDrawStyle");
    editModeScenegraphNodes.ConstraintDrawStyle->lineWidth = 1 * drawingParameters.pixelScalingFactor;
    editModeScenegraphNodes.EditRoot->addChild(editModeScenegraphNodes.ConstraintDrawStyle);

    // add the group where all the constraints has its SoSeparator
    editModeScenegraphNodes.constrGrpSelect = new SoPickStyle();  // used to toggle constraints
                                                                  // selectability
    editModeScenegraphNodes.constrGrpSelect->style.setValue(SoPickStyle::SHAPE);
    editModeScenegraphNodes.EditRoot->addChild(editModeScenegraphNodes.constrGrpSelect);
    setConstraintSelectability();  // Ensure default value;

    // disable depth testing for constraint icons so they render ON TOP of geometry lines
    // check issues #25840 and #11603
    SoDepthBuffer* constrDepthOff = new SoDepthBuffer();
    constrDepthOff->test.setValue(false);
    editModeScenegraphNodes.EditRoot->addChild(constrDepthOff);

    editModeScenegraphNodes.constrGroup = new SmSwitchboard();
    editModeScenegraphNodes.constrGroup->setName("ConstraintGroup");
    editModeScenegraphNodes.EditRoot->addChild(editModeScenegraphNodes.constrGroup);

    // re-enable depth testing for the rest of the nodes
    SoDepthBuffer* constrDepthOn = new SoDepthBuffer();
    constrDepthOn->test.setValue(true);
    editModeScenegraphNodes.EditRoot->addChild(constrDepthOn);

    SoPickStyle* ps = new SoPickStyle();  // used to following nodes aren't impacted
    ps->style.setValue(SoPickStyle::SHAPE);
    editModeScenegraphNodes.EditRoot->addChild(ps);
}