/////////////////////////////////////////////////////////////////////////////// 
// Copyright (C) 2002-2025, Open Design Alliance (the "Alliance"). 
// All rights reserved. 
// 
// This software and its documentation and related materials are owned by 
// the Alliance. The software may only be incorporated into application 
// programs owned by members of the Alliance, subject to a signed 
// Membership Agreement and Supplemental Software License Agreement with the
// Alliance. The structure and organization of this software are the valuable  
// trade secrets of the Alliance and its suppliers. The software is also 
// protected by copyright law and international treaty provisions. Application  
// programs incorporating this software must include the following statement 
// with their copyright notices:
//   
//   This application incorporates Open Design Alliance software pursuant to a license 
//   agreement with Open Design Alliance.
//   Open Design Alliance Copyright (C) 2002-2025 by Open Design Alliance. 
//   All rights reserved.
//
// By use of this software, its documentation or related materials, you 
// acknowledge and accept the above terms.
///////////////////////////////////////////////////////////////////////////////
#include "OdaCommon.h"
#include "DbCommandContext.h"
#include "Entities.h"
#include "DbBlockTableRecord.h"

#include "DbAssocNetwork.h"

#include "DWGCommandsUtils.h"
#include "DbAssocActionBody.h"
#include "OdDbAssocPersSubentIdPE.h"
#include "DbEvalGraph.h"
#include "OdDbAssocCurvePersSubentIdPE.h"
#include "DbAssoc2dConstraintGroup.h"
#include "DbObjectIterator.h"
#define STL_USING_MEMORY
#include "OdaSTL.h"

#include <algorithm>

namespace DWGConstraintUtils
{

  DwgConstraintEvaluationCallback::DwgConstraintEvaluationCallback()
  {
    _evalContext = OdRxObjectImpl<OdDbEvalContext>::createObject();
  }

  DwgConstraintEvaluationCallback::~DwgConstraintEvaluationCallback()
  {
  }

  OdDbEvalContext* DwgConstraintEvaluationCallback::getAdditionalData() const
  {
    if (_evalContext.isNull())
      return NULL;

    if (!_fixedGeom.isEmpty())
    {
      const OdString key = OD_T("TempFixedConsGeoms");
      OdDbEvalContextPair pair(key, (void*)(&_fixedGeom));
      _evalContext->insertAt(pair);
    }

    if (!_fixedRadius.isEmpty())
    {
      const OdString key = OD_T("TempFixedRadius");
      OdDbEvalContextPair pair(key, (void*)(&_fixedRadius));
      _evalContext->insertAt(pair);
    }

    if (!_fixedAngle.isEmpty())
    {
      const OdString key = OD_T("TempFixedAngle");
      OdDbEvalContextPair pair(key, (void*)(&_fixedAngle));
      _evalContext->insertAt(pair);
    }

    if (!_fixedDistance.isEmpty())
    {
      const OdString key = OD_T("TempFixedDistance");
      OdDbEvalContextPair pair(key, (void*)(&_fixedDistance));
      _evalContext->insertAt(pair);
    }

    {
      const OdString key = OD_T("AllowOverconstrain");
      OdDbEvalContextPair pair(key, (void*)(&_allowOverconstrain));
      _evalContext->insertAt(pair);
    }

    return _evalContext.get();
  }

  OdDbObjectId currentSpaceId(const OdDbDatabase* pDb)
  {
    return pDb->currentSpaceId();
    /*
    OdDbObjectId idSpace = pDb->getActiveLayoutBTRId();
    if (idSpace == pDb->getPaperSpaceId())
    {
      OdDbViewportPtr pVp = pDb->activeViewportId().safeOpenObject();
      if (pVp->number() > 1)
        idSpace = pDb->getModelSpaceId();
    }
    return idSpace;
    */
  }

  void currentUCS(const OdDbObjectId& blockId, OdGePlane* plane, OdGeMatrix3d* matrix)
  {
    OdDbDatabase* pDb = blockId.database();
    OdGePoint3d org;
    OdGeVector3d xAxis, yAxis;
    if (blockId == pDb->getModelSpaceId())
    {
      org = pDb->getUCSORG();
      xAxis = pDb->getUCSXDIR();
      yAxis = pDb->getUCSYDIR();
    }
    else if (blockId == pDb->getPaperSpaceId())
    {
      org = pDb->getPUCSORG();
      xAxis = pDb->getPUCSXDIR();
      yAxis = pDb->getPUCSYDIR();
    }
    else
    {
      xAxis = OdGeVector3d::kXAxis;
      yAxis = OdGeVector3d::kYAxis;
    }
    if (plane)
    {
      plane->set(org, xAxis, yAxis);
    }
    if (matrix)
    {
      OdGeVector3d zAxis = xAxis.crossProduct(yAxis).normalize();
      matrix->setCoordSystem(org, xAxis, yAxis, zAxis);
    }
  }

