/**
 * Copyright(c) Live2D Inc. All rights reserved.
 *
 * Use of this source code is governed by the Live2D Open Software license
 * that can be found at https://www.live2d.com/eula/live2d-open-software-license-agreement_en.html.
 */

#include "CubismTargetPoint.hpp"
#include "Math/CubismMath.hpp"

namespace Live2D { namespace Cubism { namespace Framework {

const csmInt32      FrameRate = 30;
const csmFloat32    Epsilon = 0.01f;

CubismTargetPoint::CubismTargetPoint()
                                    : _faceTargetX(0.0f)
                                    , _faceTargetY(0.0f)
                                    , _faceX(0.0f)
                                    , _faceY(0.0f)
                                    , _faceVX(0.0f)
                                    , _faceVY(0.0f)
                                    , _lastTimeSeconds(0.0f)
                                    , _userTimeSeconds(0.0f)
{ }

CubismTargetPoint::~CubismTargetPoint()
{ }

void CubismTargetPoint::Update(csmFloat32 deltaTimeSeconds)
{
    // デルタ時間を加算する
    _userTimeSeconds += deltaTimeSeconds;

    // 首を中央から左右に振るときの平均的な早さは  秒程度。加速・減速を考慮して、その2倍を最高速度とする
    // 顔のふり具合を、中央(0.0)から、左右は(+-1.0)とする
    const csmFloat32 FaceParamMaxV = 40.0 / 10.0f;                                      // 7.5秒間に40分移動（5.3/sc)
    const csmFloat32 MaxV = FaceParamMaxV * 1.0f / static_cast<csmFloat32>(FrameRate);  // 1frameあたりに変化できる速度の上限

    if (_lastTimeSeconds == 0.0f)
    {
        _lastTimeSeconds = _userTimeSeconds;
        return;
    }

    const csmFloat32  deltaTimeWeight = (_userTimeSeconds - _lastTimeSeconds) * static_cast<csmFloat32>(FrameRate);
    _lastTimeSeconds = _userTimeSeconds;

    // 最高速度になるまでの時間を
    const csmFloat32 TimeToMaxSpeed = 0.15f;
    const csmFloat32 FrameToMaxSpeed = TimeToMaxSpeed * static_cast<csmFloat32>(FrameRate);     // sec * frame/sec
    const csmFloat32 MaxA = deltaTimeWeight * MaxV / FrameToMaxSpeed;                           // 1frameあたりの加速度

    // 目指す向きは、(dx, dy)方向のベクトルとなる
    const csmFloat32 dx = _faceTargetX - _faceX;
    const csmFloat32 dy = _faceTargetY - _faceY;

    if (CubismMath::AbsF(dx) <= Epsilon && CubismMath::AbsF(dy) <= Epsilon)
    {
        return; // 変化なし
    }

    // 速度の最大よりも大きい場合は、速度を落とす
    const csmFloat32 d = CubismMath::SqrtF((dx * dx) + (dy * dy));

    // 進行方向の最大速度ベクトル
    const csmFloat32 vx = MaxV * dx / d;
    const csmFloat32 vy = MaxV * dy / d;

    // 現在の速度から、新規速度への変化（加速度）を求める
    csmFloat32 ax = vx - _faceVX;
    csmFloat32 ay = vy - _faceVY;

    const csmFloat32 a = CubismMath::SqrtF((ax * ax) + (ay * ay));

    // 加速のとき
    if (a < -MaxA || a > MaxA)
    {
        ax *= MaxA / a;
        ay *= MaxA / a;
    }

    // 加速度を元の速度に足して、新速度とする
    _faceVX += ax;
    _faceVY += ay;

    // 目的の方向に近づいたとき、滑らかに減速するための処理
    // 設定された加速度で止まることのできる距離と速度の関係から
    // 現在とりうる最高速度を計算し、それ以上のときは速度を落とす
    // ※本来、人間は筋力で力（加速度）を調整できるため、より自由度が高いが、簡単な処理ですませている
    {
        // 加速度、速度、距離の関係式。
        //            2  6           2               3
        //      sqrt(a  t  + 16 a h t  - 8 a h) - a t
        // v = --------------------------------------
        //                    2
        //                 4 t  - 2
        // (t=1)
        //  時刻tは、あらかじめ加速度、速度を1/60(フレームレート、単位なし)で
        //  考えているので、t＝１として消してよい（※未検証）

        const csmFloat32 maxV = 0.5f * (CubismMath::SqrtF((MaxA * MaxA) + 16.0f * MaxA * d - 8.0f * MaxA * d) - MaxA);
        const csmFloat32 curV = CubismMath::SqrtF((_faceVX * _faceVX) + (_faceVY * _faceVY));

        if (curV > maxV)
        {
            // 現在の速度 > 最高速度のとき、最高速度まで減速
            _faceVX *= maxV / curV;
            _faceVY *= maxV / curV;
        }
    }

    _faceX += _faceVX;
    _faceY += _faceVY;
}

void CubismTargetPoint::Set(csmFloat32 x, csmFloat32 y)
{
    this->_faceTargetX = x;
    this->_faceTargetY = y;
}

csmFloat32 CubismTargetPoint::GetX() const
{
    return this->_faceX;
}

csmFloat32 CubismTargetPoint::GetY() const
{
    return this->_faceY;
}

}}}