# Алгоритм генерации графа с помощью оценки стабильности подсборок в физическом движке PyBullet
import FreeCAD as App
import json
import re
from pyquaternion import Quaternion


def importObjAtPath(path: str):
    import importOBJ
    importOBJ.insert(u"" + path, App.ActiveDocument.Label)

    pass


def getFullPathObj(assemblyFolder: str, name: str):
    return assemblyFolder + 'sdf/meshes/' + name + '.obj'


def computedStability(refElement, childElement):
    rootElement = childElement.Shape.BoundBox
    # Создание обьекта на котором делается операция пересечения
    App.activeDocument().addObject("Part::MultiCommon", "Common")
    App.activeDocument().Common.Shapes = [refElement, childElement, ]
    App.ActiveDocument.getObject('Common').ViewObject.ShapeColor = getattr(App.ActiveDocument.getObject(
        refElement.Name).getLinkedObject(True).ViewObject, 'ShapeColor', App.ActiveDocument.getObject('Common').ViewObject.ShapeColor)
    App.ActiveDocument.getObject('Common').ViewObject.DisplayMode = getattr(App.ActiveDocument.getObject(
        childElement.Name).getLinkedObject(True).ViewObject, 'DisplayMode', App.ActiveDocument.getObject('Common').ViewObject.DisplayMode)
    App.ActiveDocument.recompute()
    obj = App.ActiveDocument.getObjectsByLabel('Common')[0]
 
    shp = obj.Shape
    bbox = shp.BoundBox
    # Если после операции пересечения зона обьекта совпадает с зоной тестируемого обьекта то тест прошел успешно   
    if bbox.XLength == rootElement.XLength and bbox.YLength == rootElement.YLength and rootElement.ZLength == bbox.ZLength:
        return True
    return False


def main():
    App.newDocument()
    # загрузка env интерфейса
    env = json.loads((open('./env.json')).read())

    # получение пути координат файла
    coordinatesFilePath = env.get('pathToTheSimulationCoordinatesFile')

    # получение папки сборки
    assemblyFolder = env.get('generationFolder')
    buildNumber = int(re.findall(r'\d', coordinatesFilePath)[0])

    # загрузка последовательности сборки ввиде списка строк деталей
    assemblyStructure = json.loads(
        (open(assemblyFolder + 'step-structure.json')).read())
    # получение номера тестируемой сборки
    assemblyNumber = int(buildNumber)
    # получение тестируемой детали в сборке
    activeDetail = assemblyStructure[assemblyNumber]
  
    subassemblyNotParticipatingInMarkup = assemblyStructure[0:assemblyNumber - 1]
    detailOfTheMarkingZoneOfWhich = assemblyStructure[assemblyNumber - 1]
    importObjAtPath(getFullPathObj(assemblyFolder, activeDetail))
    importObjAtPath(getFullPathObj(
        assemblyFolder, detailOfTheMarkingZoneOfWhich))
    meshMark = App.ActiveDocument.Objects[0]
    meshDetailOfTheMarkZone = App.ActiveDocument.Objects[1]
    meshMark.ViewObject.ShapeColor = (0.31, 0.77, 0.87)
    meshDetailOfTheMarkZone.ViewObject.ShapeColor = (0.68, 0.66, 0.95)
    for el in list(map(lambda el: getFullPathObj(assemblyFolder, el), subassemblyNotParticipatingInMarkup)):
        importObjAtPath(el)
    for el in App.ActiveDocument.Objects[2:App.ActiveDocument.Objects.__len__()]:
        el.ViewObject.ShapeColor = (0.32, 0.05, 0.38)
    # загрузка координат
    coordinates = json.loads((open(coordinatesFilePath)).read())
    inc = 1
    # перемещение обьектов в документе на точки стабильность полученные из pybullet
    for el in App.ActiveDocument.Objects:
        pos = coordinates[inc]['position']
        qua = coordinates[inc]['quaternion']
        new_quaternion = Quaternion(qua[0], qua[1], qua[2], qua[3])
        rotation_matrix = new_quaternion.rotation_matrix
        current_position = el.Placement.Base
        new_position = App.Vector(
            current_position.x, current_position.y, current_position.z)
        new_placement = App.Placement(new_position, rotation_matrix)
        el.Placement = new_placement
        App.ActiveDocument.recompute()
        el.Placement.move(App.Vector(pos[0], pos[1], pos[2]))
    # вычисление стабильность
    computedStability(meshMark, meshDetailOfTheMarkZone)
    pass


main()