  OdDbObjectId getConstraintGroup(const OdDbObjectId& spaceId, bool createIfDoesNotExist)
  {
    OdDbObjectId networkId = OdDbAssocNetwork::getInstanceFromObject(spaceId, createIfDoesNotExist);

    if (networkId.isNull())
    {
      return OdDbObjectId::kNull;
    }

    OdDbDatabase* pDb = spaceId.database();

    // Try to find the constraint group in the associative network
    //if (idConstrGroup.isNull() && createIfDoesNotExist)
    {
      OdDbAssocNetworkPtr pNetwork = networkId.openObject(OdDb::kForRead);

      if (pNetwork.isNull())
      {
        return OdDbObjectId::kNull;
      }

      const OdDbObjectIdArray actionsInNetwork = pNetwork->getActions();

      for (unsigned int i = 0; i < actionsInNetwork.logicalLength(); ++i)
      {
        const OdDbObjectId& idAction = actionsInNetwork[i];

        if (idAction == OdDbObjectId::kNull)
        {
          continue;
        }

        if (actionsInNetwork[i].objectClass() == NULL ||
          !actionsInNetwork[i].objectClass()->isDerivedFrom(OdDbAssoc2dConstraintGroup::desc()))
        {
          continue;
        }

        const OdDbAssoc2dConstraintGroupPtr pConstGrp = idAction.openObject(OdDb::kForRead);

        if (pConstGrp.isNull())
        {
          continue;
        }

        OdGePlane currentPlane;
        currentUCS(spaceId, &currentPlane);

        if (currentPlane.isCoplanarTo(pConstGrp->getWorkPlane()))
        {
          // We have an constraint group which is built on same UCS
          return idAction;
        }
      }
    }

    OdDbObjectId idConstrGroup = OdDbObjectId::kNull;
    if (createIfDoesNotExist)
    {
      OdDbAssocNetworkPtr pNetwork = networkId.openObject(OdDb::kForWrite);

      if (pNetwork.isNull())
      {
        return OdDbObjectId::kNull;
      }

      OdGePlane currentPlane;
      currentUCS(spaceId, &currentPlane);

      OdDbAssoc2dConstraintGroupPtr pConstrGroup = OdDbAssoc2dConstraintGroup::createObject();
      pConstrGroup->setWorkPlane(currentPlane);

      idConstrGroup = pDb->addOdDbObject(pConstrGroup);
      if (idConstrGroup.isNull())
      {
        return OdDbObjectId::kNull;
      }

      //pConstrGroup->close();

      if (pNetwork->addAction(idConstrGroup, true) != eOk)
      {
        return OdDbObjectId::kNull;
      }
    }

    return idConstrGroup;
  }

  OdResult addConstrainedGeometry(OdDbDatabase* pDatabase, OdDbFullSubentPathArray& aPaths, OdArray<OdConstrainedGeometry*>& pConsGeoms)
  {
    OdDbFullSubentPathArray aPathsReqMIdPoint;
    return addConstrainedGeometry(pDatabase, aPaths, aPathsReqMIdPoint, pConsGeoms);
  }

  OdResult addConstrainedGeometry(OdDbDatabase* pDatabase,
    OdDbFullSubentPathArray& aPaths,
    const OdDbFullSubentPathArray& aPathsReqMIdPoint,
    OdArray<OdConstrainedGeometry*>& pConsGeoms)
  {
    OdDbObjectId consGrpId = getConstraintGroup(currentSpaceId(pDatabase), true);

    if (consGrpId.isNull())
    {
      return eInvalidInput;
    }

    OdDbAssoc2dConstraintGroupPtr p2dConstrGrp = consGrpId.openObject(OdDb::kForWrite);

    // Could not open constraint group for updates.
    if (p2dConstrGrp.isNull())
    {
      return eInvalidInput;
    }

    //Add the relevent subentities as the constrained geometry
    for (unsigned int i = 0; i < aPaths.logicalLength(); ++i)
    {
      OdConstrainedGeometry* pConsGeom = NULL;

      OdResult es = p2dConstrGrp->addConstrainedGeometry(aPaths[i], pConsGeom);
      if (aPathsReqMIdPoint.contains(aPaths[i]) && pConsGeom->isKindOf(OdConstrainedCurve::desc()))
      {
        OdConstrainedCurve* pCurve = OdConstrainedCurve::cast(pConsGeom);

        OdArray<OdConstrainedImplicitPoint*> apImplicitPoints;
        pCurve->getConstrainedImplicitPoints(apImplicitPoints);
        bool midPointExists = false;
        for (OdConstrainedImplicitPoint* pt : apImplicitPoints)
        {
          if (pt->pointType() == OdConstrainedImplicitPoint::kMidImplicit)
          {
            midPointExists = true;
            break;
          }
        }

        if (!midPointExists)
        {
          p2dConstrGrp->addImplicitPoint(pCurve, OdConstrainedImplicitPoint::kMidImplicit, -1);
        }
      }

      if (!((es == eAlreadyInGroup) || (es == eOk)))
        return es;

      pConsGeoms.append(pConsGeom);
    }

    return eOk;
  }

  OdResult getConstrainedGeometry(OdDbDatabase* pDatabase, OdDbFullSubentPath& path, OdConstrainedGeometry*& pConsGeom)
  {
    OdDbObjectId consGrpId = getConstraintGroup(currentSpaceId(pDatabase), false);

    if (consGrpId.isNull() || path.objectIds().isEmpty())
    {
      return eInvalidInput;
    }

    OdDbAssoc2dConstraintGroupPtr p2dConstrGrp = consGrpId.openObject(OdDb::kForWrite);

    // Could not open constraint group for updates.
    if (p2dConstrGrp.isNull())
    {
      return eInvalidInput;
    }

    return p2dConstrGrp->getConstrainedGeometry(path, pConsGeom);
  }

  OdResult getFullSubentPaths(const OdDbEntity* pEnt, OdDb::SubentType subentType, OdDbFullSubentPathArray& aPaths)
  {
    OdResult es;

    // Get the Protocol extension associated with the entity
    OdDbAssocPersSubentIdPE* const pAssocPersSubentIdPE = OdDbAssocPersSubentIdPE::cast(pEnt->queryX(OdDbAssocPersSubentIdPE::desc()));
    if (!pAssocPersSubentIdPE)
      return eNullObjectPointer;

    OdArray<OdDbSubentId> subentIds;

    // Get all the subentities associated with the subentity type such as the vertices or edges
    if ((es = pAssocPersSubentIdPE->getAllSubentities(pEnt, subentType, subentIds)) != eOk)
    {
      return es;
    }

    //Get subent path and put them into aPaths collection
    for (unsigned int i = 0; i < subentIds.logicalLength(); ++i)
    {
      aPaths.append(OdDbFullSubentPath(pEnt->objectId(), subentIds[i]));
    }

    return es;
  }

