#include "Misc/MujocoXmlGenerator.h"
#include "Components/SceneComponent.h"
#include "Components/StaticMeshComponent.h"
#include "Components/SkeletalMeshComponent.h"
#include "Components/PrimitiveComponent.h"
#include "tinyxml2.h"
#include "Misc/FileHelper.h"
#include "Misc/Paths.h"
#include "Components/MujocoBodyComponent.h"
#include "Engine/SCS_Node.h"
#include "StaticMeshOperations.h"
#include "functional"
#include "Actors/MujocoStaticMeshActor.h"
#include "Engine/LevelScriptActor.h"
#include "Components/MujocoGeomComponent.h"
#include "Components/MujocoJointComponent.h"
#include "Components/MujocoSiteComponent.h"
#include "Components/MujocoActuatorComponent.h"
#include "Components/MujocoEqualityComponent.h"
#include "Components/MujocoTendonComponent.h"
#include "UObject/PropertyOptional.h"
#include "Structs/MujocoInertial.h"
#include "Structs/MujocoSite.h"
#include "Structs/MujocoGeom.h"
#include "Enums/MujocoEnums.h"
#include "Camera/CameraComponent.h"
#include "GameFramework/SpringArmComponent.h"
#include "Misc/MujocoOptions.h"
#include "array"
#include "LuckyMujoco.h"

template <typename T> FString GetAttributeMappingForProperty(FProperty* /*Prop*/)
{
	return FString();
}

template <> FString GetAttributeMappingForProperty<FMujocoTendonFixedJoint>(FProperty* Prop)
{
	// clang-format off
	constexpr std::array Mapping{
		std::pair{ TEXT("Joint"), TEXT("joint") },
		std::pair{ TEXT("Coef"), TEXT("coef") },
	};
	// clang-format on

	FString PropName = Prop->GetNameCPP();
	for (const auto& Pair : Mapping)
	{
		if (PropName.Equals(Pair.first, ESearchCase::IgnoreCase))
		{
			return FString(Pair.second);
		}
	}
	return FString();
}

template <> FString GetAttributeMappingForProperty<FMujocoTendonSpatialSite>(FProperty* Prop)
{
	// clang-format off
	constexpr std::array Mapping{
		std::pair{ TEXT("Site"), TEXT("site") },		
	};
	// clang-format on

	FString PropName = Prop->GetNameCPP();
	for (const auto& Pair : Mapping)
	{
		if (PropName.Equals(Pair.first, ESearchCase::IgnoreCase))
		{
			return FString(Pair.second);
		}
	}
	return FString();
}

template <> FString GetAttributeMappingForProperty<FMujocoTendonSpatialGeom>(FProperty* Prop)
{
	// clang-format off
	constexpr std::array Mapping{
		std::pair{ TEXT("Geom"), TEXT("geom") },		
		std::pair{ TEXT("SideSite"), TEXT("sidesite") },		
	};
	// clang-format on

	FString PropName = Prop->GetNameCPP();
	for (const auto& Pair : Mapping)
	{
		if (PropName.Equals(Pair.first, ESearchCase::IgnoreCase))
		{
			return FString(Pair.second);
		}
	}
	return FString();
}

template <> FString GetAttributeMappingForProperty<FMujocoTendonPulley>(FProperty* Prop)
{
	// clang-format off
	constexpr std::array Mapping{
		std::pair{ TEXT("Divisor"), TEXT("divisor") },		
	};
	// clang-format on

	FString PropName = Prop->GetNameCPP();
	for (const auto& Pair : Mapping)
	{
		if (PropName.Equals(Pair.first, ESearchCase::IgnoreCase))
		{
			return FString(Pair.second);
		}
	}
	return FString();
}

template <> FString GetAttributeMappingForProperty<FMujocoSimulationFlags>(FProperty* Prop)
{
	// clang-format off
	constexpr std::array Mapping{
		std::pair{ TEXT("Constraint"), TEXT("constraint") },
		std::pair{ TEXT("Equality"), TEXT("equality") },
		std::pair{ TEXT("FrictionLoss"), TEXT("frictionloss") },
		std::pair{ TEXT("Limit"), TEXT("limit") },
		std::pair{ TEXT("Contact"), TEXT("contact") },
		std::pair{ TEXT("Passive"), TEXT("passive") },
		std::pair{ TEXT("Gravity"), TEXT("gravity") },
		std::pair{ TEXT("Actuation"), TEXT("actuation") },
		std::pair{ TEXT("RefSafe"), TEXT("refsafe") },
		std::pair{ TEXT("Sensor"), TEXT("sensor") },
		std::pair{ TEXT("Energy"), TEXT("energy") },
		std::pair{ TEXT("FwdInv"), TEXT("fwdinv") },
		std::pair{ TEXT("MultiCCD"), TEXT("multiccd") },
		std::pair{ TEXT("Island"), TEXT("island") },
		std::pair{ TEXT("NativeCCD"), TEXT("nativeccd") }
	};
	// clang-format on
	FString PropName = Prop->GetNameCPP();
	for (const auto& Pair : Mapping)
	{
		if (PropName.Equals(Pair.first, ESearchCase::IgnoreCase))
		{
			return FString(Pair.second);
		}
	}
	return FString();
}

template <> FString GetAttributeMappingForProperty<FMujocoCompileOptions>(FProperty* Prop)
{
	// clang-format off
	constexpr std::array Mapping{
		std::pair{ TEXT("AutoLimits"), TEXT("autolimits") },
		std::pair{ TEXT("BoundMass"), TEXT("boundmass") },
		std::pair{ TEXT("BoundInertia"), TEXT("boundinertia") },
		std::pair{ TEXT("SetTotalMass"), TEXT("settotalmass") },
		std::pair{ TEXT("BalanceInertia"), TEXT("balanceinertia") },
		std::pair{ TEXT("StripPath"), TEXT("strippath") },
		std::pair{ TEXT("AngleUnits"), TEXT("angle") },
		std::pair{ TEXT("FitAABB"), TEXT("fitaabb") },
		std::pair{ TEXT("EulerSequence"), TEXT("eulerseq") },
		std::pair{ TEXT("MeshDir"), TEXT("meshdir") },
		std::pair{ TEXT("TextureDir"), TEXT("texturedir") },
		std::pair{ TEXT("AssetDir"), TEXT("assetdir") },
		std::pair{ TEXT("DiscardVisual"), TEXT("discardvisual") },
		std::pair{ TEXT("UseThread"), TEXT("usethread") },
		std::pair{ TEXT("FuseStatic"), TEXT("fusestatic") },
		std::pair{ TEXT("InertiaFromGeom"), TEXT("inertiafromgeom") },
		std::pair{ TEXT("AlignFree"), TEXT("alignfree") },
		std::pair{ TEXT("InertiaGroupRange"), TEXT("inertiagrouprange") }
	};
	// clang-format on
	FString PropName = Prop->GetNameCPP();
	for (const auto& Pair : Mapping)
	{
		if (PropName.Equals(Pair.first, ESearchCase::IgnoreCase))
		{
			return FString(Pair.second);
		}
	}
	return FString();
}

