#include "rive/constraints/translation_constraint.hpp"
#include "rive/transform_component.hpp"
#include "rive/math/mat2d.hpp"
#include "rive/math/vec2d.hpp"
#include <cmath>

using namespace rive;

void TranslationConstraint::constrain(TransformComponent* component) {
    Mat2D& transformA = component->mutableWorldTransform();
    Vec2D translationA(transformA[4], transformA[5]);
    Vec2D translationB;
    if (m_Target == nullptr) {
        translationB = translationA;
    } else {
        Mat2D transformB(m_Target->worldTransform());
        if (sourceSpace() == TransformSpace::local) {
            const Mat2D& targetParentWorld = getParentWorld(*m_Target);

            Mat2D inverse;
            if (!Mat2D::invert(inverse, targetParentWorld)) {
                return;
            }
            transformB = inverse * transformB;
        }
        translationB[0] = transformB[4];
        translationB[1] = transformB[5];

        if (!doesCopy()) {
            translationB[0] =
                destSpace() == TransformSpace::local ? 0.0f : translationA[0];
        } else {
            translationB[0] *= copyFactor();
            if (offset()) {
                translationB[0] += component->x();
            }
        }

        if (!doesCopyY()) {
            translationB[1] =
                destSpace() == TransformSpace::local ? 0.0f : translationA[1];
        } else {
            translationB[1] *= copyFactorY();

            if (offset()) {
                translationB[1] += component->y();
            }
        }

        if (destSpace() == TransformSpace::local) {
            // Destination space is in parent transform coordinates.
            translationB = getParentWorld(*component) * translationB;
        }
    }

    bool clampLocal = minMaxSpace() == TransformSpace::local;
    if (clampLocal) {
        // Apply min max in local space, so transform to local coordinates
        // first.
        Mat2D invert;
        if (!Mat2D::invert(invert, getParentWorld(*component))) {
            return;
        }
        // Get our target world coordinates in parent local.
        translationB = invert * translationB;
    }
    if (max() && translationB[0] > maxValue()) {
        translationB[0] = maxValue();
    }
    if (min() && translationB[0] < minValue()) {
        translationB[0] = minValue();
    }
    if (maxY() && translationB[1] > maxValueY()) {
        translationB[1] = maxValueY();
    }
    if (minY() && translationB[1] < minValueY()) {
        translationB[1] = minValueY();
    }
    if (clampLocal) {
        // Transform back to world.
        translationB = getParentWorld(*component) * translationB;
    }

    float t = strength();
    float ti = 1.0f - t;

    // Just interpolate world translation
    transformA[4] = translationA[0] * ti + translationB[0] * t;
    transformA[5] = translationA[1] * ti + translationB[1] * t;
}