import hashlib
import json
import os
import xml.etree.ElementTree as ET

import FreeCAD as App
import Robot


FREECAD_COMMIT = "0108fd4b4850cc46e625b60e53cea7a7bbe69f8d"


def trajectory_snapshot(obj):
    content = obj.Trajectory.Content
    root = ET.fromstring(content)
    waypoints = [
        {
            "name": node.attrib["name"],
            "type": node.attrib["type"],
            "position": [round(float(node.attrib[key]), 9) for key in ("Px", "Py", "Pz")],
            "velocity": round(float(node.attrib["vel"]), 9),
            "acceleration": round(float(node.attrib["acc"]), 9),
        }
        for node in root.findall("Waypoint")
    ]
    return {
        "contentSha256": hashlib.sha256(content.encode("utf-8")).hexdigest(),
        "waypointCount": len(waypoints),
        "length": round(float(obj.Trajectory.Length), 9),
        "duration": round(float(obj.Trajectory.Duration), 9),
        "waypoints": waypoints,
    }


def object_snapshot(document):
    obj = document.getObject("AccelerationProbe")
    source = document.getObject("SourceTrajectory")
    return {
        "objectSet": [{"name": item.Name, "typeId": item.TypeId} for item in document.Objects],
        "object": {
            "name": obj.Name,
            "typeId": obj.TypeId,
            "propertyTypeId": obj.getTypeIdOfProperty("Acceleration"),
            "value": {"raw": str(obj.Acceleration), "numeric": round(float(obj.Acceleration.Value), 9), "unit": str(obj.Acceleration.Unit)},
            "propertyStatus": [str(item) for item in obj.getPropertyStatus("Acceleration")],
            "editorMode": [str(item) for item in obj.getEditorMode("Acceleration")],
            "source": {"name": obj.Source.Name, "typeId": obj.Source.TypeId},
            "useAcceleration": bool(obj.UseAcceleration),
            "state": [str(item) for item in obj.State],
            "statusString": str(obj.getStatusString()),
            "trajectory": trajectory_snapshot(obj),
        },
        "sourceTrajectory": trajectory_snapshot(source),
    }


def create_native(path):
    document = App.newDocument("PropertyAccelerationRoundtrip")
    try:
        source = document.addObject("Robot::TrajectoryObject", "SourceTrajectory")
        trajectory = source.Trajectory
        trajectory = trajectory.insertWaypoints(App.Placement(App.Vector(1.0, 2.0, 3.0), App.Rotation()))
        trajectory = trajectory.insertWaypoints(App.Placement(App.Vector(4.0, 6.0, 3.0), App.Rotation()))
        source.Trajectory = trajectory
        obj = document.addObject("Robot::TrajectoryDressUpObject", "AccelerationProbe")
        obj.Source = source
        obj.UseAcceleration = True
        obj.Acceleration = "1000 mm/s^2"
        document.recompute()
        document.saveAs(path)
    finally:
        App.closeDocument(document.Name)
    reopened = App.openDocument(path)
    try:
        reopened.recompute()
        return object_snapshot(reopened)
    finally:
        App.closeDocument(reopened.Name)


def verify_native(path, resaved_path):
    document = App.openDocument(path)
    try:
        document.recompute()
        reopened = object_snapshot(document)
        document.saveAs(resaved_path)
    finally:
        App.closeDocument(document.Name)
    resaved_document = App.openDocument(resaved_path)
    try:
        resaved_document.recompute()
        resaved = object_snapshot(resaved_document)
    finally:
        App.closeDocument(resaved_document.Name)
    return {"reopened": reopened, "resaved": resaved}


mode = os.environ.get("FREECAD_PROPERTY_ACCELERATION_ROUNDTRIP_MODE", "")
path = os.environ.get("FREECAD_PROPERTY_ACCELERATION_ROUNDTRIP_PATH", "")
resaved_path = os.environ.get("FREECAD_PROPERTY_ACCELERATION_ROUNDTRIP_RESAVED_PATH", "")
if mode == "create":
    result = create_native(path)
elif mode == "verify":
    result = verify_native(path, resaved_path)
else:
    raise RuntimeError("FREECAD_PROPERTY_ACCELERATION_ROUNDTRIP_MODE must be create or verify")

print("FREECAD_PROPERTY_ACCELERATION_ROUNDTRIP_RESULT=" + json.dumps({
    "schemaVersion": 1,
    "status": "pass",
    "baselineId": "freecad-1.1.1-property-acceleration-roundtrip",
    "freecadVersion": ".".join(str(value) for value in App.Version()[:3]),
    "gitCommit": FREECAD_COMMIT,
    "mode": mode,
    "result": result,
}, sort_keys=True))