  OdResult getClosestVertexPosition(const OdDbEntityPtr pEnt, const OdGePoint3d& entPt,
    const OdGePoint3d& ptStart, const OdGePoint3d& ptEnd,
    OdGePoint3d& clossestPoint, bool& createImplicitMidPoint)
  {
    OdResult res = eOk;
    OdGePoint3dArray points;
    points.append(ptStart);
    points.append(ptEnd);

    OdDbEntityPtr realEnt;
    if (pEnt->isKindOf(OdDbBlockReference::desc()))
    {
      res = getClosestNestedEntity(OdDbBlockReference::cast(pEnt), entPt, realEnt);
      if (res != eOk)
        return res;
    }
    else
    {
      realEnt = pEnt;
    }

    OdGePoint3d midPoint;
    if (realEnt->isKindOf(OdDbArc::desc()) || realEnt->isKindOf(OdDbLine::desc()))
    {
      if ((res = getMidPoint(realEnt, midPoint)) != eOk)
        return res;
      points.append(midPoint);
    }
    else if (realEnt->isKindOf(OdDbEllipse::desc()))
    {
      points.append(OdDbEllipse::cast(realEnt)->center());
    }

    OdGePoint3dArray::iterator itr = std::min_element(std::begin(points), std::end(points),
      [&entPt](const OdGePoint3d& p1, const OdGePoint3d& p2) {
        return (p1.distanceTo(entPt) < p2.distanceTo(entPt));
      });

    if (itr == std::end(points))
      return eInvalidIntervals;

    clossestPoint = *itr;

    if (pEnt->isKindOf(OdDbArc::desc()) || pEnt->isKindOf(OdDbLine::desc()))
    {
      if (clossestPoint == midPoint)
        createImplicitMidPoint = true;
    }

    return eOk;
  }

  OdResult getClosestEdgeSubEntPath(const OdDbObjectId& entId, const OdGePoint3d& entPt, OdDbFullSubentPath& closestPath,
    double* pMinDist, OdGePoint3d* pClosestPt,
    OdDbFullSubentPath* closestEndPath, OdDbFullSubentPath* pathPtBeg, OdDbFullSubentPath* pathPtEnd,
    OdGePoint3d* closestEndPosition)
  {
    bool createImplicitMidPoint = false;
    return getClosestEdgeSubEntPath(entId, entPt, closestPath, createImplicitMidPoint,
      pMinDist, pClosestPt, closestEndPath, pathPtBeg, pathPtEnd, closestEndPosition);
  }

