/*****************************************************************************
 * $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$
 ****************************************************************************/

// -- Core image component stuff
#include "ArbitrarySingleImageComponent.h"
#include "Component.h"
#include "ImageComponent.h"

// -- Core stuff

#include "Log.h"

// -- VTK stuff
// disable warning generated by clang about the surrounded header
#include "CamiTKDisableWarnings"
#include <vtkProperty.h>
#include "CamiTKReEnableWarnings"
#include "Application.h"
#include "Transformation.h"
#include "TransformationManager.h"
#include "Property.h"
#include "Log.h"

#include <vtkSmartPointer.h>
#include <vtkUnstructuredGrid.h>
#include <vtkImageClip.h>
#include <vtkImageChangeInformation.h>
#include <vtkMatrix4x4.h>
#include <vtkTransformFilter.h>
#include <vtkDataSetMapper.h>

// Maths
#include <cmath>
#include <QVector3D>

namespace camitk {

// Useful debug macros for displaying homogeneous matrix and points
#define displayPoint(...)     CAMITK_INFO_ALT(#__VA_ARGS__ + QString(" = [%1,%2,%3,%4]")     \
                                .arg(__VA_ARGS__[0], 8, 'f', 4, ' ')                         \
                                .arg(__VA_ARGS__[1], 8, 'f', 4, ' ')                         \
                                .arg(__VA_ARGS__[2], 8, 'f', 4, ' ')                         \
                                .arg(__VA_ARGS__[3], 8, 'f', 4, ' '))

#define displayQVector3D(...)  CAMITK_INFO_ALT(#__VA_ARGS__ + QString(" = (%1,%2,%3)")       \
                                .arg(__VA_ARGS__.x(), 8, 'f', 4, ' ')                        \
                                .arg(__VA_ARGS__.y(), 8, 'f', 4, ' ')                        \
                                .arg(__VA_ARGS__.z(), 8, 'f', 4, ' '))

#define displayMatrix4x4(...) CAMITK_INFO_ALT(#__VA_ARGS__ + QString("\n[%1,%2,%3,%4]\n[%5,%6,%7,%8]\n[%9,%10,%11,%12]\n[%13,%14,%15,%16]") \
                                .arg(__VA_ARGS__->GetElement(0, 0), 8, 'f', 4, ' ')                   \
                                .arg(__VA_ARGS__->GetElement(0, 1), 8, 'f', 4, ' ')                   \
                                .arg(__VA_ARGS__->GetElement(0, 2), 8, 'f', 4, ' ')                   \
                                .arg(__VA_ARGS__->GetElement(0, 3), 8, 'f', 4, ' ')                   \
                                .arg(__VA_ARGS__->GetElement(1, 0), 8, 'f', 4, ' ')                   \
                                .arg(__VA_ARGS__->GetElement(1, 1), 8, 'f', 4, ' ')                   \
                                .arg(__VA_ARGS__->GetElement(1, 2), 8, 'f', 4, ' ')                   \
                                .arg(__VA_ARGS__->GetElement(1, 3), 8, 'f', 4, ' ')                   \
                                .arg(__VA_ARGS__->GetElement(2, 0), 8, 'f', 4, ' ')                   \
                                .arg(__VA_ARGS__->GetElement(2, 1), 8, 'f', 4, ' ')                   \
                                .arg(__VA_ARGS__->GetElement(2, 2), 8, 'f', 4, ' ')                   \
                                .arg(__VA_ARGS__->GetElement(2, 3), 8, 'f', 4, ' ')                   \
                                .arg(__VA_ARGS__->GetElement(3, 0), 8, 'f', 4, ' ')                   \
                                .arg(__VA_ARGS__->GetElement(3, 1), 8, 'f', 4, ' ')                   \
                                .arg(__VA_ARGS__->GetElement(3, 2), 8, 'f', 4, ' ')                   \
                                .arg(__VA_ARGS__->GetElement(3, 3), 8, 'f', 4, ' '))


