//
// Created by liangshuochen on 12/06/2017.
//

#include "Constraint.h"
#include "Armature.h"
#include "Bone.h"

DRAGONBONES_NAMESPACE_BEGIN

Matrix Constraint::_helpMatrix;
Transform Constraint::_helpTransform;
Point Constraint::_helpPoint;

void Constraint::_onClear()
{
    _constraintData = nullptr;
    _armature = nullptr;
    _target = nullptr;
    _root = nullptr;
    _bone = nullptr;
}

void IKConstraint::_onClear()
{
    Constraint::_onClear();

    _scaleEnabled = false;
    _bendPositive = false;
    _weight = 1.0f;
}

void IKConstraint::_computeA()
{
    const auto& ikGlobal = _target->global;
    auto& global = _root->global;
    auto& globalTransformMatrix = _root->globalTransformMatrix;

    auto radian = std::atan2(ikGlobal.y - global.y, ikGlobal.x - global.x);
    if (global.scaleX < 0.0f)
    {
        radian += Transform::PI;
    }

    global.rotation += Transform::normalizeRadian(radian - global.rotation) * _weight;
    global.toMatrix(globalTransformMatrix);
}

void IKConstraint::_computeB()
{
    const auto boneLength = _bone->_boneData->length;
    const auto parent = _root;
    const auto& ikGlobal = _target->global;
    auto& parentGlobal = parent->global;
    auto& global = _bone->global;
    auto& globalTransformMatrix = _bone->globalTransformMatrix;

    const auto x = globalTransformMatrix.a * boneLength;
    const auto y = globalTransformMatrix.b * boneLength;
    const auto lLL = x * x + y * y;
    const auto lL = sqrt(lLL);
    auto dX = global.x - parentGlobal.x;
    auto dY = global.y - parentGlobal.y;
    const auto lPP = dX * dX + dY * dY;
    const auto lP = sqrt(lPP);
    const auto rawRadian = global.rotation;
    const auto rawParentRadian = parentGlobal.rotation;
    const auto rawRadianA = std::atan2(dY, dX);

    dX = ikGlobal.x - parentGlobal.x;
    dY = ikGlobal.y - parentGlobal.y;
    const auto lTT = dX * dX + dY * dY;
    const auto lT = sqrt(lTT);

    auto radianA = 0.0f;
    if (lL + lP <= lT || lT + lL <= lP || lT + lP <= lL) 
    {
        radianA = std::atan2(ikGlobal.y - parentGlobal.y, ikGlobal.x - parentGlobal.x);
        if (lL + lP <= lT) 
        {
        }
        else if (lP < lL) 
        {
            radianA += Transform::PI;
        }
    }
    else 
    {
        const auto h = (lPP - lLL + lTT) / (2.0f * lTT);
        const auto r = sqrt(lPP - h * h * lTT) / lT;
        const auto hX = parentGlobal.x + (dX * h);
        const auto hY = parentGlobal.y + (dY * h);
        const auto rX = -dY * r;
        const auto rY = dX * r;

        auto isPPR = false;
        const auto parentParent = parent->getParent();
        if (parentParent != nullptr)
        {
            auto parentParentMatrix = parentParent->globalTransformMatrix;
            isPPR = parentParentMatrix.a * parentParentMatrix.d - parentParentMatrix.b * parentParentMatrix.c < 0.0f;
        }

        if (isPPR != _bendPositive) 
        {
            global.x = hX - rX;
            global.y = hY - rY;
        }
        else 
        {
            global.x = hX + rX;
            global.y = hY + rY;
        }

        radianA = std::atan2(global.y - parentGlobal.y, global.x - parentGlobal.x);
    }

    const auto dR = Transform::normalizeRadian(radianA - rawRadianA);
    parentGlobal.rotation = rawParentRadian + dR * _weight;
    parentGlobal.toMatrix(parent->globalTransformMatrix);
    //
    const auto currentRadianA = rawRadianA + dR * _weight;
    global.x = parentGlobal.x + cos(currentRadianA) * lP;
    global.y = parentGlobal.y + sin(currentRadianA) * lP;
    //
    auto radianB = std::atan2(ikGlobal.y - global.y, ikGlobal.x - global.x);
    if (global.scaleX < 0.0f) {
        radianB += Transform::PI;
    }

    global.rotation = parentGlobal.rotation + rawRadian - rawParentRadian + Transform::normalizeRadian(radianB - dR - rawRadian) * _weight;
    global.toMatrix(globalTransformMatrix);
}

void IKConstraint::init(ConstraintData* constraintData, Armature* armature)
{
    if (_constraintData != nullptr)
    {
        return;
    }

    _constraintData = constraintData;
    _armature = armature;
    _target = _armature->getBone(_constraintData->target->name);
    _root = _armature->getBone(_constraintData->root->name);
    _bone = _constraintData->bone != nullptr ? _armature->getBone(_constraintData->bone->name) : nullptr;

    {
        const auto ikConstraintData = static_cast<IKConstraintData*>(_constraintData);
        _bendPositive = ikConstraintData->bendPositive;
        _scaleEnabled = ikConstraintData->scaleEnabled;
        _weight = ikConstraintData->weight;
    }

    _root->_hasConstraint = true;
}

void IKConstraint::update()
{
    _root->updateByConstraint();

    if (_bone != nullptr)
    {
        _bone->updateByConstraint();
        _computeB();
    }
    else
    {
        _computeA();
    }
}

void IKConstraint::invalidUpdate()
{
    _root->invalidUpdate();

    if (_bone != nullptr)
    {
        _bone->invalidUpdate();
    }
}

DRAGONBONES_NAMESPACE_END