#include "SM_Engine3DPCH.h"
#include "SM_KeyFrame.h"
#include "SM_KeyFrameSequence.h"
#include "SM_FileVer.h"
#include "MSystemFunctions.h"



KeyFrameSequence::KeyFrameSequence()
{
  m_uKeyFrames    =0;  
  m_pKeyFrames    =0;
  m_uSamplingRate =0;
  m_fSamplingRate =0;
}

KeyFrameSequence::~KeyFrameSequence()
{
  Shutdown();
}

int KeyFrameSequence::Init()
{
  return (0);
}

int KeyFrameSequence::Shutdown()
{
  m_uKeyFrames=0;
  if (m_pKeyFrames) { delete[] m_pKeyFrames; m_pKeyFrames=0; }
  m_uSamplingRate =0;

  return (0);
}

int KeyFrameSequence::GetKeyFrame(float fTime, float fStartTime)
{
  float fFrame=(fTime-fStartTime)*(float)m_fSamplingRate;

  return ((int)floorf(fFrame))%(m_uKeyFrames-1);
}

void KeyFrameSequence::GetKeyFrame(float fTime, float fStartTime, Vector3D* pV3d, Quaternion* pQ)
{
  
  float fFrame=(fTime-fStartTime)*(float)m_fSamplingRate;

  int iFrameA=((int)floorf(fFrame))%(m_uKeyFrames-1);
  int iFrameB=iFrameA+1;
  float fLerp=fFrame-floorf(fFrame);

  
  assert(iFrameA>=0    && iFrameA<m_uKeyFrames);
  assert(iFrameB>=0    && iFrameB<m_uKeyFrames);
  assert(fLerp>=0.0f && fLerp<=1.0f);
  
  
  if (pV3d)
  {
  *pV3d=m_pKeyFrames[iFrameA].m_Position+
               (m_pKeyFrames[iFrameB].m_Position-m_pKeyFrames[iFrameA].m_Position)*fLerp;
  }


  if (pQ)
  {
    *pQ=Quaternion::Slerp(fLerp, m_pKeyFrames[iFrameA].m_Rotation, m_pKeyFrames[iFrameB].m_Rotation);
  }
}
  
KeyFrame*  KeyFrameSequence::GetKeyFrame(float fTime, float fStartTime, KeyFrame* pKeyFrame)
{ 
  GetKeyFrame(fTime, fStartTime, &pKeyFrame->m_Position, &pKeyFrame->m_Rotation);
  return (pKeyFrame);
}

int KeyFrameSequence::Load(MVFSFILE* f)
{
  if (MVFS::fread(&m_uKeyFrames, sizeof(unsigned), 1, f)!=1) return -1;

  if (GetOpenFileVer()<MAXSCENE_FILEVER)
  {
    if (MVFS::fread(&m_uSamplingRate, sizeof(unsigned), 1, f)!=1) return -1;
    m_fSamplingRate=float(m_uSamplingRate);
  }
  else
  {
    if (MVFS::fread(&m_fSamplingRate, sizeof(unsigned), 1, f)!=1) return -1;    
  }

  m_pKeyFrames=new KeyFrame[m_uKeyFrames];
  if (!m_pKeyFrames)
  {
    return -1;
  }

  if (MVFS::fread(m_pKeyFrames, sizeof(KeyFrame)*m_uKeyFrames, 1, f)!=1) return -1;

  unsigned i;

  for (i = 0 ; i < m_uKeyFrames-1 ; i++)
  {
    if (m_pKeyFrames[i].m_Rotation.Dot(m_pKeyFrames[i+1].m_Rotation) < 0.0f)
    {
      assert(!"Ooops");
    }
  }
  
  return 0;
}

int KeyFrameSequence::Save(MVFSFILE* f)
{
  //return SaveCompressed(f);
  if (MVFS::fwrite(&m_uKeyFrames, sizeof(unsigned), 1, f)!=1) return -1;
  if (MVFS::fwrite(&m_fSamplingRate, sizeof(float), 1, f)!=1) return -1;
  if (MVFS::fwrite(m_pKeyFrames, sizeof(KeyFrame)*m_uKeyFrames, 1, f)!=1) return -1;
  
  return 0;
}

int KeyFrameSequence::LoadCompressed(MVFSFILE* f)
{
  return (Load(f));
}