// -------------------- constructor  --------------------
ArbitrarySingleImageComponent::ArbitrarySingleImageComponent(Component* parentComponent, const QString& name, vtkSmartPointer<vtkWindowLevelLookupTable> lut)
    : SingleImageComponent(parentComponent, Slice::ARBITRARY, name, lut) {

    // initial arbitrary slice is centered in the volume along the original z axis
    addProperty(new Property("Translation", 0.5, tr("Current translation inside the image"), ""));
    addProperty(new Property("Rotation", QVector3D(), tr("Rotation"), "degrees"));

    // store the value for later reuse
    ImageComponent* parentImage = dynamic_cast<ImageComponent*>(parentComponent);
    dimensions = parentImage->getImageData()->GetDimensions();
    spacing = parentImage->getImageData()->GetSpacing();

    // Create frame and transformation to move from the resliced image (managed by mySlice) to the original image data frame
    initArbitraryTransformation(TransformationManager::addFrameOfReference(parentComponent->getName() + " (arbitrary)", "Arbitrary frame of image '" + parentImage->getName() + "'"),
                                TransformationManager::getFrameOfReferenceOwnership(parentImage->getDataFrame()));
    resetArbitraryTransformationMatrix();

    // apply default transformation
    ArbitrarySingleImageComponent::updateTranslation();
    ArbitrarySingleImageComponent::updateRotation();

    // set default size for the frame axis actor
    // getFrameAxisActor()->SetTotalLength(spacing[2] * 10.0, spacing[2] * 10.0, spacing[2] * 10.0);
}

// -------------------- destructor --------------------
ArbitrarySingleImageComponent::~ArbitrarySingleImageComponent() {
}


// -------------------- setFrame --------------------
void ArbitrarySingleImageComponent::setFrame(const std::shared_ptr<FrameOfReference>& newFrame) {
    // -- 1. Save the current transformation matrix from the previous arbitrary to the previous (main) frame
    vtkSmartPointer<vtkMatrix4x4> arbitraryTransformationMatrix ;
    if (arbitraryTransformation != nullptr) {
        arbitraryTransformationMatrix = arbitraryTransformation->getMatrix();
    }
    else {
        arbitraryTransformationMatrix = vtkSmartPointer<vtkMatrix4x4>::New();
    }

    // -- 2. Create an new arbitrary frame (using the same name and description)
    // (that will produce a clean break with the old system of frames / transformation)
    std::shared_ptr<FrameOfReference> newArbitraryFrame = TransformationManager::addFrameOfReference(arbitraryFrame->getName(), arbitraryFrame->getDescription());

    // -- 3. Update arbitrary and main frame and force the creation of a new arbitrary transformation
    //       between them
    initArbitraryTransformation(newArbitraryFrame, newFrame, nullptr);

    // Update the new transformation using the previous matrix
    TransformationManager::updateTransformation(arbitraryTransformation.get(), arbitraryTransformationMatrix.Get());
}

// -------------------- getAllFrames --------------------
QMultiMap<const FrameOfReference*, Component*> ArbitrarySingleImageComponent::getAllFrames(bool includeChildrenFrames) {
    QMultiMap<const FrameOfReference*, Component*> allFrames = Component::getAllFrames(includeChildrenFrames);
    allFrames.insert(this->getArbitraryFrame(), this);
    return allFrames;
}

// -------------------- getAllTransformations --------------------
QMultiMap<const Transformation*, Component*> ArbitrarySingleImageComponent::getAllTransformations(bool includeChildrenTransformations) {
    QMultiMap<const Transformation*, Component*>  allTransformations = Component::getAllTransformations();
    allTransformations.insert(getArbitraryTransformation(), this);
    return allTransformations;
}