  OdResult getClosestEdgeSubEntPath(const OdDbObjectId& entId, const OdGePoint3d& entPt, OdDbFullSubentPath& closestPath,
    bool& createImplicitMidPoint,
    double* pMinDist /*= NULL*/,
    OdGePoint3d* pClosestPt /*= NULL*/,
    OdDbFullSubentPath* closestEndPath /*= NULL*/,
    OdDbFullSubentPath* pathPtBeg /*= NULL*/,
    OdDbFullSubentPath* pathPtEnd /*= NULL*/,
    OdGePoint3d* closestEndPosition /*= NULL*/)
  {
    createImplicitMidPoint = false;
    OdResult es = eOk;
   
    OdDbEntityPtr pEnt = entId.openObject(OdDb::kForRead);
    if (pEnt->isKindOf(OdDbBlockReference::desc()))
    {
      es = getClosestNestedEntity(OdDbBlockReference::cast(pEnt), entPt, pEnt, pMinDist);
      if (es != eOk)
        return es;
    }

    if (pEnt.isNull())
    {
      return eInvalidInput;
    }

    // Get the Protocol extension associated with the entity
    OdDbAssocPersSubentIdPE* pAssocPersSubentIdPE = OdDbAssocPersSubentIdPE::cast(pEnt->queryX(OdDbAssocPersSubentIdPE::desc()));
    if (!pAssocPersSubentIdPE)
      return eNullObjectPointer;

    OdDbFullSubentPathArray aPathsTemp;
    getFullSubentPaths(pEnt, OdDb::kEdgeSubentType, aPathsTemp);

    if (aPathsTemp.logicalLength() == 0)
    {
      // Text, Multiline text or Point
      //return eInvalidInput;
      return eOk;
    }

    double minDist = -1.0;

    for (unsigned int i = 0; i < aPathsTemp.logicalLength(); ++i)
    {
      OdGeCurve3d* pEdgeCurve = nullptr;
      if ((es = pAssocPersSubentIdPE->getEdgeSubentityGeometry(pEnt, aPathsTemp[i].subentId(), pEdgeCurve)) != eOk)
      {
        return es;
      }

      std::unique_ptr<OdGeCurve3d> edgeCurve(pEdgeCurve);
      double dist = edgeCurve->distanceTo(entPt);

      if (dist < minDist || minDist < 0.0)
      {
        minDist = dist;
        closestPath = aPathsTemp[i];

        if (pClosestPt)
        {
          *pClosestPt = edgeCurve->closestPointTo(entPt);
        }

        if (closestEndPath || pathPtBeg || pathPtEnd || closestEndPosition)
        {
          OdDbSubentId startVertexSubentId;
          OdDbSubentId endVertexSubentId;
          OdArray<OdDbSubentId> otherVertexSubentIds;
          if ((es = pAssocPersSubentIdPE->getEdgeVertexSubentities(pEnt, aPathsTemp[i].subentId(), startVertexSubentId, endVertexSubentId, otherVertexSubentIds)) != eOk)
          {
            return es; //TODO: fix getEdgeVertexSubentities for LineSeg
            //es = eOk;
          }

          OdGePoint3d ptStart, ptEnd;
          if (pAssocPersSubentIdPE->getVertexSubentityGeometry(pEnt, startVertexSubentId, ptStart) == eOk)
          {
            if (pEnt->isKindOf(OdDbSpline::desc()) && startVertexSubentId.index() == OdDbAssocCurvePersSubentIdPE::kStart)
              startVertexSubentId.setIndex(0);

            if (pathPtBeg)
            {
              *pathPtBeg = OdDbFullSubentPath(entId, startVertexSubentId);
            }

            if (pAssocPersSubentIdPE->getVertexSubentityGeometry(pEnt, endVertexSubentId, ptEnd) == eOk)
            {
              if (pEnt->isKindOf(OdDbSpline::desc()) && endVertexSubentId.index() == OdDbAssocCurvePersSubentIdPE::kEnd)
                endVertexSubentId.setIndex(OdDbSpline::cast(pEnt)->numControlPoints() - 1);

              if (closestEndPath)
              {
                OdGePoint3d position;
                if ((es = getClosestVertexPosition(pEnt, entPt, ptStart, ptEnd, position, createImplicitMidPoint)) != eOk)
                  return es;

                if (ptStart != position && ptEnd != position && (pEnt->isKindOf(OdDbArc::desc()) || pEnt->isKindOf(OdDbLine::desc())))
                  *closestEndPath = OdDbFullSubentPath(entId, OdDb::kVertexSubentType, OdDbAssocCurvePersSubentIdPE::kMiddle);
                else
                  *closestEndPath = OdDbFullSubentPath(entId,
                    ptStart.distanceSqrdTo(entPt) < ptEnd.distanceSqrdTo(entPt) ? startVertexSubentId : endVertexSubentId);
              }
              if (closestEndPosition)
              {
                if ((es = getClosestVertexPosition(pEnt, entPt, ptStart, ptEnd, *closestEndPosition, createImplicitMidPoint)) != eOk)
                  return es;
              }
              if (pathPtEnd)
              {
                *pathPtEnd = OdDbFullSubentPath(entId, endVertexSubentId);
              }
            }
            else
            {
              if (closestEndPath)
              {
                *closestEndPath = OdDbFullSubentPath(entId, startVertexSubentId);
              }
              if (closestEndPosition)
              {
                *closestEndPosition = ptStart;
              }
            }
          }
        }
      }

    }

    // Need output min minDistance for 
    // AdnAssocSampleUtils::getClosestEdgeSubEntPathBref
    if (pMinDist)
    {
      *pMinDist = minDist;
    }

    return es;
  }

  OdResult getClosestAxis(const OdDbObjectId& entId, const OdGePoint3d& entPt, OdDbFullSubentPath& axisEntPath)
  {
    OdDbEntityPtr pEntity = entId.openObject(OdDb::kForRead);

    if ( pEntity.isNull() )
      return eInvalidInput;

    OdGsMarker clossesAxisIndex;
    if (pEntity->isKindOf(OdDbEllipse::desc()))
    {
      OdDbEllipsePtr pEllipse = OdDbEllipse::cast(pEntity);

      OdGeCurve3d *pCurve;
      if ( (pEllipse->getOdGeCurve(pCurve)) != eOk )
        return eInvalidInput;

      OdGeLineSeg3d majorAxis(pCurve->evalPoint(0), pCurve->evalPoint(OdaPI));
      OdGeLineSeg3d minorAxis(pCurve->evalPoint(OdaPI2), pCurve->evalPoint(OdaPI + OdaPI2));
      delete pCurve;
      double distanceToMajorAxis = majorAxis.distanceTo(entPt);
      double distanceToMinorAxis = minorAxis.distanceTo(entPt);

      clossesAxisIndex = distanceToMajorAxis <= distanceToMinorAxis ? kMajorAxis : kMinorAxis;
    }
    else if (pEntity->isKindOf(OdDbText::desc()) || pEntity->isKindOf(OdDbMText::desc()))
    {
      // Text objects has only one axis
      clossesAxisIndex = kMajorAxis;
    }
    else
    {
      return eInvalidInput;
    }

    OdDbSubentId clossesAxisId(OdDb::kAxisSubentType, clossesAxisIndex);
    axisEntPath = OdDbFullSubentPath(entId, clossesAxisId);
    return eOk;
  }

  OdResult getCenterVertexPath(const OdDbObjectId& entId, OdDbFullSubentPath &vertEntPath)
  {
      OdDbFullSubentPathArray aPaths;
      OdDbEntityPtr pEnt = entId.openObject(OdDb::kForRead);

      OdResult es = eOk;
      if ((es = DWGConstraintUtils::getFullSubentPaths(pEnt, OdDb::kVertexSubentType, aPaths)) != eOk)
          return es;

      if ( aPaths.length() != 1 )
          return eBadObjType;

      vertEntPath = aPaths[0];
      return eOk;
  }

  OdResult getClosestVertexPath(const OdDbObjectId& entId, const OdGePoint3d point, OdDbFullSubentPath& vertEntPath)
  {
    OdDbEntityPtr pEnt = entId.openObject(OdDb::kForRead);
    return getClosestVertexPath(pEnt, point, vertEntPath);
  }

