/*****************************************************************************
 * $CAMITK_LICENCE_BEGIN$
 *
 * CamiTK - Computer Assisted Medical Intervention ToolKit
 * (c) 2001-2025 Univ. Grenoble Alpes, CNRS, Grenoble INP - UGA, TIMC, 38000 Grenoble, France
 *
 * Visit http://camitk.imag.fr for more information
 *
 * This file is part of CamiTK.
 *
 * CamiTK is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License version 3
 * only, as published by the Free Software Foundation.
 *
 * CamiTK is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License version 3 for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * version 3 along with CamiTK.  If not, see <http://www.gnu.org/licenses/>.
 *
 * $CAMITK_LICENCE_END$
 ****************************************************************************/

#include "TransformationManager.h"
#include "FrameOfReference.h"
#include "Transformation.h"
#include "Log.h"

#include<QHash>
#include<QSet>
#include<QVariantMap>
#include<QVariantList>



namespace camitk {

// class members definition
std::shared_ptr<FrameOfReference> TransformationManager::worldFrame;
std::vector<std::shared_ptr<FrameOfReference>> TransformationManager::frames;
std::vector<std::shared_ptr<Transformation>> TransformationManager::transformations;
QHash<QUuid, FrameOfReference*> TransformationManager::frameMap;
QHash<QUuid, Transformation*> TransformationManager::transformationMap;
QHash<const Transformation*, QVector<std::shared_ptr<Transformation>>> TransformationManager::transformationSources;
QHash<const FrameOfReference*, QHash<const FrameOfReference*, Transformation*>> TransformationManager::transformationFromTo;
std::unordered_set<const Transformation*> TransformationManager::defaultIdentityToWorldTransformations;



// -------------------- toString --------------------
QString TransformationManager::toString() {
    // get the list of frames
    const QVector<FrameOfReference*> framesToDisplay = TransformationManager::getFramesOfReference();

    // get the list of Transformations
    const QVector<Transformation*> allTransformations = TransformationManager::getTransformations();

    // remove composite and inverse
    QVector<const Transformation*> transformationsToDisplay;
    std::copy_if(allTransformations.begin(), allTransformations.end(), std::back_inserter(transformationsToDisplay), [](auto & tr) {
        return (!TransformationManager::isCompositeTransformation(tr) && !TransformationManager::isInverseTransformation(tr));
    });

    // sort transformation A and B by from name and then by to name if from is the same
    std::sort(transformationsToDisplay.begin(), transformationsToDisplay.end(), [](const Transformation * trA, const Transformation * trB) {
        bool sameFrom = (trA->getFrom()->getName() == trB->getFrom()->getName());
        if (sameFrom) {
            return trA->getTo()->getName().compare(trB->getTo()->getName()) < 0;
        }
        else {
            return trA->getFrom()->getName().compare(trB->getFrom()->getName()) < 0;
        }
    });

    // print state using description and uuid
    QStringList msgName;
    QStringList msgUuid;
    msgName << QString::number(framesToDisplay.size()) + " frame" + ((framesToDisplay.size() > 1) ? "s:" : ":");
    msgUuid << "";
    for (const FrameOfReference* fr : framesToDisplay) {
        msgName << fr->getName();
        msgUuid << ((fr == TransformationManager::getWorldFrame()) ? "World" : fr->getUuid().toString().mid(1, 8));
    }

    msgName << ((transformationsToDisplay.size() == 0) ? "no" : QString::number(transformationsToDisplay.size()))
            + " transformation" + ((transformationsToDisplay.size() > 1) ? "s" : "")
            + ((transformationsToDisplay.size() == 0) ? "." : ":");
    msgUuid << "";
    for (const Transformation* tr : transformationsToDisplay) {
        msgName << tr->getFrom()->getName() + " -> " + tr->getTo()->getName()
                + (TransformationManager::isDefaultIdentityToWorld(tr) ? " (default to world)" : " (user defined)");
        msgUuid << ((tr->getFrom() == TransformationManager::getWorldFrame()) ? "World" : tr->getFrom()->getUuid().toString().mid(1, 8)) + " -> "
                + ((tr->getTo() == TransformationManager::getWorldFrame()) ? "World" : tr->getTo()->getUuid().toString().mid(1, 8))
                + (TransformationManager::isDefaultIdentityToWorld(tr) ? " (default)" : "");
    }

    int maxLengthMsgName = std::max_element(msgName.begin(), msgName.end(), [](QString lhs, QString rhs) {
        return lhs.length() < rhs.length();
    })->length();
    int maxLengthMsgUuid = std::max_element(msgUuid.begin(), msgUuid.end(), [](QString lhs, QString rhs) {
        return lhs.length() < rhs.length();
    })->length();

    QStringList formattedOutput;
    formattedOutput << "Transformation manager state:";
    for (int i = 0; i < msgName.size(); i++) {
        formattedOutput << msgName.at(i).leftJustified(maxLengthMsgName) + ((msgUuid.at(i).size() > 0) ? " \t| " + msgUuid.at(i).leftJustified(maxLengthMsgUuid) : "");
    }
    return formattedOutput.join("\n");
}


// -------------------- getWorldFrame --------------------
const FrameOfReference* TransformationManager::getWorldFrame() {
    if (worldFrame == nullptr) {
        // Creating WorldFrame with a specific UUID so that when it is reloaded from a workspace file, it is identified as World
        // event if CamiTK was restarted 01234567-89ab-cdef-0123-456789abcdef
        worldFrame = addFrameOfReference(QUuid("{01234567-89ab-cdef-0123-456789abcdef}"), "🌐 World", "CamiTK World Frame");
        worldFrame->init(); // force world frame index to 0 and color to white
    }

    return worldFrame.get();
}

// -------------------- getTransformations --------------------
QVector<Transformation*> TransformationManager::getTransformations() {
    QVector<Transformation*> allExistingTransformations;
    std::ranges::transform(transformations, std::back_inserter(allExistingTransformations), [](const auto & tr) {
        return tr.get();
    });
    return allExistingTransformations;
}

// -------------------- getFrameOfReferenceOwnership --------------------
std::shared_ptr<FrameOfReference> TransformationManager::getFrameOfReferenceOwnership(const QUuid& uuid) {
    if (frameMap.contains(uuid)) {
        return getFrameOfReferenceOwnership(frameMap[uuid]);
    }
    else {
        return nullptr;
    }
}

std::shared_ptr<FrameOfReference> TransformationManager::getFrameOfReferenceOwnership(const FrameOfReference* fr) {
    auto it = std::ranges::find_if(frames, [fr](const auto & itFrame) {
        return itFrame.get() == fr;
    });
    if (it != frames.end()) {
        return *it;
    }
    else {
        return nullptr;
    }
}

// -------------------- addFrameOfReference --------------------
std::shared_ptr<FrameOfReference> TransformationManager::addFrameOfReference(const FrameOfReference& fr) {
    // Use copy constructor
    std::shared_ptr<FrameOfReference> frame(new FrameOfReference(fr));
    // insert in internal structures
    frameMap[frame->getUuid()] = frame.get();
    frames.push_back(frame);
    return frame;
}

std::shared_ptr<FrameOfReference> TransformationManager::addFrameOfReference(QString name, QString description) {
    return addFrameOfReference(QUuid::createUuid(), name, description);
}

std::shared_ptr<FrameOfReference> TransformationManager::addFrameOfReference(QUuid uuid, QString name, QString description, int dimensions, const AnatomicalOrientation& ao, std::vector<Unit> units) {
    std::shared_ptr<FrameOfReference> frame;
    if (frameMap.contains(uuid)) {
        frame = getFrameOfReferenceOwnership(uuid);
        // This frame was already added
        CAMITK_TRACE_IF_ALT(frame != worldFrame, QString("FrameOfReference %1 already exists, returning existing (of name %2)").arg(uuid.toString()).arg(frame->getName()));
    }
    else {
        // Not using make_shared because of private constructor issue with it
        frame = std::shared_ptr<FrameOfReference>(new FrameOfReference(uuid, name, description, dimensions, ao, units));
        frameMap[frame->getUuid()] = frame.get();
        frames.push_back(frame);
    }
    return frame;
}

std::shared_ptr<FrameOfReference> TransformationManager::addFrameOfReference(const QVariant& variant) {
    std::shared_ptr<FrameOfReference> frame(new FrameOfReference());
    frame->fromVariant(variant);
    if (!frameMap.contains(frame->getUuid())) {
        frameMap[frame->getUuid()] = frame.get();
        frames.push_back(frame);
    }
    else { // Already loaded, use the existing one
        frame = getFrameOfReferenceOwnership(frame->getUuid());
        CAMITK_TRACE_IF_ALT(frame != worldFrame, QString("FrameOfReference %1 already exists, returning existing (of name %2)").arg(frame->getUuid().toString()).arg(frame->getName()));
    }

    return frame;
}

// -------------------- getFramesOfReference --------------------
QVector<FrameOfReference*> TransformationManager::getFramesOfReference() {
    QVector<FrameOfReference*> allExistingFrames;
    std::ranges::transform(frames, std::back_inserter(allExistingFrames), [](const auto & fr) {
        return fr.get();
    });
    return allExistingFrames;
}

// -------------------- hasPath --------------------
bool TransformationManager::hasPath(const FrameOfReference* from, const FrameOfReference* to) {
    if (from == nullptr || to == nullptr) { // Trivial case
        return false;
    }
    if (from == to) { // Trivial case
        return true;
    }
    QSet<const FrameOfReference*> explored, reachable, toExplore;
    toExplore.insert(from);
    while (!toExplore.empty()) {
        for (auto frame : toExplore) {
            if (transformationFromTo.contains(frame)) {
                for (auto* dest : transformationFromTo[frame].keys()) {
                    if (!explored.contains(dest)) {
                        if (dest == to) {
                            // found a path!
                            return true;
                        }
                        else {
                            reachable.insert(dest);
                        }
                    }
                }
                explored.insert(frame);
            }
        }
        // all frames in toExplore just have been explored, start again with the reachable list
        toExplore = reachable;
        // remove the already explored (e.g. the inverse)
        toExplore.subtract(explored);
    }

    // all possible paths were explored, none were found
    return false;
}

// -------------------- getPath --------------------
QVector<std::shared_ptr<Transformation>> TransformationManager::getPath(const FrameOfReference* source, const FrameOfReference* destination) {
    if (source == nullptr || destination == nullptr) { // Trivial case
        return {};
    }
    if (source == destination) { // Trivial case
        return {};
    }

    QSet<const FrameOfReference*> explored, reachable, toExplore;

    // pathTo contains the ordered list of transformations that link source to the frame (key)
    //
    // Given the following graph, with source = Ⓐ
    // Ⓐ -- T1 --> Ⓑ -- T2 --> Ⓒ...
    //               \
    //                -- T3 --> Ⓓ...
    //
    // then pathTo will contain
    // { Ⓐ : [],
    //   Ⓑ : [T1],
    //   Ⓒ : [T1, T2],
    //   Ⓓ : [T1, T3]
    // }
    QHash<const FrameOfReference*, QVector<Transformation*>> pathTo;

    toExplore.insert(source);
    pathTo[source] = {};

    while (!toExplore.empty()) {
        // For each frame to explore (not yet explored)
        for (auto frame : toExplore) {
            if (transformationFromTo.contains(frame)) {
                // find all frames that can be reached from the current frame using a single transformation
                for (auto* dest : transformationFromTo[frame].keys()) {
                    // If we did not explore it yet _and_ the transformation is not a composite
                    // (not taking composite transformation allows one to avoid complex path that include shortcut+inverse)
                    if (!explored.contains(dest) && !isCompositeTransformation(transformationFromTo[frame][dest])) {
                        // create a new key in pathTo for dest (i.e. a new QVector of Transformation that link source to dest)
                        // 1) copy the current path from the source to the current frame
                        pathTo[dest] = pathTo[frame];
                        // 2) and add the transformation from the current frame to dest
                        pathTo[dest].push_back(transformationFromTo[frame][dest]);
                        // If we found destination, that's it
                        if (dest == destination) {
                            // Get the shared_ptrs from the raw pointers to return the path
                            QVector<std::shared_ptr<Transformation>> result;
                            result.reserve(pathTo.size());
                            for (const auto* tr : pathTo[destination]) {
                                result.push_back(getTransformationSharedPtr(tr));
                            }
                            return result;
                        }
                        else {
                            reachable.insert(dest);
                        }
                    }
                }
                explored.insert(frame);
            }
        }
        // all frames in toExplore just have been explored, start again with the reachable list
        toExplore = reachable;
        // remove the already explored (e.g. the inverse)
        toExplore.subtract(explored);
    }
    // all possible paths were explored, none were found
    return {};
}

// -------------------- isDefaultIdentityToWorld --------------------
bool TransformationManager::isDefaultIdentityToWorld(const Transformation* tr) {
    return defaultIdentityToWorldTransformations.contains(tr);
}

// -------------------- getTransformation --------------------
Transformation* TransformationManager::getTransformation(const FrameOfReference* from, const FrameOfReference* to) {
    // If we have it already, return it
    if (transformationFromTo.contains(from) && transformationFromTo[from].contains(to)) {
        return transformationFromTo[from][to];
    }

    // Find a path between those frames, and compute a transformation from the path
    // If there is no path this will return nullptr
    // otherwise it will return a composite Transformation which will update itself if any of its source Transformations is updated
    return addCompositeTransformation(getPath(from, to)).get();
}

// -------------------- getTransformationSharedPtr --------------------
std::shared_ptr<Transformation> TransformationManager::getTransformationSharedPtr(const Transformation* tr) {
    // Refuse to give owning pointer for composite
    if (tr == nullptr || isCompositeTransformation(tr)) {
        return nullptr;
    }

    auto it = std::ranges::find_if(transformations, [tr](const auto & itTransformation) {
        return itTransformation.get() == tr;
    });
    if (it != transformations.end()) {
        return *it;
    }
    else {
        return nullptr;
    }
}

// -------------------- getTransformationOwnership --------------------
std::shared_ptr<Transformation> TransformationManager::getTransformationOwnership(const Transformation* tr) {
    // Refuse to give owning pointer for default identity to world
    if (isDefaultIdentityToWorld(tr)) {
        return nullptr;
    }
    else {
        return getTransformationSharedPtr(tr);
    }
}

std::shared_ptr<Transformation> TransformationManager::getTransformationOwnership(const QUuid& uuid) {
    if (transformationMap.contains(uuid) && !hasSources(transformationMap[uuid])) {
        return getTransformationOwnership(transformationMap[uuid]);
    }
    else {
        return nullptr;
    }
}

std::shared_ptr<Transformation> TransformationManager::getTransformationOwnership(const FrameOfReference* from, const FrameOfReference* to) {
    if (transformationFromTo.contains(from) && transformationFromTo[from].contains(to)) {
        return getTransformationOwnership(transformationFromTo[from][to]->getUuid());
    }
    else {
        return nullptr;
    }
}

// -------------------- ensurePathToWorld --------------------
void TransformationManager::ensurePathToWorld(const FrameOfReference* frame) {
    // If there is no possible Transformation between the frame and the world frame,
    // just create a new default Identity Transformation between this frame and worldFrame
    // and set the default flag to this new transformation and its inverse
    if (frame && !hasPath(frame, getWorldFrame())) {
        std::shared_ptr<Transformation> tr = addTransformation(frame, getWorldFrame());
        defaultIdentityToWorldTransformations.insert(tr.get());
        defaultIdentityToWorldTransformations.insert(transformationFromTo[getWorldFrame()][frame]);
    }
}

// -------------------- preferredDefaultIdentityToWorldLink --------------------
bool TransformationManager::preferredDefaultIdentityToWorldLink(const FrameOfReference* frame) {
    // 1. try create a new direct transformation: this will remove any path from frame to world that have
    // a default identity transformation (it won't work if there is none)
    std::shared_ptr<Transformation> newTr = addTransformation(frame, getWorldFrame());
    if (newTr != nullptr) {
        // 2. now that we have direct transformation, mark it as default
        defaultIdentityToWorldTransformations.insert(newTr.get());
        defaultIdentityToWorldTransformations.insert(transformationFromTo[getWorldFrame()][frame]);
    }
    return (newTr != nullptr);
}

// -------------------- getDirectTransformations --------------------
QVector<Transformation*> TransformationManager::getDirectTransformations() {
    QVector<Transformation*> directTransformations;
    for (const auto& tr : transformations) {
        if (transformationSources[tr.get()].empty()) {
            directTransformations.push_back(tr.get());
        }
    }
    return directTransformations;
}

// -------------------- registerTransformation --------------------
std::shared_ptr<Transformation> TransformationManager::registerTransformation(Transformation* tr, const QVector<std::shared_ptr<Transformation>>& sources) {
    // create the owner shared_ptr
    std::shared_ptr<Transformation> sharedPtr(tr);

    // store in the members
    transformations.push_back(sharedPtr);
    transformationMap[tr->getUuid()] = tr;
    transformationSources[tr] = sources;
    transformationFromTo[tr->getFrom()][tr->getTo()] = tr;

    return sharedPtr;
}

// -------------------- addCompositeTransformation --------------------
std::shared_ptr<Transformation> TransformationManager::addCompositeTransformation(QVector<std::shared_ptr<Transformation>> transforms) {
    // This function should never be called with less than two transformations
    if (transforms.size() < 2) {
        return nullptr;
    }

    // Chain matrices at the vtkTransform level
    auto transform = vtkSmartPointer<vtkTransform>::New();
    transform->PostMultiply();
    for (const auto& tr : transforms) {
        transform->Concatenate(tr->getTransform());
    }

    // Sources contain the composed transformation's sources (if they have sources), or themselves (if they don't)
    QVector<std::shared_ptr<Transformation>> sources;
    for (const auto& tr : transforms) {
        if (hasSources(tr.get())) {
            // if there is a source, there must be only one, as tr is an inverse
            sources.push_back(transformationSources[tr.get()].at(0));
        }
        else {
            sources.push_back(tr);
        }
    }

    // Register this Transformation
    std::shared_ptr<Transformation> result = registerTransformation(new Transformation(transforms[0]->from, transforms.back()->to, transform), sources);

    // Register its inverse (which has the same sources as result is a composite)
    registerTransformation(result->getInverse(), sources);

    return result;
}

// -------------------- removeDefaultPaths --------------------
bool TransformationManager::removeDefaultPaths(const FrameOfReference* from, const FrameOfReference* to) {
    const Transformation* tr = getTransformation(from, to);
    if (tr == nullptr) {
        // There is no path!
        return false;
    }
    // Keep the list of share_ptr we want to remove
    std::unordered_set<const Transformation*> defaultTransformationsToRemove;

    //-- 1. if tr is a default, just remove it and its inverse
    if (isDefaultIdentityToWorld(tr)) {
        // If we have a default Transformation, remove it
        defaultTransformationsToRemove.insert(getInverseTransformation(tr));
        defaultTransformationsToRemove.insert(tr);
    }
    else {
        //-- 2. if tr is not a default, delete all its sources that are defaults and their inverse
        for (const auto& source : transformationSources[tr]) {
            if (isDefaultIdentityToWorld(source.get())) {
                defaultTransformationsToRemove.insert(getInverseTransformation(source.get()));
                defaultTransformationsToRemove.insert(source.get());
            }
        }
    }

    if (defaultTransformationsToRemove.empty()) {
        return true; // nothing to delete
    }

    //-- 3. remove all composites (may remove tr if it is a composite itself)
    cleanupCompositeTransformations();

    //-- 4. remove the default transformations found in the given path
    for (const auto& toRemove : defaultTransformationsToRemove) {
        // We need to call getExistingTransformation before removing it from internal structures
        std::shared_ptr<Transformation> toRemoveSharedPtr = getTransformationSharedPtr(toRemove);
        removeTransformationFromInternalStructures(toRemove);
        std::erase(transformations, toRemoveSharedPtr);
    }

    return true;
}

// -------------------- addTransformation --------------------
std::shared_ptr<Transformation> TransformationManager::addTransformation(const std::shared_ptr<FrameOfReference>& from, const std::shared_ptr<FrameOfReference>& to, vtkSmartPointer<vtkTransform> vtkTr) {
    if (from == nullptr || to == nullptr || from == to || vtkTr == nullptr) {
        CAMITK_WARNING_ALT(QString("Cannot create Transformation:\nInvalid/identical frames of reference or empty transformation"));
        return nullptr;
    }
    // If there is already a path between from and to, try to remove it
    removeDefaultPaths(from.get(), to.get());
    if (!hasPath(from.get(), to.get())) {
        // Ok, register the Transformation and its inverse
        std::shared_ptr<Transformation> result = registerTransformation(new Transformation(from, to, vtkTr));
        registerTransformation(result->getInverse(), {result});
        return result;
    }
    else {
        CAMITK_WARNING_ALT(QString("Cannot create Transformation:\nThere is already a defined path from %1 to %2").arg(from->getName()).arg(to->getName()));
        return nullptr;
    }
}

std::shared_ptr<Transformation> TransformationManager::addTransformation(const QVariant& variant) {
    // BE CAREFUL: we have to manually set the right from and to FrameOfReference pointers using
    // the content of the QVariant:
    // - the Transformation cannot find out the FrameOfReference pointer from the UUID stored in the QVariant
    // - TransformationManager can get the pointer from the UUID

    QVariantMap trMap = variant.toMap();
    Transformation* tr = new Transformation(getFrameOfReferenceOwnership(trMap.value("from").toUuid()), getFrameOfReferenceOwnership(trMap.value("to").toUuid()), QUuid());
    // Basic validity checks
    if (tr == nullptr || tr->getFrom() == nullptr || tr->getTo() == nullptr || tr->getFrom() == tr->getTo()) {
        delete tr;
        return nullptr;
    }

    // Now load the rest of the data (uuid, matrix...)
    tr->fromVariant(variant);

    // Check UUID is valid and not already registered, and vtkTransform is not null
    if (tr->getUuid().isNull() || transformationMap.contains(tr->getUuid()) || tr->getTransform() == nullptr) {
        delete tr;
        return nullptr;
    }

    // Ensure there is no existing path already
    removeDefaultPaths(tr->getFrom(), tr->getTo());
    if (!hasPath(tr->getFrom(), tr->getTo())) {
        // Register the Transformation and its inverse
        std::shared_ptr<Transformation> registeredTr = registerTransformation(tr);
        std::shared_ptr<Transformation> registeredInverseTr = registerTransformation(registeredTr->getInverse(), {registeredTr});

        // If those were defaultIdentityToWorld Transformations, add them to the set
        if (trMap.value("defaultIdentityToWorld", false).toBool()) {
            defaultIdentityToWorldTransformations.insert(registeredTr.get());
            defaultIdentityToWorldTransformations.insert(registeredInverseTr.get());
        }
        return registeredTr;
    }
    else {
        CAMITK_WARNING_ALT(QString("Cannot create Transformation:\nThere is already a defined path from %1 to %2").arg(tr->from->getName()).arg(tr->to->getName()));
        delete tr;
        return nullptr;
    }
}

std::shared_ptr<Transformation> TransformationManager::addTransformation(const std::shared_ptr<FrameOfReference>& from, const std::shared_ptr<FrameOfReference>& to, const vtkMatrix4x4* matrix) {
    vtkSmartPointer<vtkTransform> vtkTr = vtkSmartPointer<vtkTransform>::New();
    // Duplicate the matrix (to avoid unmanaged matrix changes)
    vtkSmartPointer<vtkMatrix4x4> newMatrix = vtkSmartPointer<vtkMatrix4x4>::New();
    newMatrix->DeepCopy(matrix);
    vtkTr->SetMatrix(newMatrix);
    return addTransformation(from, to, vtkTr);
}

std::shared_ptr<Transformation> TransformationManager::addTransformation(const std::shared_ptr<FrameOfReference>& from, const std::shared_ptr<FrameOfReference>& to) {
    return addTransformation(from, to, vtkSmartPointer<vtkTransform>::New());
}

std::shared_ptr<Transformation> TransformationManager::addTransformation(const FrameOfReference* from, const FrameOfReference* to) {
    return addTransformation(getFrameOfReferenceOwnership(from), getFrameOfReferenceOwnership(to));
}

std::shared_ptr<Transformation> TransformationManager::addTransformation(const FrameOfReference* from, const FrameOfReference* to, vtkSmartPointer<vtkTransform> vtkTr) {
    return addTransformation(getFrameOfReferenceOwnership(from), getFrameOfReferenceOwnership(to), vtkTr);
}

std::shared_ptr<Transformation> TransformationManager::addTransformation(const FrameOfReference* from, const FrameOfReference* to, const vtkMatrix4x4* matrix) {
    return addTransformation(getFrameOfReferenceOwnership(from), getFrameOfReferenceOwnership(to), matrix);
}

// ---------------------- removeTransformationFromInternalStructures ------------------
void TransformationManager::removeTransformationFromInternalStructures(const Transformation* trToRemove) {
    // Remove it from transformationFromTo
    transformationFromTo[trToRemove->getFrom()].remove(trToRemove->getTo());
    // Remove its sources from transformationSources
    transformationSources.remove(trToRemove);
    // If present, remove it from the set of default Transformations
    defaultIdentityToWorldTransformations.erase(trToRemove);
    transformationMap.remove(trToRemove->getUuid());
}

// ---------------------- cleanupCompositeTransformations ------------------
void TransformationManager::cleanupCompositeTransformations() {
    // Helper lambda that will remove the provided Transformation from internal structure and return true if it is a composite, or return false otherwise
    auto checkAndRemoveCompositeTransformation = [](const std::shared_ptr<Transformation>& tr) {
        if (isCompositeTransformation(tr.get())) {
            removeTransformationFromInternalStructures(tr.get());
            return true;
        }
        else {
            return false;
        }
    };
    // erase composite transforms from the ownership (the line below) and the internals (done in the lambda)
    std::erase_if(transformations, checkAndRemoveCompositeTransformation);
}

// ---------------------- cleanupFramesAndTransformations ------------------
void TransformationManager::cleanupFramesAndTransformations() {
    // STEP 1: remove all compositeTransformation
    cleanupCompositeTransformations();

    // STEP 2: Now remove unused transforms
    // Helper lambda that will remove the provided Transformation from internal structure and return true if it is unused, or return false
    auto checkAndRemoveUnusedTransformation = [](const std::shared_ptr<Transformation>& tr) -> bool {
        // Unused = Only one owner (transformations), and one of their Frames has three owners max (frames, the current transformation and maybe its inverse)
        if (tr.use_count() == 1 && (tr.get()->from.use_count() <= 3 || tr.get()->to.use_count() <= 3)) {
            removeTransformationFromInternalStructures(tr.get());
            return true;
        }
        else {
            return false;
        }
    };

    // erase until there is nothing to remove
    // This empty loop is needed because
    // - on the first loop the inverse transformations will be removed,
    // - the second time the direct ones (which were owned by their inverse) will be removed,
    // - then transformations which were using frames which are now unused
    // - ...
    while (std::erase_if(transformations, checkAndRemoveUnusedTransformation) != 0) {
        ;
    }

    // STEP 3: Detect and remove unused frames:
    auto checkAndRemoveUnusedFrame = [](const std::shared_ptr<FrameOfReference>& fr) -> bool {
        if (fr.use_count() == 1) { // Unused
            frameMap.remove(fr->getUuid());
            return true;
        }
        else {
            return false;
        }
    };
    std::erase_if(frames, checkAndRemoveUnusedFrame);
}

// -------------------- removeTransformation --------------------
bool TransformationManager::removeTransformation(std::shared_ptr<Transformation>& tr) {
    if (tr == nullptr) {
        return false;
    }

    // Check the number of owners of this shared_ptr
    // The owners of tr include
    // - the composite transformations based on tr
    // - the inverse transformation (only if tr has no sources) (if tr has only one sources, tr is an inverse transformation)
    // - the TransformationManager (transformations vector)
    // - any other shared_ptr holder
    // If there is only one other shared_ptr holder, this is the caller of this method, therefore tr can be deleted.

    //-- 1. cleanup all composites to simplify counting
    cleanupCompositeTransformations();

    //-- 2. check if tr is an inverse
    bool deletable = false;
    if (isInverseTransformation(tr.get())) {
        deletable = (tr.use_count() == 2); // two owners remain: the TransformationManager and the caller
    }
    else {
        deletable = (tr.use_count() == 3); // three owners remain: the TransformationManager, the caller and the inverse
    }
    if (deletable) {
        // We need to call getExistingTransformation before removing it from internal structures
        std::shared_ptr<Transformation> inversePtr = getTransformationSharedPtr(getInverseTransformation(tr.get()));
        removeTransformationFromInternalStructures(inversePtr.get());
        removeTransformationFromInternalStructures(tr.get());

        // now delete from the ownership (transformations)
        std::erase(transformations, inversePtr); // remove inverse shared_ptr
        std::erase(transformations, tr);

        // explicit decrease of counters
        inversePtr = nullptr;
        tr = nullptr; // set parameter use_count() to 0
    }
    return deletable;
}

bool TransformationManager::removeTransformation(const FrameOfReference* from, const FrameOfReference* to) {
    std::shared_ptr<Transformation> tr = getTransformationOwnership(from, to);
    return removeTransformation(tr);
}

// -------------------- updateTransformation --------------------
bool TransformationManager::updateTransformation(Transformation* tr, vtkMatrix4x4* rawMatrix) {
    if (tr == nullptr || rawMatrix == nullptr) {
        return false;
    }

    vtkSmartPointer<vtkMatrix4x4> matrix = vtkSmartPointer<vtkMatrix4x4>::New();
    matrix->DeepCopy(rawMatrix);
    if (isDefaultIdentityToWorld(tr) || !isCompositeTransformation(tr)) {
        // manage direct/inverse cases
        Transformation* trToModify;
        Transformation* inverseOfTrToModify;
        vtkSmartPointer<vtkMatrix4x4> newMatrix;
        // as tr is a inverse or a direct transformation
        if (isInverseTransformation(tr)) {
            // tr is the inverse of a default transformation
            inverseOfTrToModify = tr;
            trToModify = transformationFromTo[tr->getTo()][tr->getFrom()];
            // the new matrix of tr is in fact the inverse of the given matrix
            newMatrix = vtkSmartPointer<vtkMatrix4x4>::New();
            newMatrix->DeepCopy(matrix);
            newMatrix->Invert();
        }
        else {
            // tr is a default transformation
            trToModify = tr;
            inverseOfTrToModify = transformationFromTo[tr->getTo()][tr->getFrom()];
            newMatrix = matrix;
        }

        // Ensure that tr and its inverse are not marked as default anymore
        defaultIdentityToWorldTransformations.erase(trToModify);
        defaultIdentityToWorldTransformations.erase(inverseOfTrToModify);

        // Now set both values
        trToModify->setMatrix(newMatrix);

        return true;
    }
    else {
        // Cannot update a Composite Transformation
        CAMITK_WARNING_ALT(QString("Cannot update Transformation %1 because it is a Composite Transformation.").arg(tr->getName()));
        return false;
    }
}

bool TransformationManager::updateTransformation(Transformation* tr, vtkSmartPointer<vtkTransform> vtkTr) {
    if (tr != nullptr && vtkTr != nullptr) {
        return updateTransformation(tr, vtkTr->GetMatrix());
    }
    else {
        return false;
    }
}

bool TransformationManager::updateTransformation(const FrameOfReference* from, const FrameOfReference* to, vtkMatrix4x4* matrix) {
    if (transformationFromTo.contains(from) && transformationFromTo[from].contains(to)) {
        return updateTransformation(transformationFromTo[from][to], matrix);
    }
    else {
        return false;
    }
}

bool TransformationManager::updateTransformation(const FrameOfReference* from, const FrameOfReference* to, vtkSmartPointer<vtkTransform> vtkTr) {
    if (transformationFromTo.contains(from) && transformationFromTo[from].contains(to)) {
        return updateTransformation(transformationFromTo[from][to], vtkTr);
    }
    else {
        return false;
    }
}

// -------------------- hasSources --------------------
bool TransformationManager::hasSources(const Transformation* tr) {
    if (tr == nullptr) {
        return false;
    }
    else {
        return (transformationSources[tr].size() != 0);
    }
}


// -------------------- isInverseTransformation --------------------
bool TransformationManager::isInverseTransformation(const Transformation* tr) {
    if (tr == nullptr) {
        return false;
    }
    else {
        return (transformationSources[tr].size() == 1);
    }
}

// -------------------- getInverseTransformation --------------------
Transformation* camitk::TransformationManager::getInverseTransformation(const Transformation* tr) {
    if (tr == nullptr) {
        return nullptr;
    }
    else {
        return getTransformation(tr->getTo(), tr->getFrom());
    }
}

// -------------------- isCompositeTransformation --------------------
bool TransformationManager::isCompositeTransformation(const Transformation* tr) {
    if (tr == nullptr) {
        return false;
    }
    else {
        return (transformationSources[tr].size() > 1);
    }
}

// -------------------- toVariant --------------------
QVariant TransformationManager::toVariant() {
    QVariantMap output;
    // Add all frames of reference
    QVariantList framesVariant;
    for (const auto& frame : frames) {
        // Frames have a valid Uuid because it was set by TransformationManager
        framesVariant.append(frame->toVariant());
    }
    output["frames"] = framesVariant;

    // Add all geometrical transforms
    QVariantList transformsVariant;
    for (const auto& tr : getDirectTransformations()) {
        // Transformations have a valid Uuid because it was set by TransformationManager
        QVariantMap trVariant = tr->toVariant().toMap();
        if (TransformationManager::isDefaultIdentityToWorld(tr)) {
            trVariant.insert("defaultIdentityToWorld", QVariant(true));
        }
        transformsVariant.append(trVariant);
    }
    output["transformations"] = transformsVariant;
    return output;
}

// -------------------- fromVariant --------------------
void TransformationManager::fromVariant(const QVariant& input) {
    QVariantMap inputMap = input.toMap();
    // Load frames of reference
    if (inputMap.contains("frames")) {
        for (const auto& frame : inputMap["frames"].toList()) {
            TransformationManager::addFrameOfReference(frame);
        }
    }

    // Load transformations
    if (inputMap.contains("transformations")) {
        for (const auto& transfo : inputMap["transformations"].toList()) {
            TransformationManager::addTransformation(transfo);
        }
    }
}

// -------------------- getSources --------------------
QVector<Transformation*> TransformationManager::getSources(const Transformation* tr) {
    if (tr == nullptr) {
        return {};
    }
    else {
        QVector<Transformation*> sources;
        // use raw pointer instead of shared_ptr
        std::ranges::transform(transformationSources[tr], std::back_inserter(sources), [](const auto & s) {
            return s.get();
        });
        return sources;
    }
}

} // namespace camitk