#include "TransformLerper.h"

//#include "RKObjectFactory.h"
#include "CasualCore.h"

TransformLerper::TransformLerper()
  : m_lerping(false)
  , m_posStart(RKVector::Origin)
  , m_posEnd(RKVector::Origin)
  , m_rotStart(RKQuaternion::Identity)
  , m_rotEnd(RKQuaternion::Identity)
  , m_lerpTime(0.0f)
  , m_maxLerpTime(1.0f)
  , m_pObject(NULL)
  , m_pCallback(NULL)
  , m_pCallbackOwner(NULL)
{
}

TransformLerper::~TransformLerper()
{
}

void TransformLerper::Init(RKTransformObject* a_pLerpObj, RKVector a_posEnd, RKQuaternion a_rotEnd, float a_lerpLength /*= 1.0f*/)
{
  Init(a_pLerpObj, a_pLerpObj->GetWorldPosition(), a_posEnd, a_pLerpObj->GetWorldRotation(), a_rotEnd, a_lerpLength);
}

void TransformLerper::Init(RKTransformObject* a_pLerpObj, RKVector a_posStart, RKVector a_posEnd, RKQuaternion a_rotStart,
  RKQuaternion a_rotEnd, float a_lerpLength /*= 1.0f*/, float a_easeInPercentage /*= 0.0f*/, float a_easeOutPercentage /*= 0.0f*/)
{
  m_pObject = a_pLerpObj;
  m_posStart = a_posStart;
  m_posEnd = a_posEnd;
  m_rotStart = a_rotStart;
  m_rotEnd = a_rotEnd;
  m_lerping = true;
  m_maxLerpTime = a_lerpLength;
  m_easeInPercentage = a_easeInPercentage;
  m_easeOutPercentage = a_easeOutPercentage;
}

void TransformLerper::StartLerp()
{
  m_lerpTime = 0.0f;
  m_lerping = true;
}

void TransformLerper::StopLerp()
{
  m_lerpTime = 0.0f;
  m_lerping = false;
}

double CubicHermite(float t, float p0, float p1, float m0, float m1) {
  float t2 = t*t;
  float a_tDecel = t2*t;
  return (2 * a_tDecel - 3 * t2 + 1)*p0 + (a_tDecel - 2 * t2 + t)*m0 + (-2 * a_tDecel + 3 * t2)*p1 + (a_tDecel - t2)*m1;
}

float Ease(float a_tAccel, float a_tCruising, float a_tDecel, float a_maxVal, float a_tCurrent) {
  float v = a_maxVal / (a_tAccel / 2 + a_tCruising + a_tDecel / 2);
  float x1 = v * a_tAccel / 2;
  float x2 = v * a_tCruising;
  float x3 = v * a_tDecel / 2;

  // protect against divide by 0
  if (a_tAccel < RKEPSILON)  { a_tAccel = RKEPSILON; }
  if (a_tCruising < RKEPSILON)  { a_tCruising = RKEPSILON; }
  if (a_tDecel < RKEPSILON)  { a_tDecel = RKEPSILON; }

  float ret;
  if (a_tCurrent <= a_tAccel)
  {
    ret = CubicHermite(a_tCurrent / a_tAccel, 0, x1, 0, x2 / a_tCruising * a_tAccel);
  }
  else if (a_tCurrent <= a_tAccel + a_tCruising)
  {
    ret = x1 + x2 * (a_tCurrent - a_tAccel) / a_tCruising;
  }
  else
  {
    ret = CubicHermite((a_tCurrent - a_tAccel - a_tCruising) / a_tDecel, x1 + x2, a_maxVal, x2 / a_tCruising*a_tDecel, 0);
  }

  return ret;
}

void TransformLerper::Update(float a_dt)
{
  if (m_lerping)
  {
    m_lerpTime += a_dt;
    if (m_lerpTime > m_maxLerpTime)
      m_lerpTime = m_maxLerpTime;

    float lerpDt;

    if (m_easeInPercentage < RKEPSILON && m_easeOutPercentage < RKEPSILON)
    {
      lerpDt = m_lerpTime / m_maxLerpTime;
    }
    else
    {
      float easeIn = m_maxLerpTime * m_easeInPercentage;
      float easeOut = m_maxLerpTime * m_easeOutPercentage;
      float timeCruising = m_maxLerpTime - easeIn - easeOut;

      lerpDt = Ease(easeIn, timeCruising, easeOut, 1.0f, m_lerpTime);
    }

    RKVector newPos = m_posStart;
    newPos.Lerp(m_posEnd, lerpDt);
    m_pObject->SetWorldPosition(newPos);

    RKQuaternion newRot = m_rotStart;
    newRot.Slerp(m_rotEnd, lerpDt);
    newRot.Normalize();
    m_pObject->SetWorldRotation(newRot);

    if (m_lerpTime >= m_maxLerpTime || ((m_posStart - m_posEnd).LengthSquared() < RKEPSILON))
    {
      m_pObject->SetWorldPosition(m_posEnd);
      m_pObject->SetWorldRotation(m_rotEnd);
      m_lerpTime = 0.0f;
      m_lerping = false;
      if (m_pCallback != NULL)
      {
        (*m_pCallback)(m_pCallbackOwner);
      }
    }
  }
}

void TransformLerper::SetCallback(LerpEndCallback a_pCallback, void* a_pOwner /*= NULL*/)
{
  m_pCallback = a_pCallback;
  m_pCallbackOwner = a_pOwner;
}