  OdResult getClosestVertexPath(const OdDbEntityPtr pEnt, const OdGePoint3d point, OdDbFullSubentPath& vertEntPath)
  {
    OdDbFullSubentPathArray aPaths;
    OdResult es = eOk;
    if ((es = DWGConstraintUtils::getFullSubentPaths(pEnt, OdDb::kVertexSubentType, aPaths)) != eOk)
      return es;

    if (aPaths.length() < 1)
      return eBadObjType;

    OdDbAssocPersSubentIdPE* pAssocPersSubentIdPE = OdDbAssocPersSubentIdPE::cast(pEnt->queryX(OdDbAssocPersSubentIdPE::desc()));
    if (!pAssocPersSubentIdPE)
      return eNullObjectPointer;

    OdGePoint3d vertexPosition;
    vertEntPath = aPaths[0];
    if ((es = pAssocPersSubentIdPE->getVertexSubentityGeometry(pEnt, vertEntPath.subentId(), vertexPosition)) != eOk)
      return es;

    double minDistance = point.distanceTo(vertexPosition);
    double distance;

    for (const OdDbFullSubentPath& subentPath : aPaths)
    {
      if ((es = pAssocPersSubentIdPE->getVertexSubentityGeometry(pEnt, subentPath.subentId(), vertexPosition)) != eOk)
        return es;

      if ((distance = point.distanceTo(vertexPosition)) < minDistance)
      {
        vertEntPath = subentPath;
        minDistance = distance;
      }
    }

    return es;
  }

  OdResult getEdgeSubEntPathByVertexPath(const OdDbEntityPtr entity, const OdDbFullSubentPath& vertexPath, OdDbFullSubentPathArray& edgePaths)
  {
    OdResult res = eOk;
    OdDbFullSubentPathArray edgePathsTemp;
    res = getFullSubentPaths(entity, OdDb::kEdgeSubentType, edgePathsTemp);
    if (res != eOk)
      return res;

    OdGsMarker vertexIndex = vertexPath.subentId().index();

    if (vertexIndex - edgePathsTemp.last().subentId().index() == 1) // Last vertex
    {
      edgePaths.push_back(edgePathsTemp.last());
      return res;
    }

    for (const OdDbFullSubentPath& edgePath : edgePathsTemp)
    {
      if (edgePath.subentId().index() == vertexIndex)
      {
        edgePaths.push_back(edgePath);
        break;
      }
    }

    return res;
  }

  bool isNeighboringVertices(const OdDbFullSubentPath& vertPath1, const OdDbFullSubentPath& vertPath2)
  {
    ODA_ASSERT_ONCE(vertPath1.subentId().type() == OdDb::kVertexSubentType && vertPath2.subentId().type() == OdDb::kVertexSubentType);
    ODA_ASSERT_ONCE(!(vertPath1 == vertPath2)); // operator != doesn't declared

    OdDbObjectIdArray objIds1 = vertPath1.objectIds();
    OdDbObjectIdArray objIds2 = vertPath2.objectIds();
    ODA_ASSERT_ONCE(objIds1.logicalLength() != 0 && objIds2.logicalLength() != 0);

    if (objIds1[0] != objIds2[0]) // Different entities
      return false;

    auto indexDistance = std::abs(vertPath1.subentId().index() - vertPath2.subentId().index());
    if (indexDistance == 1)
      return true;

    return false;
  }

  OdResult addGeomConstraint(OdDbDatabase* pDatabase, const OdGeomConstraint::GeomConstraintType& constraintType,
    OdDbFullSubentPathArray& aPaths, DwgConstraintEvaluationCallback* callback)
  {
    OdResult es;

    // Get the 2D constraint Group
    const OdString dictionaryKey(ASSOCNETWORK_DICTIONARYKEY);

    OdDbObjectId consGrpId = getConstraintGroup(currentSpaceId(pDatabase), true);

    if (consGrpId.isNull())
    {
      return eNullObjectId;
    }

    OdDbAssoc2dConstraintGroupPtr p2dConstrGrp = consGrpId.openObject(OdDb::kForWrite);

    // Add the constraint to the selected entities
    OdGeomConstraint* ppNewConstraint = NULL;

    if ((es = p2dConstrGrp->addGeometricalConstraint(constraintType, aPaths, &ppNewConstraint)) != eOk)
    {
      return es;
    }

    OdDbObjectId idNetwork = OdDbAssocNetwork::getInstanceFromDatabase(pDatabase, true, dictionaryKey);

    if (idNetwork.isNull())
    {
      return eInvalidInput;
    }

    OdDbAssocNetworkPtr pNetwork = idNetwork.openObject(OdDb::kForWrite);

    if (pNetwork.isNull())
    {
      return eInvalidInput;
    }

    DwgConstraintEvaluationCallback tmpCallback;
    if (!callback)
    {
      callback = &tmpCallback;
    }
    callback->_allowOverconstrain = false;

    pNetwork->evaluate(callback);

    if (p2dConstrGrp->status() == kFailedToEvaluateAssocStatus)
    {
      OdResult res = p2dConstrGrp->deleteConstraint(ppNewConstraint);
      ODA_VERIFY_ONCE(res == eOk);
      return eInvalidInput;
    }

    return es;
  }