// -------------------- initArbitraryTransformation --------------------
void ArbitrarySingleImageComponent::initArbitraryTransformation(const std::shared_ptr<FrameOfReference>& arbitraryFrame, const std::shared_ptr<FrameOfReference>& dataFrame, const std::shared_ptr<Transformation>& tr) {
    // Remove previous transformation (if any) from arbitrary to main
    TransformationManager::removeTransformation(arbitraryTransformation);
    // after removeTransformation arbitraryTransformation must be nullptr

    // set the frames
    setArbitraryFrame(arbitraryFrame);
    SingleImageComponent::setFrame(dataFrame);

    // create a default transformation from the arbitrary frame to the main frame if needed
    if (tr == nullptr) {
        arbitraryTransformation = TransformationManager::addTransformation(getArbitraryFrame(), getFrame());
    }
    else {
        arbitraryTransformation = tr;
    }

    // giving arbitraryTransformation to mySlice will allow the reslicer to compute the proper pixel values
    // depending on the current arbitrary orientation
    mySlice->setArbitraryTransform(arbitraryTransformation->getTransform());
}

// -------------------- resetArbitraryTransformationMatrix --------------------
void ArbitrarySingleImageComponent::resetArbitraryTransformationMatrix() {
    // position x and y at the center of the slice
    vtkSmartPointer<vtkMatrix4x4> T_a2m = vtkSmartPointer<vtkMatrix4x4>::New();
    T_a2m->Identity();
    T_a2m->SetElement(0, 3, dimensions[0] * spacing[0] / 2.0);
    T_a2m->SetElement(1, 3, dimensions[1] * spacing[1] / 2.0);
    TransformationManager::updateTransformation(arbitraryTransformation.get(), T_a2m.Get());
}

// -------------------- propertyValueChanged --------------------
void ArbitrarySingleImageComponent::propertyValueChanged(QString name) {
    if (name == "Translation") {
        // Changing Translation
        updateTranslation();
    }
    else if (name == "Rotation") {
        // Changing Rotation
        updateRotation();
    }
    else {
        SingleImageComponent::propertyValueChanged(name);
    }
}

// -------------------- updatePropertyFromTransformation --------------------
void ArbitrarySingleImageComponent::updatePropertyFromTransformation() {
    blockSignals(true);
    setPropertyValue("Translation", computeTranslationRatio());
    // update euler angles
    double orientation[3];
    getArbitraryTransformation()->getTransform()->GetOrientation(orientation);
    setPropertyValue("Rotation", QVector3D(orientation[0], orientation[1], orientation[2]));
    blockSignals(false);
}

// -------------------- resetTransform --------------------
void ArbitrarySingleImageComponent::resetTransform() {
    // position x and y at the center of the slice
    resetArbitraryTransformationMatrix();

    setPropertyValue("Rotation", QVector3D());
    // position z at 50% of the volume
    setPropertyValue("Translation", 0.5);
}

// -------------------- updateTranslation --------------------
void ArbitrarySingleImageComponent::updateTranslation() {
    // Set the translation value from the corresponding property value
    double translationRatio = getPropertyValue("Translation").toDouble();

    // Check interval validity
    if (translationRatio < 0.0) {
        translationRatio = 0.0;
    }
    else {
        if (translationRatio > 1.0) {
            translationRatio = 1.0;
        }
    }

    // Compute the intersection of the z vector with the image borders
    // i.e. the intersections given by C_m ± CZ_m with the image borders
    // intersection of C_m → -CZ_m with the image border
    QVector3D Cminus_m;
    // intersection of C_m → +CZ_m with the image border
    QVector3D Cplus_m;
    computeIntersectionsWithImageBorders(Cminus_m, Cplus_m);

    QVector3D CminusCplus_m = Cplus_m - Cminus_m;
    CminusCplus_m = roundTo4Decimals(CminusCplus_m);

    /// modify the translation part of the current transformation from arbitrary to main
    vtkSmartPointer<vtkMatrix4x4> T_a2m = vtkSmartPointer<vtkMatrix4x4>::New();
    T_a2m->DeepCopy(getArbitraryTransformation()->getMatrix());
    for (int i = 0; i < 3; i++) {
        T_a2m->SetElement(i, 3, Cminus_m[i] + translationRatio * CminusCplus_m[i]);
    }

    cleanMatrix(T_a2m);
    if (checkCenter(T_a2m)) {
        TransformationManager::updateTransformation(arbitraryTransformation.get(), T_a2m.Get());
        // update picking representation (update pickplane position + hide pixel actor)
        updatePickPlane();
        getPixelActor()->VisibilityOff();
    }
}

