UCLASS()
class BLANKCPP_API AVRurdfRobot : public AActor
{
    GENERATED_BODY()

protected:
    // Called when the game starts or when spawned
    virtual void BeginPlay() override;

public:
    // Sets default values for this actor's properties
    AVRurdfRobot();

    // Called every frame
    virtual void Tick(float DeltaTime) override;

    // Called at construction
    virtual void OnConstruction(const FTransform &Transform) override;

    // urdf link structure
    struct FurdfLink {
        FString Name;

        struct {
            FVector Location;
            FRotator Rotation;
            FString MeshName;
            FVector MeshScale;

            // unused, apply material manually
            struct {
                FString Name;
                FColor Color;
                FString Texture;
            } Material;
        } Visual;

        struct {
            FVector Location;
            FRotator Rotation;
            FString MeshName;
            FVector MeshScale;
        } Collision;

        struct {
            FVector Location;
            FRotator Rotation;
            float Mass;
        } Inertial;

        bool operator== (const FurdfLink &Link) {
            return Name.Equals(Link.Name);
        }

        bool operator== (const FString &String) {
            return Name.Equals(String);
        }
    };

    // urdf join structure
    struct FurdfJoint {
        FString Name;
        FString Type;
        FString Parent;
        FString Child;

        FVector Location;
        FRotator Rotation;
        FVector Axis;

        // Unused
        float LowerLimit;
        float UpperLimit;
        float Effort;
        float Velocity;

        // Unused
        float k_velocity;
        float damping;
        float friction;

        bool operator== (const FurdfJoint &Joint) {
            return Name.Equals(Joint.Name);
        }

        bool operator== (const FString &String) {
            return Name.Equals(String);
        }
    };

    // Tree structure
    struct FurdfNode {
        FurdfLink Link;
        FurdfJoint Joint;

        FurdfNode* Parent;
        TArray<FurdfNode*> Children;

        ~FurdfNode() {
            for (int i = 0; i < Children.Num(); i++)
                delete Children[i];
        }
    };

    // Root node, no parent, only children
    FurdfNode* RootNode;
};