template <> FString GetAttributeMappingForProperty<FMujocoOptions>(FProperty* Prop)
{
	// clang-format off
	constexpr std::array Mapping{
		std::pair{ TEXT("TimeStep"), TEXT("timestep") },
		std::pair{ TEXT("ApiRate"), TEXT("apirate") },
		std::pair{ TEXT("ImpRatio"), TEXT("impratio") },
		std::pair{ TEXT("Gravity"), TEXT("gravity") },
		std::pair{ TEXT("Wind"), TEXT("wind") },
		std::pair{ TEXT("Magnetic"), TEXT("magnetic") },
		std::pair{ TEXT("Density"), TEXT("density") },
		std::pair{ TEXT("Viscosity"), TEXT("viscosity") },
		std::pair{ TEXT("MarginOverride"), TEXT("o_margin") },
		std::pair{ TEXT("Integrator"), TEXT("integrator") },
		std::pair{ TEXT("ConeType"), TEXT("cone") },
		std::pair{ TEXT("JacobianType"), TEXT("jacobian") },
		std::pair{ TEXT("Solver"), TEXT("solver") },
		std::pair{ TEXT("MaxIterations"), TEXT("iterations") },
		std::pair{ TEXT("SolverTolerance"), TEXT("tolerance") },
		std::pair{ TEXT("LineSearchIterations"), TEXT("ls_iterations") },
		std::pair{ TEXT("LineSearchTolerance"), TEXT("ls_tolerance") },
		std::pair{ TEXT("NoSlipIterations"), TEXT("noslip_iterations") },
		std::pair{ TEXT("NoSlipTolerance"), TEXT("noslip_tolerance") },
		std::pair{ TEXT("CCDIterations"), TEXT("ccd_iterations") },
		std::pair{ TEXT("CCDTolerance"), TEXT("ccd_tolerance") },
		std::pair{ TEXT("SDFIterations"), TEXT("sdf_iterations") },
		std::pair{ TEXT("SDFInitPoints"), TEXT("sdf_initpoints") },
		std::pair{ TEXT("ActuatorGroupDisable"), TEXT("actuatorgroupdisable") }
	};
	// clang-format on
	FString PropName = Prop->GetNameCPP();
	for (const auto& Pair : Mapping)
	{
		if (PropName.Equals(Pair.first, ESearchCase::IgnoreCase))
		{
			return FString(Pair.second);
		}
	}
	return FString();
}

template <> FString GetAttributeMappingForProperty<FMujocoActuatorV2>(FProperty* Prop)
{
	// clang-format off
	constexpr std::array Mapping{		
		std::pair{ TEXT("Group"), TEXT("group") },
		std::pair{ TEXT("CtrlLimited"), TEXT("ctrllimited") },
		std::pair{ TEXT("ForceLimited"), TEXT("forcelimited") },
		std::pair{ TEXT("ActLimited"), TEXT("actlimited") },
		std::pair{ TEXT("CtrlRange"), TEXT("ctrlrange") },
		std::pair{ TEXT("ForceRange"), TEXT("forcerange") },
		std::pair{ TEXT("ActRange"), TEXT("actrange") },
		std::pair{ TEXT("LengthRange"), TEXT("lengthrange") },
		std::pair{ TEXT("Gear"), TEXT("gear") },
		std::pair{ TEXT("CrankLength"), TEXT("cranklength") },
		std::pair{ TEXT("Joint"), TEXT("joint") },
		std::pair{ TEXT("JointInParent"), TEXT("jointinparent") },
		std::pair{ TEXT("Site"), TEXT("site") },
		std::pair{ TEXT("RefSite"), TEXT("refsite") },
		std::pair{ TEXT("Body"), TEXT("body") },
		std::pair{ TEXT("Tendon"), TEXT("tendon") },
		std::pair{ TEXT("CrankSite"), TEXT("cranksite") },
		std::pair{ TEXT("SliderSite"), TEXT("slidersite") },
		std::pair{ TEXT("Kp"), TEXT("kp") },
		std::pair{ TEXT("Kv"), TEXT("kv") },
		std::pair{ TEXT("DampingRatio"), TEXT("dampratio") },
		std::pair{ TEXT("TimeConst"), TEXT("timeconst") },
		std::pair{ TEXT("InheritRange"), TEXT("inheritrange") },
		std::pair{ TEXT("Area"), TEXT("area") },
		std::pair{ TEXT("Diameter"), TEXT("diameter") },
		std::pair{ TEXT("Bias"), TEXT("bias") },
		std::pair{ TEXT("MuscleTimeConsts"), TEXT("timeconst") },
		std::pair{ TEXT("TauSmooth"), TEXT("tausmooth") },
		std::pair{ TEXT("Range"), TEXT("range") },
		std::pair{ TEXT("Force"), TEXT("force") },
		std::pair{ TEXT("Scale"), TEXT("scale") },
		std::pair{ TEXT("VMax"), TEXT("vmax") },
		std::pair{ TEXT("FpMax"), TEXT("fpmax") },
		std::pair{ TEXT("FvMax"), TEXT("fvmax") },
		std::pair{ TEXT("LMin"), TEXT("lmin") },
		std::pair{ TEXT("LMax"), TEXT("lmax") },
		std::pair{ TEXT("AdhesionGain"), TEXT("gain") },
		std::pair{ TEXT("DynType"), TEXT("dyntype") },
		std::pair{ TEXT("GainType"), TEXT("gaintype") },
		std::pair{ TEXT("BiasType"), TEXT("biastype") },
		std::pair{ TEXT("DynPrm"), TEXT("dynprm") },
		std::pair{ TEXT("GainPrm"), TEXT("gainprm") },
		std::pair{ TEXT("BiasPrm"), TEXT("biasprm") },
		std::pair{ TEXT("ActEarly"), TEXT("actearly") },
	};
	// clang-format on
	FString PropName = Prop->GetNameCPP();
	for (const auto& Pair : Mapping)
	{
		if (PropName.Equals(Pair.first, ESearchCase::IgnoreCase))
		{
			return FString(Pair.second);
		}
	}
	return FString();
}

template <> FString GetAttributeMappingForProperty<FMujocoEquality>(FProperty* Prop)
{
	// clang-format off
	constexpr std::array Mapping{		
		std::pair{ TEXT("Active"), TEXT("active") },
		std::pair{ TEXT("Body1"), TEXT("body1") },
		std::pair{ TEXT("Body2"), TEXT("body2") },
		std::pair{ TEXT("Site1"), TEXT("site1") },
		std::pair{ TEXT("Site2"), TEXT("site2") },
		std::pair{ TEXT("Joint1"), TEXT("joint1") },
		std::pair{ TEXT("Joint2"), TEXT("joint2") },
		std::pair{ TEXT("Tendon1"), TEXT("tendon1") },
		std::pair{ TEXT("Tendon2"), TEXT("tendon2") },
		std::pair{ TEXT("Flex"), TEXT("flex") },
		std::pair{ TEXT("SolRef"), TEXT("solref") },
		std::pair{ TEXT("SolImp"), TEXT("solimp") },
		std::pair{ TEXT("Anchor"), TEXT("anchor") },
		std::pair{ TEXT("RelPose"), TEXT("relpose") },
		std::pair{ TEXT("TorqueScale"), TEXT("torquescale") },
		std::pair{ TEXT("PolyCoef"), TEXT("polycoef") }
	};
	// clang-format on
	FString PropName = Prop->GetNameCPP();
	for (const auto& Pair : Mapping)
	{
		if (PropName.Equals(Pair.first, ESearchCase::IgnoreCase))
		{
			return FString(Pair.second);
		}
	}
	return FString();
}

template <> FString GetAttributeMappingForProperty<FMujocoGeom>(FProperty* Prop)
{
	// clang-format off
	constexpr std::array Mapping{
		std::pair{ TEXT("ConType"), TEXT("contype") },
		std::pair{ TEXT("ConAffinity"), TEXT("conaffinity") },
		std::pair{ TEXT("ConDim"), TEXT("condim") },
		std::pair{ TEXT("Group"), TEXT("group") },
		std::pair{ TEXT("Priority"), TEXT("priority") },
		std::pair{ TEXT("Friction"), TEXT("friction") },
		std::pair{ TEXT("Mass"), TEXT("mass") },
		std::pair{ TEXT("Density"), TEXT("density") },
		std::pair{ TEXT("ShellInertia"), TEXT("shellinertia") },
		std::pair{ TEXT("Solmix"), TEXT("solmix") },
		std::pair{ TEXT("SolImp"), TEXT("solimp") },
		std::pair{ TEXT("SolRef"), TEXT("solref") },
		std::pair{ TEXT("Margin"), TEXT("margin") },
		std::pair{ TEXT("Gap"), TEXT("gap") }
	};
	// clang-format on
	FString PropName = Prop->GetNameCPP();
	for (const auto& Pair : Mapping)
	{
		if (PropName.Equals(Pair.first, ESearchCase::IgnoreCase))
		{
			return FString(Pair.second);
		}
	}
	return FString();
}