// -------------------- updateRotation --------------------
void ArbitrarySingleImageComponent::updateRotation() {
    QVector3D rotation = getPropertyValue("Rotation").value<QVector3D>();
    double angleX = rotation.x();
    double angleY = rotation.y();
    double angleZ = rotation.z();

    // isolate translation
    QVector3D translation_a2m;
    for (int i = 0; i < 3; i++) {
        translation_a2m[i] = getArbitraryTransformation()->getMatrix()->GetElement(i, 3);
    }

    // create rotation
    vtkSmartPointer<vtkTransform> transform_R_a = vtkSmartPointer<vtkTransform>::New();
    transform_R_a->Identity();
    transform_R_a->RotateZ(angleZ);
    transform_R_a->RotateY(angleY);
    transform_R_a->RotateX(angleX);
    transform_R_a->Update();

    // concatenate rotation and translation to compute the new transformation matrix
    // from arbitrary frame to main frame
    vtkSmartPointer<vtkMatrix4x4> T_a2m = transform_R_a->GetMatrix();
    for (int i = 0; i < 3; i++) {
        T_a2m->SetElement(i, 3, translation_a2m[i]);
    }
    cleanMatrix(T_a2m);

    if (checkCenter(T_a2m)) {
        TransformationManager::updateTransformation(arbitraryTransformation.get(), T_a2m.Get());
        // update picking representation (update pickplane position + hide pixel actor)
        updatePickPlane();
        getPixelActor()->VisibilityOff();

        // update the property (but do not propagate as this is the current value)
        blockSignals(true);
        setPropertyValue("Translation", computeTranslationRatio());
        blockSignals(false);
    }
}

// -------------------- computeTranslationRatio --------------------
double ArbitrarySingleImageComponent::computeTranslationRatio() const {
    // compute the intersection of C_m ± CZ_m with the image borders
    // intersection of C_m → -CZ_m with the image borders
    QVector3D Cminus_m;
    // intersection of C_m → +CZ_m with the image borders
    QVector3D Cplus_m;
    computeIntersectionsWithImageBorders(Cminus_m, Cplus_m);

    // C_m is the center of rotation
    double C_m[4];
    getArbitraryCenter(C_m);

    // vector from Cminus to C
    QVector3D CminusC_m = QVector3D(C_m[0], C_m[1], C_m[2]) - Cminus_m;
    QVector3D CminusCplus_m = Cplus_m - Cminus_m;
    return CminusC_m.length() / CminusCplus_m.length();
}

// -------------------- computeIntersectionsWithImageBorders --------------------
void ArbitrarySingleImageComponent::computeIntersectionsWithImageBorders(QVector3D& Cz_min, QVector3D& Cz_max) const {
    // C_m is the center of rotation
    double C_m[4];
    getArbitraryCenter(C_m);

    // CZ_m is the vector perpendicular to the current arbitrary plane expressed in the main frame
    double CZ_m[4];
    getArbitraryPlaneNormal(CZ_m);

    // compute the intersection of C_m ± CZ_m with the image border
    computeIntersectionsWithImageBorders(QVector3D(C_m[0], C_m[1], C_m[2]), QVector3D(CZ_m[0], CZ_m[1], CZ_m[2]), Cz_min, Cz_max);
}