int KeyFrameSequence::SaveCompressed(MVFSFILE* f)
{
  KeyFrameSequence Compressed;

  Compress(&Compressed);
  
  if (MVFS::fwrite(&Compressed.m_uKeyFrames, sizeof(unsigned), 1, f)!=1) return -1;
  if (MVFS::fwrite(&Compressed.m_fSamplingRate, sizeof(float), 1, f)!=1) return -1;
  if (MVFS::fwrite(Compressed.m_pKeyFrames, sizeof(KeyFrame)*Compressed.m_uKeyFrames, 1, f)!=1) return -1;  

  return 0;
}


void KeyFrameSequence::Compress(KeyFrameSequence* pCompressed)
{
  pCompressed->Shutdown();
  
  
  float fSamplingRate=this->m_fSamplingRate;
  
  float fBest=fSamplingRate;
  for ( ; fSamplingRate>1.0f ; fSamplingRate-=1.0f)
  {
    pCompressed->Resample(this, fSamplingRate);    


    float fMaxDist, fMaxAngle;
    GetPercentageDiff(this, pCompressed, &fMaxDist, &fMaxAngle);

    if (fMaxAngle>0.01f || fMaxDist>0.05f)
    {
      break;
    }
    else
    {
      fBest=fSamplingRate;
    }
  }


  pCompressed->Resample(this, fBest);    
}

int KeyFrameSequence::Resample(KeyFrameSequence* pSource, float fSampleRate)
{
  if (this==pSource)
  {
    assert(!"Can't do self resample");
  }

  Shutdown();

  // Calculate final sample rate
  float    fLength=float(pSource->m_uKeyFrames-1)/pSource->m_fSamplingRate;
  unsigned uNewSamples=(unsigned) ceilf(fSampleRate*fLength)+1;
  
  // Fill out sequence
  m_pKeyFrames=new KeyFrame[uNewSamples];
  if (!m_pKeyFrames)
  {
    return -1;
  }

  m_uKeyFrames   =uNewSamples;
  m_fSamplingRate=float(uNewSamples-1)/fLength;

  // Do actual resample
  unsigned i;

  float fTimeDelta=fLength/(float)(m_uKeyFrames-1);
  for (i=0 ; i<m_uKeyFrames ; i++)
  {
    pSource->GetKeyFrame(i*fTimeDelta, 0.0f, &m_pKeyFrames[i]);
  }

  m_pKeyFrames[m_uKeyFrames-1]=pSource->m_pKeyFrames[pSource->m_uKeyFrames-1];

  return 0;
}

void KeyFrameSequence::GetPercentageDiff(KeyFrameSequence* pBase, KeyFrameSequence* pSeq, float* fMaxDistance, float* fMaxAngle)
{
  unsigned i;

  *fMaxDistance=0.0f;
  *fMaxAngle=0.0f;
  
  float fBaseDist=FLT_MIN;
  for (i=0 ; i<pBase->m_uKeyFrames-1 ; i++)
  {
    fBaseDist=max(fBaseDist, (pBase->m_pKeyFrames[i].m_Position-pBase->m_pKeyFrames[i+1].m_Position).Length());
  }
  
  for (i=0 ; i<pBase->m_uKeyFrames-1 ; i++)
  {
    float fTime=float(i)/pBase->m_fSamplingRate;

    int iBaseA=pBase->GetKeyFrame(fTime, 0.0f);
    int iBaseB=pBase->GetKeyFrame(fTime, 0.0f)+1;

    
    

    KeyFrame Base;
    KeyFrame Seq;
    
    pBase->GetKeyFrame(fTime, 0, &Base);
    pSeq->GetKeyFrame(fTime, 0, &Seq);

    float fKeysDist =(Base.m_Position-Seq.m_Position).Length();
    float fKeysAngle=Quaternion::Angle(Base.m_Rotation, Seq.m_Rotation);

    float fRelDist =fBaseDist==0.0f?0.0f:fBaseDist>0.5f?fKeysDist/fBaseDist:fKeysDist;
      
    if (fRelDist>*fMaxDistance)
    {
      *fMaxDistance=fRelDist;
    }

    if (fKeysAngle>*fMaxAngle)
    {
      *fMaxAngle=fKeysAngle;
    }



  }  
}