template <> FString GetAttributeMappingForProperty<FMujocoInertial>(FProperty* Prop)
{
	// clang-format off
	constexpr std::array Mapping{
		std::pair{ TEXT("Pos"), TEXT("pos") },
		std::pair{ TEXT("Mass"), TEXT("mass") },
		std::pair{ TEXT("DiagInertia"), TEXT("diaginertia") },
		std::pair{ TEXT("FullInertia"), TEXT("fullinertia") }
	};
	// clang-format on
	FString PropName = Prop->GetNameCPP();
	for (const auto& Pair : Mapping)
	{
		if (PropName.Equals(Pair.first, ESearchCase::IgnoreCase))
		{
			return FString(Pair.second);
		}
	}
	return FString();
}

template <> FString GetAttributeMappingForProperty<FMujocoJoint>(FProperty* Prop)
{
	// clang-format off
	constexpr std::array Mapping{
		std::pair{ TEXT("Type"), TEXT("type") },
		std::pair{ TEXT("Group"), TEXT("group") },
		std::pair{ TEXT("Position"), TEXT("pos") },
		std::pair{ TEXT("Axis"), TEXT("axis") },
		std::pair{ TEXT("SpringDamper"), TEXT("springdamper") },
		std::pair{ TEXT("Stiffness"), TEXT("stiffness") },
		std::pair{ TEXT("Range"), TEXT("range") },
		std::pair{ TEXT("LimitType"), TEXT("limited") },
		std::pair{ TEXT("ActuatorForceRange"), TEXT("actuatorfrcrange") },
		std::pair{ TEXT("ActuatorForceLimitType"), TEXT("actuatorfrclimited") },
		std::pair{ TEXT("bActuatorGravityCompensation"), TEXT("actuatorgravcomp") },
		std::pair{ TEXT("Margin"), TEXT("margin") },
		std::pair{ TEXT("Reference"), TEXT("ref") },
		std::pair{ TEXT("SpringReference"), TEXT("springref") },
		std::pair{ TEXT("Armature"), TEXT("armature") },
		std::pair{ TEXT("Damping"), TEXT("damping") },
		std::pair{ TEXT("FrictionLoss"), TEXT("frictionloss") }
	};
	// clang-format on
	FString PropName = Prop->GetNameCPP();
	for (const auto& Pair : Mapping)
	{
		if (PropName.Equals(Pair.first, ESearchCase::IgnoreCase))
		{
			return FString(Pair.second);
		}
	}
	return FString();
}

template <> FString GetAttributeMappingForProperty<FMujocoSite>(FProperty* Prop)
{
	// clang-format off
	constexpr std::array Mapping{
		std::pair{ TEXT("Type"), TEXT("type") },
		std::pair{ TEXT("Group"), TEXT("group") },
		std::pair{ TEXT("Rgba"), TEXT("rgba") },
		std::pair{ TEXT("Size"), TEXT("size") }		
	};
	// clang-format on
	FString PropName = Prop->GetNameCPP();
	for (const auto& Pair : Mapping)
	{
		if (PropName.Equals(Pair.first, ESearchCase::IgnoreCase))
		{
			return FString(Pair.second);
		}
	}
	return FString();
}

template <> FString GetAttributeMappingForProperty<FMujocoTendon>(FProperty* Prop)
{
	// clang-format off
	constexpr std::array Mapping{		
		std::pair{ TEXT("Name"), TEXT("name") },
		std::pair{ TEXT("Group"), TEXT("group") },
		std::pair{ TEXT("Limited"), TEXT("limited") },
		std::pair{ TEXT("Range"), TEXT("range") },
		std::pair{ TEXT("SolRefLimit"), TEXT("solreflimit") },
		std::pair{ TEXT("SolImpLimit"), TEXT("solimplimit") },
		std::pair{ TEXT("SolRefFriction"), TEXT("solreffriction") },
		std::pair{ TEXT("SolImpFriction"), TEXT("solimpfriction") },
		std::pair{ TEXT("Margin"), TEXT("margin") },
		std::pair{ TEXT("FrictionLoss"), TEXT("frictionloss") },
		std::pair{ TEXT("Width"), TEXT("width") },
		std::pair{ TEXT("SpringLength"), TEXT("springlength") },
		std::pair{ TEXT("Stiffness"), TEXT("stiffness") },
		std::pair{ TEXT("Damping"), TEXT("damping") },
		std::pair{ TEXT("SpatialSite"), TEXT("site") },
		std::pair{ TEXT("SpatialGeom"), TEXT("geom") },
		std::pair{ TEXT("Pulley"), TEXT("pulley") },
		std::pair{ TEXT("FixedJoint"), TEXT("fixed") }
	};
	// clang-format on
	FString PropName = Prop->GetNameCPP();
	for (const auto& Pair : Mapping)
	{
		if (PropName.Equals(Pair.first, ESearchCase::IgnoreCase))
		{
			return FString(Pair.second);
		}
	}
	return FString();
}

template <typename T> FString GetStructToString(FStructProperty* StructProperty, void* Container)
{
	if (StructProperty->Struct->GetFName() == T::StaticStruct()->GetFName())
	{
		T* StructInstance = StructProperty->ContainerPtrToValuePtr<T>(Container);
		if (StructInstance)
		{
			return StructInstance->ToString();
		}
	}
	return FString();
}