void ArbitrarySingleImageComponent::computeIntersectionsWithImageBorders(const QVector3D& origin, const QVector3D& upVector, QVector3D& intersectionMin, QVector3D& intersectionMax) const {
    QVector3D downVector = -upVector;

    // absolute values of centers of faces
    double xCenter = dimensions[0] * spacing[0] / 2.0;
    double yCenter = dimensions[1] * spacing[1] / 2.0;
    double zCenter = dimensions[2] * spacing[2] / 2.0;

    // check intersection to front plane
    double zMin = 0.0;
    QVector3D frontCenter(xCenter, yCenter, zMin);
    bool intersect = linePlaneIntersectionPoint(downVector, origin, QVector3D(0.0, 0.0, 1.0), frontCenter, intersectionMin);
    if (intersect && pointInsideVolume(intersectionMin)) {
        // back plane
        // substract a little more than half of the voxel size to make sure the cutting plane does not go out of the image bound
        // TODO remove this substraction when the 1/2 voxel of displacement bug in the viewer is fixed
        double zMax = dimensions[2] * spacing[2] - spacing[2] / 1.9;
        linePlaneIntersectionPoint(upVector, origin, QVector3D(0.0, 0.0, -1.0), QVector3D(xCenter, yCenter, zMax), intersectionMax);
    }
    else {
        // check intersection to top plane
        double yMin = 0.0;
        QVector3D topCenter(xCenter, yMin, zCenter);
        intersect = linePlaneIntersectionPoint(downVector, origin, QVector3D(0.0, 1.0, 0.0), topCenter, intersectionMin);
        if (intersect && pointInsideVolume(intersectionMin)) {
            // bottom plane
            double yMax = dimensions[1] * spacing[1] - spacing[1] / 1.9; // TODO remove this substraction when the 1/2 voxel of displacement bug in the viewer is fixed
            linePlaneIntersectionPoint(upVector, origin, QVector3D(0.0, -1.0, 0.0), QVector3D(xCenter, yMax, zCenter), intersectionMax);
        }
        else {
            // intersection is with left/right plane
            double xMin = 0.0;
            double xMax = dimensions[0] * spacing[0] - spacing[0] / 1.9; // TODO remove this substraction when the 1/2 voxel of displacement bug in the viewer is fixed
            linePlaneIntersectionPoint(downVector, origin, QVector3D(1.0, 0.0, 0.0), QVector3D(xMin, yCenter, zCenter), intersectionMin);
            linePlaneIntersectionPoint(upVector, origin, QVector3D(-1.0, 0.0, 0.0), QVector3D(xMax, yCenter, zCenter), intersectionMax);
        }
    }

    // rounding to avoid drifts
    intersectionMin = roundTo4Decimals(intersectionMin);
    intersectionMax = roundTo4Decimals(intersectionMax);
}

// -------------------- checkCenter --------------------
bool ArbitrarySingleImageComponent::checkCenter(vtkSmartPointer<vtkMatrix4x4> transform) const {
    // positive check only if the given transform will keep the center inside the image bounding box
    // if the center is going to be moved outside of the image bounding box, the given transform
    // should not be used
    // center is given by the translation
    return pointInsideVolume(QVector3D(transform->GetElement(0, 3), transform->GetElement(1, 3), transform->GetElement(2, 3)));
}

// -------------------- pointInsideVolume --------------------
bool ArbitrarySingleImageComponent::pointInsideVolume(QVector3D p) const {
    QVector3D pRounded = roundTo4Decimals(p);
    return (pRounded.x() >= 0.0
            && pRounded.x() <= dimensions[0] * spacing[0]
            && pRounded.y() >= 0.0
            && pRounded.y() <= dimensions[1] * spacing[1]
            && pRounded.z() >= 0.0
            && pRounded.z() <= dimensions[2] * spacing[2]);
}

// -------------------- getImageCenterInParent --------------------
void ArbitrarySingleImageComponent::getArbitraryCenter(double center[4]) const {
    // center == C_m, the center of the arbitrary frame expressed in the main frame
    for (int i = 0; i < 3; i++) {
        center[i] = getArbitraryTransformation()->getMatrix()->GetElement(i, 3);
    }
    center[3] = 1.0;
}