  OdGeCurve3d* getCurveForEdgeSubentity(const OdDbObjectId& entId, const OdDbFullSubentPath& edgeSubent)
  {
    OdDbEntityPtr pEntity = entId.openObject(OdDb::kForRead);

    if (pEntity.isNull())
    {
      return NULL;
    }

    OdDbAssocPersSubentIdPE* const pAssocPersSubentIdPE = OdDbAssocPersSubentIdPE::cast(pEntity->queryX(OdDbAssocPersSubentIdPE::desc()));
    if (!pAssocPersSubentIdPE)
      return NULL;

    OdGeCurve3d* pCurve = NULL;

    if (pAssocPersSubentIdPE->getEdgeSubentityGeometry(pEntity.get(), edgeSubent.subentId(), pCurve) != eOk)
    {
      return NULL;
    }

    return pCurve;
  }

  OdGeMatrix3d getConstraintGroupCoordSysInv(OdDbAssoc2dConstraintGroup* p2dConstrGrp)
  {
    OdGePlane plane = p2dConstrGrp->getWorkPlane();

    OdGePoint3d origin;
    OdGeVector3d xAxis, yAxis;
    plane.getCoordSystem(origin, xAxis, yAxis);

    OdGeMatrix3d coordSys;
    coordSys.setCoordSystem(origin, xAxis, yAxis, plane.normal());

    return coordSys.inverse();
  }

  OdResult getClosestConstrainedPoint(OdConstrainedGeometry*& pConsGeom, const OdGePoint3d& entPtInConstGrp, OdConstrainedPoint*& point)
  {
    if (!pConsGeom->isKindOf(OdConstrainedCurve::desc()))
      return eInvalidInput;

    OdConstrainedCurve* pConstrainedCurve = OdConstrainedCurve::cast(pConsGeom);

    OdArray<OdConstrainedImplicitPoint*> apImplicitPoints;

    pConstrainedCurve->getConstrainedImplicitPoints(apImplicitPoints);

    point = apImplicitPoints[0];

    double distMin = entPtInConstGrp.distanceTo(apImplicitPoints[0]->point());

    for (unsigned int i = 1; i < apImplicitPoints.logicalLength(); ++i)
    {
      double dist = entPtInConstGrp.distanceTo(apImplicitPoints[i]->point());

      if (dist < distMin)
      {
        distMin = dist;
        point = apImplicitPoints[i];
      }
    }

    return eOk;
  }

  OdResult getLinearEdgeData(const OdDbObjectId& entId, const OdGePoint3d& entPt, OdDbFullSubentPath& edgeEntPath,
    OdGePoint3d& startPoint, OdGePoint3d& endPoint)
  {
    OdResult es;
    bool createImplicitMidPoint = false;

    if ((es = getClosestEdgeSubEntPath(entId, entPt, edgeEntPath, createImplicitMidPoint)) != eOk)
      return es;

    OdDbEntityPtr pEntity = entId.safeOpenObject(OdDb::kForRead);

    if (pEntity.isNull())
      return eInvalidInput;

    OdDbAssocPersSubentIdPE* const pAssocPersSubentIdPE =
      OdDbAssocPersSubentIdPE::cast(pEntity->queryX(OdDbAssocPersSubentIdPE::desc()));
    if (!pAssocPersSubentIdPE)
      return eNullObjectPointer;

    OdGeCurve3d* pSegment = nullptr;
    if ((es = pAssocPersSubentIdPE->getEdgeSubentityGeometry(pEntity, edgeEntPath.subentId(), pSegment)) != eOk)
      return es;

    std::unique_ptr<OdGeCurve3d> segment(pSegment);
    
    if (!(segment->type() == OdGe::kLineSeg3d || segment->type() == OdGe::kLinearEnt3d))
      return eInvalidInput;

    if (!segment->hasStartPoint(startPoint))
    {
      return eInvalidInput;
    }

    if (!segment->hasEndPoint(endPoint))
    {
      return eInvalidInput;
    }

    return eOk;
  }

  bool isText(const OdDbObjectId& entId)
  {
    OdDbEntityPtr pEnt = entId.openObject(OdDb::kForRead);
    bool retVal = !pEnt.isNull() && (pEnt->isKindOf(OdDbMText::desc()) || pEnt->isKindOf(OdDbText::desc()));
    return retVal;
  }

  bool isPoint(const OdDbObjectId& entId)
  {
    OdDbEntityPtr pEnt = entId.openObject(OdDb::kForRead);
    bool retVal = !pEnt.isNull() && pEnt->isKindOf(OdDbPoint::desc());
    return retVal;
  }

  bool isEllipse(const OdDbObjectId& entId)
  {
    OdDbEntityPtr pEnt = entId.openObject(OdDb::kForRead);
    bool retVal = !pEnt.isNull() && pEnt->isKindOf(OdDbEllipse::desc());
    return retVal;
  }

  bool isCircle(const OdDbObjectId& entId)
  {
    OdDbEntityPtr pEnt = entId.openObject(OdDb::kForRead);
    bool retVal = !pEnt.isNull() && pEnt->isKindOf(OdDbCircle::desc());
    return retVal;
  }

  bool isBlockReference(const OdDbObjectId& entId)
  {
    OdDbEntityPtr pEnt = entId.openObject(OdDb::kForRead);
    bool retVal = !pEnt.isNull() && pEnt->isKindOf(OdDbBlockReference::desc());
    return retVal;
  }

  void addHintDistanceFixation(OdDbDatabasePtr pDatabase, OdDbFullSubentPath &firstPointPath, OdDbFullSubentPath &seondPointPath, DwgConstraintEvaluationCallback& callback)
  {
    OdConstrainedGeometry* pFirstPointGeom = NULL;
    OdConstrainedGeometry* pSecondPointGeom = NULL;
    if (getConstrainedGeometry(pDatabase, firstPointPath, pFirstPointGeom) == eOk
      && getConstrainedGeometry(pDatabase, seondPointPath, pSecondPointGeom) == eOk)
    {
      DistanceFixation& distanceFixation = *callback._fixedDistance.append();
      distanceFixation.pt1 = pFirstPointGeom->nodeId();
      distanceFixation.pt2 = pSecondPointGeom->nodeId();
    }
  }