const TSet<FString> NotLowerCase = { "Euler", "RK4", "PGS", "CG", "Newton" };
FString GetPropertyValueAsString(FProperty* Property, void* Container)
{
	FString ValueStr;
	if (FEnumProperty* EnumProperty = CastField<FEnumProperty>(Property))
	{
		int64 EnumValue = EnumProperty->GetUnderlyingProperty()->GetSignedIntPropertyValue(EnumProperty->ContainerPtrToValuePtr<void>(Container));
		auto DisplayText = EnumProperty->GetEnum()->GetNameStringByValue(EnumValue);

		if (!NotLowerCase.Contains(DisplayText))
		{
			ValueStr = DisplayText.ToLower();
		}
		else
		{
			ValueStr = DisplayText;
		}

		if (Property->GetFName() == GET_MEMBER_NAME_CHECKED(FMujocoGeom, ConDim))
		{
			ValueStr = FString::FromInt(EnumValue);
		}
		else if (ValueStr.EndsWith("_"))
		{
			ValueStr = ValueStr.LeftChop(1);
		}
	}
	else if (FArrayProperty* NumericArrayProperty = CastField<FArrayProperty>(Property))
	{
		FProperty* InnerProperty = NumericArrayProperty->Inner;
		FScriptArrayHelper ArrayHelper(NumericArrayProperty, NumericArrayProperty->ContainerPtrToValuePtr<void>(Container));
		
		if (InnerProperty->IsA<FNumericProperty>() && CastField<FNumericProperty>(InnerProperty)->IsFloatingPoint())
		{
			TArray<FString> Values;
			for (int32 i = 0; i < ArrayHelper.Num(); ++i)
			{
				double Value = CastField<FNumericProperty>(InnerProperty)->GetFloatingPointPropertyValue(ArrayHelper.GetRawPtr(i));
				Values.Add(FString::SanitizeFloat(Value));
			}
			ValueStr = FString::Join(Values, TEXT(" "));
		}
		else if (InnerProperty->IsA<FNumericProperty>() && CastField<FNumericProperty>(InnerProperty)->IsInteger())
		{
			TArray<FString> Values;
			for (int32 i = 0; i < ArrayHelper.Num(); ++i)
			{
				int64 Value = CastField<FNumericProperty>(InnerProperty)->GetSignedIntPropertyValue(ArrayHelper.GetRawPtr(i));				
				Values.Add(FString::Printf(TEXT("%lld"), Value));
			}
			ValueStr = FString::Join(Values, TEXT(" "));
		}
	}
	else if (FNumericProperty* NumericProperty = CastField<FNumericProperty>(Property))
	{
		if (NumericProperty->IsFloatingPoint())
		{
			double Value = NumericProperty->GetFloatingPointPropertyValue(NumericProperty->ContainerPtrToValuePtr<void>(Container));
			ValueStr = FString::SanitizeFloat(Value);
		}
		else if (NumericProperty->IsInteger())
		{
			int64 Value = NumericProperty->GetSignedIntPropertyValue(NumericProperty->ContainerPtrToValuePtr<void>(Container));
			ValueStr = FString::Printf(TEXT("%lld"), Value);
		}
	}
	else if (FBoolProperty* BoolProperty = CastField<FBoolProperty>(Property))
	{
		bool Value = BoolProperty->GetPropertyValue(BoolProperty->ContainerPtrToValuePtr<void>(Container));
		ValueStr = Value ? TEXT("true") : TEXT("false");
	}
	else if (FStrProperty* StringProperty = CastField<FStrProperty>(Property))
	{
		FString* String = StringProperty->ContainerPtrToValuePtr<FString>(Container);
		if (String && !String->IsEmpty())
		{
			ValueStr = StringProperty->GetPropertyValue(String);
		}
	}
	else if (FNameProperty* NameProperty = CastField<FNameProperty>(Property))
	{
		auto* Name = NameProperty->ContainerPtrToValuePtr<FName>(Container);
		if (Name && !Name->IsNone())
		{
			ValueStr = NameProperty->GetPropertyValue(Name).ToString();
		}
	}
	else if (FOptionalProperty* OptionalProperty = CastField<FOptionalProperty>(Property))
	{
		void* OptionalPtr = OptionalProperty->ContainerPtrToValuePtr<void>(Container);
		if (OptionalPtr && OptionalProperty->IsSet(OptionalPtr))
		{
			FProperty* InnerProperty = OptionalProperty->GetValueProperty();
			if (InnerProperty)
			{
				ValueStr = GetPropertyValueAsString(InnerProperty, OptionalPtr);
			}
		}
	}
	else if (FStructProperty* StructProperty = CastField<FStructProperty>(Property))
	{
		if (StructProperty->Struct->GetFName() == NAME_Vector)
		{
			FVector* Vector = StructProperty->ContainerPtrToValuePtr<FVector>(Container);
			ValueStr = FString::Printf(TEXT("%f %f %f"), Vector->X, Vector->Y, Vector->Z);
		}
		else if (StructProperty->Struct->GetFName() == NAME_Vector2D)
		{
			FVector2D* Vector = StructProperty->ContainerPtrToValuePtr<FVector2D>(Container);
			ValueStr = FString::Printf(TEXT("%f %f"), Vector->X, Vector->Y);
		}
		else if (StructProperty->Struct->GetFName() == NAME_IntPoint)
		{
			FIntPoint* IntPoint = StructProperty->ContainerPtrToValuePtr<FIntPoint>(Container);
			ValueStr = FString::Printf(TEXT("%d %d"), IntPoint->X, IntPoint->Y);
		}
		else
		{
			ValueStr = GetStructToString<FMujocoFriction>(StructProperty, Container);
			if (ValueStr.IsEmpty())
				ValueStr = GetStructToString<FMujocoSolImp>(StructProperty, Container);
			if (ValueStr.IsEmpty())
				ValueStr = GetStructToString<FMujocoSolRef>(StructProperty, Container);
			if (ValueStr.IsEmpty())
				ValueStr = GetStructToString<FMujocoRelPose>(StructProperty, Container);
			if (ValueStr.IsEmpty())
				ValueStr = GetStructToString<FMujocoPolyCoef>(StructProperty, Container);
			if (ValueStr.IsEmpty())
				ValueStr = GetStructToString<FMujocoGear>(StructProperty, Container);
		}
	}
	else
	{
		UE_LOG(LogMujoco, Warning, TEXT("Unsupported property type %s"), *Property->GetClass()->GetName());
	}
	return ValueStr;
}

template <> FString GetAttributeMappingForProperty<UMujocoBodyComponent>(FProperty* Prop)
{
	// clang-format off
    constexpr std::array Mapping{
        std::pair{ TEXT("Mocap"), TEXT("mocap") },
        std::pair{ TEXT("GravComp"), TEXT("gravcomp") }        
    };
	// clang-format on

	FString PropName = Prop->GetNameCPP();
	for (const auto& Pair : Mapping)
	{
		if (PropName.Equals(Pair.first, ESearchCase::IgnoreCase))
		{
			return FString(Pair.second);
		}
	}
	return FString();
}

template <typename T> FString GetPropertyAttributeName(FProperty* Prop)
{
	return GetAttributeMappingForProperty<T>(Prop);
}

template <typename T> void ApplyStructAttributes(tinyxml2::XMLElement* XmlElem, const T& StructData)
{
	for (TFieldIterator<FProperty> It(T::StaticStruct()); It; ++It)
	{
		FProperty* Prop = *It;
		const FString AttributeName = GetPropertyAttributeName<T>(Prop);
		if (AttributeName.IsEmpty())
		{
			continue;
		}
		const FString Value = GetPropertyValueAsString(Prop, reinterpret_cast<void*>(const_cast<T*>(&StructData)));
		if (Value.IsEmpty())
		{
			continue;
		}
		XmlElem->SetAttribute(TCHAR_TO_ANSI(*AttributeName), TCHAR_TO_ANSI(*Value));
	}
}

bool FMujocoXmlGenerator::VisitGeomComponent(USCS_Node* SCSNode, UMujocoGeomComponent* GeomComponent, tinyxml2::XMLElement* Element)
{
	FString Name = SCSNode ? SCSNode->GetVariableName().ToString() : GeomComponent->GetName();

	tinyxml2::XMLElement* GeomElement = Element->GetDocument()->NewElement("geom");
	const FMujocoGeom& GeomStruct = GeomComponent->Geom;
	GeomElement->SetAttribute("name", TCHAR_TO_ANSI(*Name));
	GeomElement->SetAttribute("type", "mesh");
	FTransform BodyTransform = GeomComponent->GetRelativeTransform();
	FVector Position = BodyTransform.GetLocation() * 0.01f;
	FQuat Rotation = BodyTransform.Rotator().Quaternion();
	FVector Scale = BodyTransform.GetScale3D();
	if (!Position.IsZero())
	{
		GeomElement->SetAttribute("pos", TCHAR_TO_ANSI(*FString::Printf(TEXT("%f %f %f"), Position.X, -Position.Y, Position.Z)));
	}
	if (!Rotation.IsIdentity())
	{
		GeomElement->SetAttribute("quat", TCHAR_TO_ANSI(*FString::Printf(TEXT("%f %f %f %f"), -Rotation.W, Rotation.X, -Rotation.Y, Rotation.Z)));
	}
	ApplyStructAttributes(GeomElement, GeomStruct);
	if (UStaticMesh* StaticMesh = GeomComponent->GetStaticMesh())
	{
		tinyxml2::XMLElement* Root = Element->GetDocument()->FirstChildElement("mujoco");
		ensureMsgf(Root, TEXT("No mujoco root element found in xml document"));
		tinyxml2::XMLElement* AssetRoot = Root->FirstChildElement("asset");
		ensureMsgf(AssetRoot, TEXT("No asset element found in mujoco root element"));
		tinyxml2::XMLElement* MeshElement = Element->GetDocument()->NewElement("mesh");
		FString MeshPath = GeomComponent->GetFName().ToString();
		ObjectMap.FindOrAdd(MeshPath, GeomComponent);
		FString MeshName = FString::Printf(TEXT("%s_%s_%d"), *Name, *StaticMesh->GetName(), StaticMesh->GetUniqueID());
		MeshElement->SetAttribute("file", TCHAR_TO_ANSI(*MeshPath));
		MeshElement->SetAttribute("name", TCHAR_TO_ANSI(*MeshName));
		if (Scale.X != 1.0f || Scale.Y != 1.0f || Scale.Z != 1.0f)
		{
			MeshElement->SetAttribute("scale", TCHAR_TO_ANSI(*FString::Printf(TEXT("%f %f %f"), Scale.X, Scale.Y, Scale.Z)));
		}
		AssetRoot->InsertEndChild(MeshElement);
		GeomElement->SetAttribute("mesh", TCHAR_TO_ANSI(*MeshName));
	}
	Element->InsertEndChild(GeomElement);
	return true;
}