// -------------------- getArbitraryPlaneNormal --------------------
void ArbitrarySingleImageComponent::getArbitraryPlaneNormal(double normalVector[4]) const {
    // z is normal to the cutting plane
    // normalVector_a is the cutting plane normal vector expressed in the arbitrary frame
    double normalVector_a[4] = { 0.0, 0.0, 1.0, 0.0 };

    // note: for homogeneous coordinates, both double[4] can be used for representing 3D points and 3D vectors.
    // But their is a difference in the last component that distinguishes a 3D vector from a 3D point
    // - if the last component is 1.0, the double[4] represents a 3D point in homogeneous coordinates
    // - if the last component is 0.0, the double[4] represents a 3D vector in homogeneous vector coordinates
    // This guarantees coherent result when multiplying with homogeneous matrix

    // zDirection is the vector perpendicular to the current arbitrary plane expressed in the main frame
    getArbitraryTransformation()->getMatrix()->MultiplyPoint(normalVector_a, normalVector);
    normalVector[3] = 0.0;
}

// -------------------- get3DCursor --------------------
vtkSmartPointer<vtkActor> ArbitrarySingleImageComponent::get3DCursor() {
    // Because we cannot add the same actor to multiple viewers
    // We duplicate the 3D cursor Actor of the parent ImageComponent or we return our copy if it was already built
    if (getParentComponent() != nullptr && cursorActor == nullptr) {
        vtkSmartPointer<vtkActor> parentCursor = getParentComponent()->get3DCursor();
        if (parentCursor != nullptr) {
            cursorActor = vtkSmartPointer<vtkActor>::New();
            vtkSmartPointer<vtkDataSetMapper> cursorMapper = vtkSmartPointer<vtkDataSetMapper>::New();
            auto transformFilter = vtkSmartPointer<vtkTransformFilter>::New();
            transformFilter->SetInputData(parentCursor->GetMapper()->GetInput());
            // use the inverse transform (ask the transformation manager to always have an updated version)
            transformFilter->SetTransform(TransformationManager::getTransformation(frameOfReference.get(), arbitraryFrame.get())->getTransform());
            cursorMapper->SetInputConnection(transformFilter->GetOutputPort());
            cursorActor->SetMapper(cursorMapper);
            cursorActor->SetProperty(parentCursor->GetProperty());
            // The cursor cannot be picked
            cursorActor->PickableOff();
            cursorActor->VisibilityOn();
        }
    }
    return cursorActor;
}

// -------------------- setSlice --------------------
void ArbitrarySingleImageComponent::setSlice(int s) {
    // updateTranslation(0.0, 0.0, double(s) / 100.0);
    // internal translation is a percentage, while s is an int
    // but if the current slice (int) computed from the current translation percentage is equals to s,
    // then nothing should be modified
    if (getSlice() != s) {
        // make sure s is inside [0..100]
        if (s < 0) {
            s = 0;
        }
        else {
            if (s > 100) {
                s = 100;
            }
        }
        setPropertyValue("Translation", double(s) / 100.0);
    }
}

void ArbitrarySingleImageComponent::setSlice(double x, double y, double z) {
    /// x,y,z are expressed in the main frame (parent ImageComponent's data frame)
    /// → this is the new absolute translation
    /// modify the translation part of the current transformation from arbitrary to main
    /// x and y are not managed by set slice. Do not modify these values
    vtkSmartPointer<vtkMatrix4x4> T_a2m = vtkSmartPointer<vtkMatrix4x4>::New();
    T_a2m->DeepCopy(getArbitraryTransformation()->getMatrix());
    T_a2m->SetElement(0, 3, x);
    T_a2m->SetElement(1, 3, y);
    T_a2m->SetElement(2, 3, z);

    cleanMatrix(T_a2m);
    if (checkCenter(T_a2m)) {
        TransformationManager::updateTransformation(arbitraryTransformation.get(), T_a2m.Get());

        // translate to the plane that is parallel to z direction
        // Update the pick point actor
        // Set pixel position in current slice
        setPixelRealPosition(x, y, z);

        // update picking representation (update pickplane position + hide pixel actor)
        updatePickPlane();
        getPixelActor()->VisibilityOff();

        // update the property (but do not propagate as this is the current value)
        blockSignals(true);
        setPropertyValue("Translation", computeTranslationRatio());
        blockSignals(false);
    }
}