  void addHintGeometryFixation(OdConstraintGroupNodeId nodeId, DwgConstraintEvaluationCallback& callback)
  {
    callback._fixedGeom.append(nodeId);
  }

  void addHintGeometryFixation(OdDbDatabasePtr pDatabase, OdDbFullSubentPath& subentPath, DwgConstraintEvaluationCallback& callback)
  {
    OdConstrainedGeometry* pPointGeom = NULL;
    if (getConstrainedGeometry(pDatabase, subentPath, pPointGeom) == eOk)
      callback._fixedGeom.append(pPointGeom->nodeId());
  }

  void addHintAngleFixation(OdConstraintGroupNodeId nodeId, DwgConstraintEvaluationCallback& callback)
  {
    AngleFixation& angleFixation = *callback._fixedAngle.append();
    angleFixation.lineToFixAngle = nodeId;
  }

  void addHintArcFixation(OdDbDatabasePtr pDatabase, OdDbFullSubentPath& centerPath, OdConstraintGroupNodeId nodeId, DwgConstraintEvaluationCallback& callback)
  {
    OdConstrainedGeometry* pPointGeomCenter = NULL;
    if (getConstrainedGeometry(pDatabase, centerPath, pPointGeomCenter) == eOk)
    {
      ArcFixation& arcFixation = *callback._fixedRadius.append();
      arcFixation.arc = nodeId;
      arcFixation.center = pPointGeomCenter->nodeId();
    }
  }

  void GetAllObjectsWithImplicitPoints(const OdArray<OdConstrainedGeometry*>& aCurveGeom,
    const OdArray<OdConstrainedGeometry*>& aPointGeom,
    OdGroupNodeIdArray& aNodeId)
  {
    OdConstrainedCurve* curve;
    OdArray<OdConstrainedImplicitPoint*> aPoints;
    unsigned int i, j, sizePoints, size = aCurveGeom.length();
    for (i = 0; i < size; i++)
    {
      curve = OdConstrainedCurve::cast(aCurveGeom[i]);
      aNodeId.push_back(curve->nodeId());
      aPoints.clear();
      curve->getConstrainedImplicitPoints(aPoints);
      sizePoints = aPoints.length();
      for (j = 0; j < sizePoints; j++)
        aNodeId.push_back(aPoints[j]->nodeId());
    }

    size = aPointGeom.length();
    for (i = 0; i < size; i++)
      aNodeId.push_back(aPointGeom[i]->nodeId());
  }

  OdConstrainedImplicitPoint* FindImplicitPoint(int pointIndex,
    OdArray<OdConstrainedImplicitPoint*>& apImplicitPoints)
  {
    int size = apImplicitPoints.size();
    for (int i = 0; i < size; i++)
    {
      if (apImplicitPoints[i]->pointIndex() == pointIndex)
        return apImplicitPoints[i];
    }

    return NULL;
  }

  void getImplicitPoints(const OdConstrainedGeometry* constrainedGeometry, ImplicitPointArray& aPoints)
  {
    if (constrainedGeometry->isKindOf(OdConstrainedCurve::desc()))
      OdConstrainedCurve::cast(constrainedGeometry)->getConstrainedImplicitPoints(aPoints);
    else
      ODA_FAIL_ONCE();
  }

  OdConstrainedImplicitPoint* selectPoint(const ImplicitPointArray& implicitPointArray, OdConstrainedImplicitPoint::ImplicitPointType pointType)
  {
    ImplicitPointArray::const_iterator pointIterator;

    pointIterator = std::find_if(std::begin(implicitPointArray), std::end(implicitPointArray),
      [pointType](ImplicitPointArray::value_type element) { return element->pointType() == pointType; });

    if (pointIterator == std::end(implicitPointArray))
      return nullptr;

    return *pointIterator;
  }

  CurvePtr getCurve(const OdDbObjectId& entityId, const OdGePoint3d& pickedPoint)
  {
    OdDbFullSubentPath edgeSubent;
    bool createImplicitMidPoint = false;

    if (DWGConstraintUtils::getClosestEdgeSubEntPath(entityId, pickedPoint, edgeSubent, createImplicitMidPoint) != eOk)
      return nullptr;

    CurvePtr pCurve(DWGConstraintUtils::getCurveForEdgeSubentity(entityId, edgeSubent));
    return pCurve;
  }

  OdGe::EntityId entityType(const OdDbObjectId& entityId, const OdGePoint3d& entityPoint)
  {
    if (!isBlockReference(entityId))
    {
      CurvePtr curve = getCurve(entityId, entityPoint);

      if (!curve)
      {
        ODA_ASSERT_ONCE(curve);
        return OdGe::kEntity3d;
      }

      return curve->type();
    }
    else
    {
      OdDbEntityPtr nestedEntity;
      getClosestNestedEntity(entityId.safeOpenObject(), entityPoint, nestedEntity);
      if (nestedEntity->isKindOf(OdDbCurve::desc()))
      {
        OdDbCurvePtr dbCurve = nestedEntity;
        OdGeCurve3d *geCurve;
        dbCurve->getOdGeCurve(geCurve);
        OdGe::EntityId type = geCurve->type();
        delete geCurve;
        return type;
      }
      else
      {
        return OdGe::kEntity3d;
      }
    }
  }