bool FMujocoXmlGenerator::VisitJointComponent(USCS_Node* SCSNode, UMujocoJointComponent* JointComponent, tinyxml2::XMLElement* Element)
{
	FString Name = SCSNode ? SCSNode->GetVariableName().ToString() : JointComponent->GetName();

	tinyxml2::XMLElement* JointElement = Element->GetDocument()->NewElement("joint");
	const FMujocoJoint& JointStruct = JointComponent->Joint;
	JointElement->SetAttribute("name", TCHAR_TO_ANSI(*Name));
	ApplyStructAttributes(JointElement, JointStruct);
	Element->InsertEndChild(JointElement);
	return true;
}

bool FMujocoXmlGenerator::VisitSiteComponent(USCS_Node* SCSNode, UMujocoSiteComponent* SiteComponent, tinyxml2::XMLElement* Element)
{
	FString Name = SCSNode ? SCSNode->GetVariableName().ToString() : SiteComponent->GetName();

	tinyxml2::XMLElement* SiteElement = Element->GetDocument()->NewElement("site");
	const FMujocoSite& SiteStruct = SiteComponent->Site;
	SiteElement->SetAttribute("name", TCHAR_TO_ANSI(*Name));
	FTransform SiteTransform = SiteComponent->GetRelativeTransform();
	FVector Position = SiteTransform.GetLocation() * 0.01f;
	FQuat Rotation = SiteTransform.Rotator().Quaternion();
	if (!Position.IsZero())
	{
		SiteElement->SetAttribute("pos", TCHAR_TO_ANSI(*FString::Printf(TEXT("%f %f %f"), Position.X, -Position.Y, Position.Z)));
	}
	if (!Rotation.IsIdentity())
	{
		SiteElement->SetAttribute("quat", TCHAR_TO_ANSI(*FString::Printf(TEXT("%f %f %f %f"), -Rotation.W, Rotation.X, -Rotation.Y, Rotation.Z)));
	}
	ApplyStructAttributes(SiteElement, SiteStruct);
	Element->InsertEndChild(SiteElement);
	return true;
}

bool FMujocoXmlGenerator::VisitActuatorComponent(USCS_Node* SCSNode, UMujocoActuatorComponent* ActuatorComponent, tinyxml2::XMLElement* Element)
{
	FString Name = SCSNode ? SCSNode->GetVariableName().ToString() : ActuatorComponent->GetName();

	tinyxml2::XMLElement* ActuatorElement = nullptr;
	const FMujocoActuatorV2& ActuatorStruct = ActuatorComponent->Actuator;
	if (FEnumProperty* TypeProperty = CastField<FEnumProperty>(FMujocoActuatorV2::StaticStruct()->FindPropertyByName("Type")))
	{
		int64 EnumValue = TypeProperty->GetUnderlyingProperty()->GetSignedIntPropertyValue(TypeProperty->ContainerPtrToValuePtr<void>(&ActuatorStruct));
		auto DisplayText = TypeProperty->GetEnum()->GetNameStringByValue(EnumValue);
		auto ValueStr = DisplayText.ToLower();
		ActuatorElement = Element->GetDocument()->NewElement(TCHAR_TO_ANSI(*ValueStr));
	}
	else
	{
		return false;
	}
	ActuatorElement->SetAttribute("name", TCHAR_TO_ANSI(*Name));
	ApplyStructAttributes(ActuatorElement, ActuatorStruct);
	tinyxml2::XMLElement* ActuatorRoot = Element->FirstChildElement("actuator");
	ensureMsgf(ActuatorRoot, TEXT("No actuator element found in mujoco root element"));
	ActuatorRoot->InsertEndChild(ActuatorElement);
	return true;
}

bool FMujocoXmlGenerator::VisitEqualityComponent(USCS_Node* SCSNode, UMujocoEqualityComponent* EqualityComponent, tinyxml2::XMLElement* Element)
{
	FString Name = SCSNode ? SCSNode->GetVariableName().ToString() : EqualityComponent->GetName();

	tinyxml2::XMLElement* EqualityElement = nullptr;
	const FMujocoEquality& EqualityStruct = EqualityComponent->Equality;
	if (FEnumProperty* TypeProperty = CastField<FEnumProperty>(FMujocoEquality::StaticStruct()->FindPropertyByName("Type")))
	{
		int64 EnumValue = TypeProperty->GetUnderlyingProperty()->GetSignedIntPropertyValue(TypeProperty->ContainerPtrToValuePtr<void>(&EqualityStruct));
		auto DisplayText = TypeProperty->GetEnum()->GetNameStringByValue(EnumValue);
		auto ValueStr = DisplayText.ToLower();
		EqualityElement = Element->GetDocument()->NewElement(TCHAR_TO_ANSI(*ValueStr));
	}
	else
	{
		return false;
	}
	ApplyStructAttributes(EqualityElement, EqualityStruct);
	tinyxml2::XMLElement* EqualityRoot = Element->FirstChildElement("equality");
	ensureMsgf(EqualityRoot, TEXT("No equality element found in mujoco root element"));
	EqualityRoot->InsertEndChild(EqualityElement);
	return true;
}

bool FMujocoXmlGenerator::VisitTendonComponent(USCS_Node* SCSNode, UMujocoTendonComponent* TendonComponent, tinyxml2::XMLElement* Element)
{
	FString Name = SCSNode ? SCSNode->GetVariableName().ToString() : TendonComponent->GetName();

	tinyxml2::XMLElement* TendonElement = nullptr;
	const FMujocoTendon& TendonStruct = TendonComponent->Tendon;
	if (FEnumProperty* TypeProperty = CastField<FEnumProperty>(FMujocoTendon::StaticStruct()->FindPropertyByName("Type")))
	{
		int64 EnumValue = TypeProperty->GetUnderlyingProperty()->GetSignedIntPropertyValue(TypeProperty->ContainerPtrToValuePtr<void>(&TendonStruct));
		auto DisplayText = TypeProperty->GetEnum()->GetNameStringByValue(EnumValue);
		auto ValueStr = DisplayText.ToLower();
		TendonElement = Element->GetDocument()->NewElement(TCHAR_TO_ANSI(*ValueStr));
	}
	else
	{
		return false;
	}
	ApplyStructAttributes(TendonElement, TendonStruct);
	for (const auto& SpatialSite : TendonComponent->Tendon.SpatialSite)
	{
		tinyxml2::XMLElement* SpatialElement = TendonElement->GetDocument()->NewElement("site");
		ApplyStructAttributes(SpatialElement, SpatialSite);
		TendonElement->InsertEndChild(SpatialElement);
	}
	for (const auto& SpatialGeom : TendonComponent->Tendon.SpatialGeom)
	{
		tinyxml2::XMLElement* SpatialElement = TendonElement->GetDocument()->NewElement("geom");
		ApplyStructAttributes(SpatialElement, SpatialGeom);
		TendonElement->InsertEndChild(SpatialElement);
	}
	for (const auto& Pulley : TendonComponent->Tendon.Pulley)
	{
		tinyxml2::XMLElement* PulleyElement = TendonElement->GetDocument()->NewElement("pulley");
		ApplyStructAttributes(PulleyElement, Pulley);
		TendonElement->InsertEndChild(PulleyElement);
	}
	for (const auto& FixedJoint : TendonComponent->Tendon.FixedJoint)
	{
		tinyxml2::XMLElement* FixedElement = TendonElement->GetDocument()->NewElement("joint");
		ApplyStructAttributes(FixedElement, FixedJoint);
		TendonElement->InsertEndChild(FixedElement);
	}
	tinyxml2::XMLElement* TendonRoot = Element->FirstChildElement("tendon");
	ensureMsgf(TendonRoot, TEXT("No tendon element found in mujoco root element"));
	TendonRoot->InsertEndChild(TendonElement);
	return true;
}