// -------------------- getSlice --------------------
int ArbitrarySingleImageComponent::getSlice() const {
    return computeTranslationRatio() * 100.0;
}

// -------------------- getNumberOfSlices --------------------
int ArbitrarySingleImageComponent::getNumberOfSlices() const {
    return 100;
}


// -----------------------
//  maths utility methods
// -----------------------

// -------------------- Multiply4x4 --------------------
template<typename T>
vtkSmartPointer<vtkMatrix4x4> ArbitrarySingleImageComponent::Multiply4x4(T a, T b) {
    vtkSmartPointer<vtkMatrix4x4> c = vtkSmartPointer<vtkMatrix4x4>::New();
    vtkMatrix4x4::Multiply4x4(a, b, c);
    return c;
}

template<typename T, typename... Args>
vtkSmartPointer<vtkMatrix4x4> ArbitrarySingleImageComponent::Multiply4x4(T a, T b, Args... args) {
    return Multiply4x4(a, Multiply4x4(b, args...));
}

// -------------------- linePlaneIntersectionPoint --------------------
bool ArbitrarySingleImageComponent::linePlaneIntersectionPoint(QVector3D lineVector, QVector3D linePoint, QVector3D planeNormal, QVector3D planePoint, QVector3D& intersection) {
    lineVector.normalize();
    planeNormal.normalize();
    // Let P(x,y,z) be the intersection point
    // As the plane equation is:
    //     (P - planePoint) . planeNormal = 0     (. denotes dot product)
    // and the line equation:
    //     P = linePoint + k * lineVector
    // The equation linking both above is:
    //     (linePoint + k * lineVector - planePoint) . planeNormal = 0
    // =>  k = - [ (linePoint - planePoint).planeNormal ] / (lineVector . planeNormal)
    // if (lineVector . planeNormal) == 0.0 line is parallel to plane, this method should return false
    float lDotN = QVector3D::dotProduct(lineVector, planeNormal);

    if (fabs(lDotN) < 1e-10) {
        // line and plane are parallel
        return false;
    }
    else {
        QVector3D u = linePoint - planePoint; // vector from plane point to the line point
        float uDotN = QVector3D::dotProduct(u, planeNormal);
        float k = - uDotN / lDotN;
        intersection = linePoint + k * lineVector;
        return true;
    }
}

// -------------------- roundTo4Decimals --------------------
float ArbitrarySingleImageComponent::roundTo4Decimals(float input) {
    float output = (int)(input * 10000 + .5);
    return (float) output / 10000;
}

QVector3D ArbitrarySingleImageComponent::roundTo4Decimals(QVector3D input) {
    return QVector3D(roundTo4Decimals(input.x()), roundTo4Decimals(input.y()), roundTo4Decimals(input.z()));
}

// -------------------- cleanMatrix --------------------
void ArbitrarySingleImageComponent::cleanMatrix(vtkSmartPointer<vtkMatrix4x4> matrixToClean, double epsilon) {
    for (int i = 0; i < 3; i++) {
        for (int j = 0; j < 3; j++) {
            if (fabs(matrixToClean->GetElement(i, j)) < epsilon) {
                matrixToClean->SetElement(i, j, 0.0);
            }
        }
    }
    matrixToClean->SetElement(3, 0, 0.0);
    matrixToClean->SetElement(3, 1, 0.0);
    matrixToClean->SetElement(3, 2, 0.0);
    matrixToClean->SetElement(3, 3, 1.0);
}

}