  OdDbObjectId selectClosestEntity(const OdGePoint3d& pt, const OdDbObjectId& entId1, const OdDbObjectId& entId2)
  {
    OdResult es = eOk;
    OdDbEntityPtr entity1 = entId1.safeOpenObject();
    OdDbEntityPtr entity2 = entId2.safeOpenObject();
    OdDbAssocPersSubentIdPE* assocPersSubentIdPE = OdDbAssocPersSubentIdPE::cast(entity1->queryX(OdDbAssocPersSubentIdPE::desc()));
    if (!assocPersSubentIdPE)
      return OdDbObjectId::kNull;
    
    // Get the entId1 vertex, which closest to the point pt and calculate minDistance between points.
    OdDbFullSubentPath clossesVertexOnEnt1;
    double distance1 = -1;
    if ((es = getClosestVertexPath(entId1, pt, clossesVertexOnEnt1)) == eOk)
    {
      OdGePoint3d point;
      if ((es = assocPersSubentIdPE->getVertexSubentityGeometry(entity1, clossesVertexOnEnt1.subentId(), point)) == eOk)
        distance1 = point.distanceTo(pt);
    }

    // Get the entId2 vertex, which closest to the point pt.
    OdDbFullSubentPath clossesVertexOnEnt2;
    double distance2 = -1;
    if ((es = getClosestVertexPath(entId2, pt, clossesVertexOnEnt2)) == eOk)
    {
      OdGePoint3d point;
      if ((es = assocPersSubentIdPE->getVertexSubentityGeometry(entity2, clossesVertexOnEnt2.subentId(), point)) == eOk)
        distance2 = point.distanceTo(pt);
    }

    return distance1 < distance2 ? entId1 : entId2;
  }

  OdResult getClosestNestedEntity(const OdDbBlockReferencePtr blockReference,
    const OdGePoint3d& pt,
    OdDbEntityPtr &nestedEntity,
    double* pMinDist /*= nullptr*/)
  {
    OdResult res = eOk;

    //blockReference->subGetSubentPathsAtGsMarker
    OdDbBlockTableRecordPtr blockTableRecord = blockReference->blockTableRecord().safeOpenObject();
    OdGeMatrix3d transformMatrix = blockReference->blockTransform();

    double minDistance = -1.0;
    for (OdDbObjectIteratorPtr itr = blockTableRecord->newIterator(); !itr->done(); itr->step())
    {
      OdDbEntityPtr entity = itr->entity();
      if (entity->isErased())
        continue;

      double distance;
      if (entity->isKindOf(OdDbBlockReference::desc()))
      {
        // Block can contains nested blocks. Var 'entity' before calling contains ptr to nested block,
        // when function finished 'entity' contains ptr to the nested entity
        res = getClosestNestedEntity(entity, pt, entity, &distance);
      }
      else if(entity->isKindOf(OdDbCurve::desc()) ||
        entity->isKindOf(OdDbPoint::desc()) ||
        entity->isKindOf(OdDbText::desc()) ||
        entity->isKindOf(OdDbMText::desc()))
      {
        res = calculateDistanceToNestedEntity(pt, entity, blockReference->blockTransform(), distance);
      }
      else
      {
        continue;
      }

      if (res != eOk)
        return res;

      if (minDistance == -1.0 || distance < minDistance)
      {
        nestedEntity = entity;
        minDistance = distance;
      }
    }

    if (pMinDist)
      *pMinDist = minDistance;

    return eOk;
  }

  OdResult calculateDistanceToNestedEntity(const OdGePoint3d& pt,
    const OdDbEntityPtr entity,
    const OdGeMatrix3d& blockTransformation,
    double& result)
  {
    OdResult res = eOk;

    OdDbEntityPtr transformedEntity;
    res = entity->getTransformedCopy(blockTransformation, transformedEntity);
    if (res != eOk)
      return res;
    
    OdDbFullSubentPath clossestVertex;
    res = getClosestVertexPath(transformedEntity, pt, clossestVertex);
    if (res != eOk)
      return res;

    OdGePoint3d point;
    OdDbAssocPersSubentIdPE* assocPersSubentIdPE = OdDbAssocPersSubentIdPE::cast(transformedEntity->queryX(OdDbAssocPersSubentIdPE::desc()));
    if (!assocPersSubentIdPE)
      return eNullObjectPointer;

    res = assocPersSubentIdPE->getVertexSubentityGeometry(transformedEntity, clossestVertex.subentId(), point);
    if (res != eOk)
      return res;

    result = point.distanceTo(pt);
    return eOk;
  }

  OdResult getMidPoint(const OdDbEntityPtr entity, OdGePoint3d& midPoint)
  {
    OdResult res = eOk;

    if (entity->isKindOf(OdDbArc::desc()))
    {
      OdDbArcPtr arc = OdDbArc::cast(entity);

      double startParam = 0;
      if ((res = arc->getStartParam(startParam)) != eOk)
        return res;

      double endParam = 0;
      if ((res = arc->getEndParam(endParam)) != eOk)
        return res;

      double midParam = (startParam + endParam) / 2;
      res = arc->getPointAtParam(midParam, midPoint);
      return res;
    }
    else if (entity->isKindOf(OdDbLine::desc()))
    {
      OdDbLinePtr line = OdDbLine::cast(entity);
      OdGePoint3d startPoint = line->startPoint();
      OdGePoint3d endPoint = line->endPoint();
      midPoint = OdGePoint3d((startPoint.x + endPoint.x) / 2, (startPoint.y + endPoint.y) / 2, 0);
      return eOk;
    }

    return eInvalidInput;
  }

  bool moduleIsLoaded()
  {
    OdRxModulePtr dwgConstraintsModule = ::odrxDynamicLinker()->getModule(DwgConstraintsModuleName);
    return (dwgConstraintsModule != nullptr);
  }
}