bool FMujocoXmlGenerator::VisitCameraComponent(USCS_Node* SCSNode, UCameraComponent* CameraComponent, tinyxml2::XMLElement* Element, USpringArmComponent* SpringArmComponent)
{
	FString Name = SCSNode ? SCSNode->GetVariableName().ToString() : CameraComponent->GetName();

	if (SpringArmComponent)
	{
		FTransform SpringArmTransform = SpringArmComponent->GetRelativeTransform();
		float SpringArmLength = SpringArmComponent->TargetArmLength * 0.01f;
		FVector Position = SpringArmTransform.GetLocation() * 0.1f - SpringArmTransform.GetRotation().GetForwardVector() * SpringArmLength;
		FQuat Rotation = SpringArmTransform.GetRotation().Rotator().Quaternion();
		tinyxml2::XMLElement* FrameElement = Element->GetDocument()->NewElement("frame");
		tinyxml2::XMLElement* CameraElement = Element->GetDocument()->NewElement("camera");
		CameraElement->SetAttribute("name", TCHAR_TO_ANSI(*Name));
		if (!Position.IsZero())
		{
			FrameElement->SetAttribute("pos", TCHAR_TO_ANSI(*FString::Printf(TEXT("%f %f %f"), Position.X, -Position.Y, Position.Z)));
		}
		if (!Rotation.IsIdentity())
		{
			FrameElement->SetAttribute("quat", TCHAR_TO_ANSI(*FString::Printf(TEXT("%f %f %f %f"), -Rotation.W, Rotation.X, -Rotation.Y, Rotation.Z)));
		}
		auto ElementName = Element->Name();
		if (std::strcmp(ElementName, "body") == 0)
		{
			auto BodyName = Element->Attribute("name");
			CameraElement->SetAttribute("mode", "targetbody");
			CameraElement->SetAttribute("target", BodyName);
		}
		else
		{
			CameraElement->SetAttribute("mode", "track");
		}
		FrameElement->InsertEndChild(CameraElement);
		Element->InsertEndChild(FrameElement);
	}
	else
	{
		tinyxml2::XMLElement* FrameElement = Element->GetDocument()->NewElement("frame");
		tinyxml2::XMLElement* CameraElement = Element->GetDocument()->NewElement("camera");
		CameraElement->SetAttribute("name", TCHAR_TO_ANSI(*Name));
		FTransform CameraTransform = CameraComponent->GetRelativeTransform();
		FVector Position = CameraTransform.GetLocation() * 0.01f;
		const FQuat CamExtra = FQuat(FRotator(0, -90, -90));
		CameraElement->SetAttribute("quat", TCHAR_TO_ANSI(*FString::Printf(TEXT("%f %f %f %f"), CamExtra.W, CamExtra.X, CamExtra.Y, CamExtra.Z)));
		FQuat Rotation = CameraTransform.Rotator().Quaternion();
		if (!Position.IsZero())
		{
			FrameElement->SetAttribute("pos", TCHAR_TO_ANSI(*FString::Printf(TEXT("%f %f %f"), Position.X, -Position.Y, Position.Z)));
		}
		if (!Rotation.IsIdentity())
		{
			FrameElement->SetAttribute("quat", TCHAR_TO_ANSI(*FString::Printf(TEXT("%f %f %f %f"), -Rotation.W, Rotation.X, -Rotation.Y, Rotation.Z)));
		}
		FrameElement->InsertEndChild(CameraElement);
		Element->InsertEndChild(FrameElement);
	}
	return true;
}

bool FMujocoXmlGenerator::VisitSpringArm(USCS_Node* SCSNode, USpringArmComponent* SpringArmComponent, tinyxml2::XMLElement* Element)
{
	FString Name = SCSNode ? SCSNode->GetVariableName().ToString() : SpringArmComponent->GetName();

	if (!SCSNode)
	{
		TArray<USceneComponent*> CameraComponents;
		SpringArmComponent->GetChildrenComponents(true, CameraComponents);
		for (USceneComponent* ChildComponent : CameraComponents)
		{
			if (UCameraComponent* CameraComponent = Cast<UCameraComponent>(ChildComponent))
			{
				if (!VisitCameraComponent(SCSNode, CameraComponent, Element, SpringArmComponent))
				{
					return false;
				}
				break;
			}
		}
	}
	else
	{
		for (USCS_Node* ChildNode : SCSNode->GetChildNodes())
		{
			if (UCameraComponent* CameraComponent = Cast<UCameraComponent>(ChildNode->GetActualComponentTemplate(SpringArmComponent->GetTypedOuter<UBlueprintGeneratedClass>())))
			{
				if (!VisitCameraComponent(ChildNode, CameraComponent, Element, SpringArmComponent))
				{
					return false;
				}
				break;
			}
		}
	}
	return true;
}

bool FMujocoXmlGenerator::VisitBodyComponent(USCS_Node* SCSNode, UMujocoBodyComponent* BodyComponent, tinyxml2::XMLElement* Element)
{
	FString Name = SCSNode ? SCSNode->GetVariableName().ToString() : BodyComponent->GetName();

	tinyxml2::XMLElement* BodyElement = Element->GetDocument()->NewElement("body");
	BodyElement->SetAttribute("name", TCHAR_TO_ANSI(*Name));
	FTransform BodyTransform = BodyComponent->GetRelativeTransform();
	FVector Position = BodyTransform.GetLocation() * 0.01f;
	FQuat Rotation = BodyTransform.Rotator().Quaternion();
	if (!Position.IsZero())
	{
		BodyElement->SetAttribute("pos", TCHAR_TO_ANSI(*FString::Printf(TEXT("%f %f %f"), Position.X, -Position.Y, Position.Z)));
	}
	if (!Rotation.IsIdentity())
	{
		BodyElement->SetAttribute("quat", TCHAR_TO_ANSI(*FString::Printf(TEXT("%f %f %f %f"), -Rotation.W, Rotation.X, -Rotation.Y, Rotation.Z)));
	}

	for (TFieldIterator<FProperty> PropIt(BodyComponent->GetClass()); PropIt; ++PropIt)
	{
		FProperty* Prop = *PropIt;
		const FString AttributeName = GetPropertyAttributeName<UMujocoBodyComponent>(Prop);
		if (AttributeName.IsEmpty())
		{
			continue;
		}
		const FString Value = GetPropertyValueAsString(Prop, BodyComponent);
		if (Value.IsEmpty())
		{
			continue;
		}
		BodyElement->SetAttribute(TCHAR_TO_ANSI(*AttributeName), TCHAR_TO_ANSI(*Value));
	}

	if (BodyComponent->Inertial.IsSet())
	{
		tinyxml2::XMLElement* InertialElement = BodyElement->GetDocument()->NewElement("inertial");
		const FMujocoInertial& InertialStruct = BodyComponent->Inertial.GetValue();
		ApplyStructAttributes(InertialElement, InertialStruct);
		BodyElement->InsertEndChild(InertialElement);
	}
	if (SCSNode)
	{
		for (USCS_Node* ChildNode : SCSNode->GetChildNodes())
		{
			if (UMujocoBodyComponent* ChildBodyComponent = Cast<UMujocoBodyComponent>(ChildNode->GetActualComponentTemplate(BodyComponent->GetTypedOuter<UBlueprintGeneratedClass>())))
			{
				if (!VisitBodyComponent(ChildNode, ChildBodyComponent, BodyElement))
				{
					return false;
				}
			}
			else if (UMujocoGeomComponent* GeomComponent = Cast<UMujocoGeomComponent>(ChildNode->GetActualComponentTemplate(BodyComponent->GetTypedOuter<UBlueprintGeneratedClass>())))
			{
				if (!VisitGeomComponent(ChildNode, GeomComponent, BodyElement))
				{
					return false;
				}
			}
			else if (UMujocoJointComponent* JointComponent = Cast<UMujocoJointComponent>(ChildNode->GetActualComponentTemplate(BodyComponent->GetTypedOuter<UBlueprintGeneratedClass>())))
			{
				if (!VisitJointComponent(ChildNode, JointComponent, BodyElement))
				{
					return false;
				}
			}
			else if (UMujocoSiteComponent* SiteComponent = Cast<UMujocoSiteComponent>(ChildNode->GetActualComponentTemplate(BodyComponent->GetTypedOuter<UBlueprintGeneratedClass>())))
			{
				if (!VisitSiteComponent(ChildNode, SiteComponent, BodyElement))
				{
					return false;
				}
			}
			else if (UCameraComponent* CameraComponent = Cast<UCameraComponent>(ChildNode->GetActualComponentTemplate(BodyComponent->GetTypedOuter<UBlueprintGeneratedClass>())))
			{
				if (!VisitCameraComponent(ChildNode, CameraComponent, BodyElement))
				{
					return false;
				}
			}
			else if (USpringArmComponent* SpringArmComponent = Cast<USpringArmComponent>(ChildNode->GetActualComponentTemplate(BodyComponent->GetTypedOuter<UBlueprintGeneratedClass>())))
			{
				if (!VisitSpringArm(ChildNode, SpringArmComponent, BodyElement))
				{
					return false;
				}
			}
		}
	}
	else
	{
		TArray<USceneComponent*> ChildComponents;
		BodyComponent->GetChildrenComponents(false, ChildComponents);

		TSet<USceneComponent*> UniqueChildrens;
		for (USceneComponent* ChildComponent : ChildComponents)
		{
			UniqueChildrens.Add(ChildComponent);
		}

		for (USceneComponent* ChildComponent : UniqueChildrens)
		{
			if (UMujocoBodyComponent* ChildBodyComponent = Cast<UMujocoBodyComponent>(ChildComponent))
			{
				if (!VisitBodyComponent(nullptr, ChildBodyComponent, BodyElement))
				{
					return false;
				}
			}
			else if (UMujocoGeomComponent* GeomComponent = Cast<UMujocoGeomComponent>(ChildComponent))
			{
				if (!VisitGeomComponent(nullptr, GeomComponent, BodyElement))
				{
					return false;
				}
			}
			else if (UMujocoJointComponent* JointComponent = Cast<UMujocoJointComponent>(ChildComponent))
			{
				if (!VisitJointComponent(nullptr, JointComponent, BodyElement))
				{
					return false;
				}
			}
			else if (UMujocoSiteComponent* SiteComponent = Cast<UMujocoSiteComponent>(ChildComponent))
			{
				if (!VisitSiteComponent(nullptr, SiteComponent, BodyElement))
				{
					return false;
				}
			}
			else if (UCameraComponent* CameraComponent = Cast<UCameraComponent>(ChildComponent))
			{
				if (!VisitCameraComponent(nullptr, CameraComponent, BodyElement))
				{
					return false;
				}
			}
			else if (USpringArmComponent* SpringArmComponent = Cast<USpringArmComponent>(ChildComponent))
			{
				if (!VisitSpringArm(nullptr, SpringArmComponent, BodyElement))
				{
					return false;
				}
			}
		}
	}
	if (std::strcmp(Element->Name(), "mujoco") == 0)
	{
		tinyxml2::XMLElement* WorldBodyElement = Element->FirstChildElement("worldbody");
		ensureMsgf(WorldBodyElement, TEXT("No worldbody element found in mujoco root element"));
		WorldBodyElement->InsertEndChild(BodyElement);
	}
	else if (std::strcmp(Element->Name(), "body") == 0)
	{
		Element->InsertEndChild(BodyElement);
	}
	else
	{
		ensureMsgf(false, TEXT("Unexpected element type"));
		return false;
	}
	return true;
}

template <typename T> TMap<FString, FString> ProcessProperties(void* StructPtr)
{
	TMap<FString, FString> OptionsMap;
	UStruct* Struct = T::StaticStruct();
	for (TFieldIterator<FProperty> PropIt(Struct); PropIt; ++PropIt)
	{
		FProperty* Property = *PropIt;
		const FString AttributeName = GetAttributeMappingForProperty<T>(Property);
		if (AttributeName.IsEmpty())
		{
			continue;
		}
		FName OverridePropName(*FString::Printf(TEXT("bOverride%s"), *Property->GetNameCPP()));
		if (FProperty* OverrideProperty = Struct->FindPropertyByName(OverridePropName))
		{
			void* OverridePtr = OverrideProperty->ContainerPtrToValuePtr<void>(StructPtr);
			bool bOverride = false;
			OverrideProperty->GetValue_InContainer(StructPtr, &bOverride);
			if (!bOverride)
			{
				continue;
			}
		}
		const FString ValueStr = GetPropertyValueAsString(Property, StructPtr);
		OptionsMap.Add(AttributeName, ValueStr);
	}
	return OptionsMap;
}

bool FMujocoXmlGenerator::ParseExportOptions(UMujocoExportOptions* Data, tinyxml2::XMLElement* Element)
{
	if (!Data || !Element)
	{
		return false;
	}
	Element->SetAttribute("model", TCHAR_TO_ANSI(*Data->ModelName));

	TMap<FString, FString> CompilerOptions = ProcessProperties<FMujocoCompileOptions>(&Data->Compiler);
	TMap<FString, FString> MujocoOptions = ProcessProperties<FMujocoOptions>(&Data->Options);
	TMap<FString, FString> SimulationFlags = ProcessProperties<FMujocoSimulationFlags>(&Data->SimulationFlags);

	if (CompilerOptions.Num() > 0)
	{
		tinyxml2::XMLElement* CompilerElement = Element->InsertNewChildElement("compiler");
		for (const auto& [Key, Value] : CompilerOptions)
		{
			CompilerElement->SetAttribute(TCHAR_TO_ANSI(*Key), TCHAR_TO_ANSI(*Value));
		}
	}
	if (MujocoOptions.Num() > 0 || SimulationFlags.Num() > 0)
	{
		tinyxml2::XMLElement* MujocoElement = Element->InsertNewChildElement("option");
		for (const auto& [Key, Value] : MujocoOptions)
		{
			MujocoElement->SetAttribute(TCHAR_TO_ANSI(*Key), TCHAR_TO_ANSI(*Value));
		}
		if (SimulationFlags.Num() > 0)
		{
			tinyxml2::XMLElement* FlagsElement = MujocoElement->InsertNewChildElement("flag");
			for (const auto& [Key, Value] : SimulationFlags)
			{
				FlagsElement->SetAttribute(TCHAR_TO_ANSI(*Key), TCHAR_TO_ANSI(*Value));
			}
		}
	}
	return true;
}

bool FMujocoXmlGenerator::ParseBlueprintAsset(UBlueprint* BlueprintAsset, tinyxml2::XMLElement* Root)
{
	if (!BlueprintAsset || !BlueprintAsset->GeneratedClass)
	{
		return false;
	}
	AActor* DefaultActor = Cast<AActor>(BlueprintAsset->GeneratedClass->GetDefaultObject());
	if (!DefaultActor)
	{
		return false;
	}
	if (!BlueprintAsset->SimpleConstructionScript)
	{
		return false;
	}
	if (UBlueprintGeneratedClass* BPGeneratedClass = Cast<UBlueprintGeneratedClass>(BlueprintAsset->GeneratedClass))
	{
		for (USCS_Node* SCSNode : BlueprintAsset->SimpleConstructionScript->GetRootNodes())
		{
			if (!SCSNode)
			{
				continue;
			}
			if (UMujocoBodyComponent* BodyComp = Cast<UMujocoBodyComponent>(SCSNode->GetActualComponentTemplate(BPGeneratedClass)))
			{
				if (!VisitBodyComponent(SCSNode, BodyComp, Root))
				{
					return false;
				}
			}
			else if (UMujocoActuatorComponent* ActuatorComp = Cast<UMujocoActuatorComponent>(SCSNode->GetActualComponentTemplate(BPGeneratedClass)))
			{
				if (!VisitActuatorComponent(SCSNode, ActuatorComp, Root))
				{
					return false;
				}
			}
			else if (UMujocoEqualityComponent* EqualityComp = Cast<UMujocoEqualityComponent>(SCSNode->GetActualComponentTemplate(BPGeneratedClass)))
			{
				if (!VisitEqualityComponent(SCSNode, EqualityComp, Root))
				{
					return false;
				}
			}
			else if (UMujocoTendonComponent* TendonComp = Cast<UMujocoTendonComponent>(SCSNode->GetActualComponentTemplate(BPGeneratedClass)))
			{
				if (!VisitTendonComponent(SCSNode, TendonComp, Root))
				{
					return false;
				}
			}
		}
	}
	return true;
}

bool FMujocoXmlGenerator::ParseActorAsset(AActor* Actor, tinyxml2::XMLElement* Root)
{
	if (!Actor)
	{
		return false;
	}
	for (UActorComponent* Component : Actor->GetComponents())
	{
		if (UMujocoBodyComponent* BodyComp = Cast<UMujocoBodyComponent>(Component))
		{
			if (BodyComp->GetAttachParent())
			{
				continue;
			}
			if (!VisitBodyComponent(nullptr, BodyComp, Root))
			{
				return false;
			}
		}
		else if (UMujocoActuatorComponent* ActuatorComp = Cast<UMujocoActuatorComponent>(Component))
		{
			if (!VisitActuatorComponent(nullptr, ActuatorComp, Root))
			{
				return false;
			}
		}
		else if (UMujocoEqualityComponent* EqualityComp = Cast<UMujocoEqualityComponent>(Component))
		{
			if (!VisitEqualityComponent(nullptr, EqualityComp, Root))
			{
				return false;
			}
		}
		else if (UMujocoTendonComponent* TendonComp = Cast<UMujocoTendonComponent>(Component))
		{
			if (!VisitTendonComponent(nullptr, TendonComp, Root))
			{
				return false;
			}
		}
	}
	return true;
}

TUniquePtr<tinyxml2::XMLDocument> FMujocoXmlGenerator::GenerateMujocoXml(
	const TObjectPtr<UMujocoExportOptions>& ExportOptions,
	const TArray<UObject*>& Objects,
	const FString& ExportFilename,
	TMap<FString, FString> ContactExclusion)
{
	TUniquePtr<tinyxml2::XMLDocument> Doc = MakeUnique<tinyxml2::XMLDocument>();
	Doc->InsertFirstChild(Doc->NewDeclaration("xml version=\"1.0\" encoding=\"utf-8\""));
	tinyxml2::XMLElement* Root = Doc->NewElement("mujoco");
	tinyxml2::XMLElement* AssetRoot = Doc->NewElement("asset");
	tinyxml2::XMLElement* WorldBodyRoot = Doc->NewElement("worldbody");
	ParseExportOptions(ExportOptions, Root);
	Root->InsertEndChild(AssetRoot);
	Root->InsertEndChild(WorldBodyRoot);
	Root->InsertEndChild(Doc->NewElement("equality"));
	Root->InsertEndChild(Doc->NewElement("tendon"));
	Root->InsertEndChild(Doc->NewElement("actuator"));

	// TODO Refacto using property from the MujocoActor aka the robot
	// TODO Maybe using a ParentObject as parameter which holds that information?
	// TODO Need actor refactoring anyway - Merge Pawn and Mujoco Actor?
	if (!ContactExclusion.IsEmpty())
	{
		Root->InsertEndChild(Doc->NewElement("contact"));

		for (auto& [Body1, Body2] : ContactExclusion)
		{
			// Don't add empty strings to the map please!
			if (Body1.IsEmpty() || Body2.IsEmpty()) continue;

			// Create the contact exclusion - formatted as (SO100 example)
			// <exclude body1="Body_Base" body2="Body_Rotation_Pitch"/> 
			const auto Exclude = Doc->NewElement("exclude");
			Exclude->SetAttribute("body1", TCHAR_TO_ANSI(*Body1));
			Exclude->SetAttribute("body2", TCHAR_TO_ANSI(*Body2));

			// Add exclusion to the contact tag
			Root->FirstChildElement("contact")->InsertEndChild(Exclude); 
		}
	}
	
	if (ExportOptions->bAddSkyBox)
	{
		tinyxml2::XMLElement* TextureElement = AssetRoot->GetDocument()->NewElement("texture");
		TextureElement->SetAttribute("type", "skybox");
		TextureElement->SetAttribute("builtin", "gradient");
		TextureElement->SetAttribute("rgb1", "0.3 0.5 0.7");
		TextureElement->SetAttribute("rgb2", "0 0 0");
		TextureElement->SetAttribute("width", "512");
		TextureElement->SetAttribute("height", "3072");
		AssetRoot->InsertEndChild(TextureElement);
		tinyxml2::XMLElement* LightElement = WorldBodyRoot->GetDocument()->NewElement("light");
		LightElement->SetAttribute("pos", "0 0 1.5");
		LightElement->SetAttribute("dir", "0 0 -1");
		LightElement->SetAttribute("directional", "true");
		WorldBodyRoot->InsertEndChild(LightElement);
	}
	if (ExportOptions->bAddGroundPlane)
	{
		tinyxml2::XMLElement* TextureElement = AssetRoot->GetDocument()->NewElement("texture");
		TextureElement->SetAttribute("type", "2d");
		TextureElement->SetAttribute("name", "groundplane");
		TextureElement->SetAttribute("builtin", "checker");
		TextureElement->SetAttribute("mark", "edge");
		TextureElement->SetAttribute("rgb1", "0.2 0.3 0.4");
		TextureElement->SetAttribute("rgb2", "0.1 0.2 0.3");
		TextureElement->SetAttribute("markrgb", "0.8 0.8 0.8");
		TextureElement->SetAttribute("width", "300");
		TextureElement->SetAttribute("height", "300");
		AssetRoot->InsertEndChild(TextureElement);
		tinyxml2::XMLElement* MaterialElement = AssetRoot->GetDocument()->NewElement("material");
		MaterialElement->SetAttribute("name", "groundplane");
		MaterialElement->SetAttribute("texture", "groundplane");
		MaterialElement->SetAttribute("texuniform", "true");
		MaterialElement->SetAttribute("texrepeat", "100 100");
		MaterialElement->SetAttribute("reflectance", "0.2");
		AssetRoot->InsertEndChild(MaterialElement);
		tinyxml2::XMLElement* GroundPlaneElement = WorldBodyRoot->GetDocument()->NewElement("geom");
		GroundPlaneElement->SetAttribute("name", "floor");
		GroundPlaneElement->SetAttribute("size", "0 0 0.05");
		GroundPlaneElement->SetAttribute("type", "plane");
		GroundPlaneElement->SetAttribute("material", "groundplane");
		WorldBodyRoot->InsertEndChild(GroundPlaneElement);
	}
	Doc->InsertEndChild(Root);
	for (UObject* Object : Objects)
	{
		if (UBlueprint* Blueprint = Cast<UBlueprint>(Object))
		{
			ParseBlueprintAsset(Blueprint, Root);
		}
		else if (AActor* Actor = Cast<AActor>(Object))
		{
			ParseActorAsset(Actor, Root);
		}
	}
	TSet<FString> ExportedMeshes;
	for (tinyxml2::XMLElement* MeshElement = AssetRoot->FirstChildElement("mesh"); MeshElement; MeshElement = MeshElement->NextSiblingElement("mesh"))
	{
		FString MeshPath = MeshElement->Attribute("file");

		const auto& GeomIter = ObjectMap.Find(MeshPath);
		if (!GeomIter->IsValid())
		{
			UE_LOG(LogTemp, Warning, TEXT("Failed to load geom %s not found"), *MeshPath);
			continue;
		}

		auto* GeomObj = Cast<UMujocoGeomComponent>(GeomIter->Get());
		if (!GeomObj)
		{
			UE_LOG(LogTemp, Warning, TEXT("Failed to load geom %s invalid ptr"), *MeshPath);
			continue;
		}
		FString MeshName = FPaths::GetBaseFilename(MeshPath) + TEXT("_") + FString::FromInt(GeomObj->GetUniqueID());
		FString MeshFilename = FPaths::Combine(FPaths::GetPath(ExportFilename), ExportOptions->Compiler.AssetDir, MeshName + TEXT(".obj"));
		MeshFilename = FPaths::ConvertRelativePathToFull(MeshFilename);
		if (!ExportedMeshes.Contains(MeshName))
		{
			ExportedMeshes.Add(MeshName);
			if (UMujocoStaticMesh* MujocoMesh = Cast<UMujocoStaticMesh>(GeomObj->MujocoStaticMesh))
			{
				FFileHelper::SaveStringToFile(MujocoMesh->ObjFileData, *MeshFilename);
			}
		}
		MeshElement->SetAttribute("file", TCHAR_TO_ANSI(*MeshFilename));
	}
	return MoveTemp(Doc);
}