/*=========================================================================

  Copyright (c) Kitware, Inc.
  All rights reserved.

     This software is distributed WITHOUT ANY WARRANTY; without even
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
     PURPOSE.  See the above copyright notice for more information.

=========================================================================*/

#include "mvPoseInterpolator.h"

#include "mvLobby.h"
#include "vtkCardinalSpline.h"
#include "vtkInteractorStyle3D.h"
#include "vtkOpenVRCamera.h"
#include "vtkOpenVRRenderer.h"
#include "vtkOpenVRRenderWindow.h"
#include "vtkRenderWindowInteractor.h"
#include "vtkTupleInterpolator.h"

#include "vtkOpenVROverlayInternal.h"

mvPoseInterpolator::mvPoseInterpolator()
{
  this->Last = new vtkOpenVRCameraPose();
  for (auto &s : this->Splines)
  {
    s = vtkCardinalSpline::New();
    s->SetLeftConstraint(1);
    s->SetRightConstraint(1);
  }
}

mvPoseInterpolator::~mvPoseInterpolator()
{
  delete this->Last;
  for (auto &s : this->Splines)
  {
    s->Delete();
  }
}

void mvPoseInterpolator::Fly(
  vtkOpenVRCameraPose *start,
  vtkOpenVRCameraPose *end,
  vtkRenderer *ren,
  double percent)
{
  vtkCamera *cam = ren->GetActiveCamera();
  vtkOpenVRRenderWindow *win =
    static_cast<vtkOpenVRRenderWindow *>(ren->GetVTKWindow());

  if (this->Start != start || this->LastPercentage > percent)
  {
    this->Start = start;
    this->End = end;
    *this->Last = *start;
    this->LastPercentage = 0.0;
    // if the distance between is small then do not orient along the path
    double distance = sqrt(vtkMath::Distance2BetweenPoints(start->Translation, end->Translation));
    this->FlightMode = (distance / this->Start->Distance > 5.0);

    // set first derivatives
    for (int i = 0; i < 3; ++i)
    {
      if (this->FlightMode)
      {
        this->Splines[i]->AddPoint(0.0,start->Translation[i]);
        this->Splines[i]->AddPoint(1.0,end->Translation[i]);
        // this->Splines[i]->SetLeftValue(-start->ViewDirection[i]*distance*0.6);
        // this->Splines[i]->SetRightValue(-end->ViewDirection[i]*distance*0.6);
        this->Splines[i]->SetLeftValue(0.0);
        this->Splines[i]->SetRightValue(0.0);
      }
      else
      {
        this->Splines[i]->AddPoint(0.0,start->Translation[i]);
        this->Splines[i]->AddPoint(1.0,end->Translation[i]);
        this->Splines[i]->SetLeftValue(0.0);
        this->Splines[i]->SetRightValue(0.0);
      }
    }
    cam->GetDirectionOfProjection(this->StartViewDirection.GetData());
    win->GetPhysicalViewDirection(this->StartPhysicalViewDirection.GetData());
    vtkVector3d cvup;
    win->GetPhysicalViewUp(cvup.GetData());
    vtkVector3d civright = this->StartViewDirection.Cross(cvup);
    this->StartViewDirection = cvup.Cross(civright);

    // compute theta rotation angles
    double angle = acos(vtkMath::Dot(this->Start->ViewDirection, this->End->ViewDirection));
    this->AdjustViewDirection = (fabs(angle) > 0.4);
    this->Splines[3]->SetLeftValue(0.0);
    this->Splines[3]->SetRightValue(0.0);
    this->Splines[3]->AddPoint(0.0,0.0); // start with no rotation
    if (this->AdjustViewDirection)
    {
      double theta;
      double lastTheta = 0.0;
      vtkVector3d evd;

      // add points along the direction of travel
      for (double pos = 0.2; pos < 0.85; pos += 0.02)
      {
        evd[0] = this->Splines[0]->Evaluate(pos) - this->Splines[0]->Evaluate(pos + 0.1);
        evd[1] = this->Splines[1]->Evaluate(pos) - this->Splines[1]->Evaluate(pos + 0.1);
        evd[2] = this->Splines[2]->Evaluate(pos) - this->Splines[2]->Evaluate(pos + 0.1);
        evd = evd - evd*evd.Dot(cvup);
        evd.Normalize();
        theta = acos(evd.Dot(this->StartViewDirection));
        if (cvup.Dot(this->StartViewDirection.Cross(evd)) < 0.0)
        {
          theta = -theta;
        }
        // always pick a theta closest to the last theta
        while (fabs(theta + 6.2832 - lastTheta) < fabs(theta - lastTheta))
        {
          theta += 6.2832;
        }
        while (fabs(theta - 6.2832 - lastTheta) < fabs(theta - lastTheta))
        {
          theta -= 6.2832;
        }
        this->Splines[3]->AddPoint(pos,theta);
        lastTheta = theta;
      }

      evd.Set(
        this->End->ViewDirection[0], this->End->ViewDirection[1], this->End->ViewDirection[2]);
      evd = evd - evd*evd.Dot(cvup);
      evd.Normalize();
      theta = acos(evd.Dot(this->StartViewDirection));
      if (cvup.Dot(this->StartViewDirection.Cross(evd)) < 0.0)
      {
        theta = -theta;
      }
      // always pick a theta closest to the last theta
      while (fabs(theta + 6.2832 - lastTheta) < fabs(theta - lastTheta))
      {
        theta += 6.2832;
      }
      while (fabs(theta - 6.2832 - lastTheta) < fabs(theta - lastTheta))
      {
        theta -= 6.2832;
      }
      this->Splines[3]->AddPoint(1.0,theta);
      lastTheta = theta;
    }
    else
    {
      this->Splines[3]->AddPoint(1.0,0.0); // end with no rotation
    }
  }

  // now interpolate
  vtkVector3d trans;
  trans[0] = this->Splines[0]->Evaluate(percent);
  trans[1] = this->Splines[1]->Evaluate(percent);
  trans[2] = this->Splines[2]->Evaluate(percent);
  win->SetPhysicalTranslation(trans.GetData());

  win->SetPhysicalScale(
    (1.0 - percent)*this->Start->Distance + percent*this->End->Distance);

  vtkInteractorStyle3D *is =
    static_cast<vtkInteractorStyle3D *>(win->GetInteractor()->GetInteractorStyle());
  is->SetDollyPhysicalSpeed((1.0 - percent)*this->Start->MotionFactor + this->End->MotionFactor);

  #if 0
  if (this->AdjustViewDirection)
  {
    // what direction are we going  in
    vtkVector3d trans2;
    if (this->FlightMode)
    {
      if (percent < 0.2)
      {
        for (int i = 0; i < 3; ++i)
        {
          trans2[i] = (0.2 - percent)*this->Start->ViewDirection[i] +
            percent*(this->Splines[i]->Evaluate(0.2) - this->Splines[i]->Evaluate(0.25));
        }
      }
      if (percent >= 0.2 && percent <= 0.75)
      {
        trans2[0] = this->Splines[0]->Evaluate(percent + 0.05);
        trans2[1] = this->Splines[1]->Evaluate(percent + 0.05);
        trans2[2] = this->Splines[2]->Evaluate(percent + 0.05);
        trans2 = trans - trans2; // camera movement is opposite the world translation
      }
      if (percent > 0.75)
      {
        for (int i = 0; i < 3; ++i)
        {
          trans2[i] = (percent - 0.75)*this->End->ViewDirection[i] +
            (1.0 - percent)*(this->Splines[i]->Evaluate(0.75) - this->Splines[i]->Evaluate(0.8));
        }
      }
    }
    else
    {
      for (int i = 0; i < 3; ++i)
      {
        trans2[i] = (1.0 -percent)*this->Start->ViewDirection[i] + percent*this->End->ViewDirection[i];
      }
    }
    trans2.Normalize();
#endif

  // apply rotation angle
  vtkVector3d cvup;
  win->GetPhysicalViewUp(cvup.GetData());
  double theta = this->Splines[3]->Evaluate(percent);
  // rotate civdir by theta
  vtkVector3d civright = this->StartPhysicalViewDirection.Cross(cvup);
  vtkVector3d nivdir = this->StartPhysicalViewDirection*cos(theta) - civright*sin(theta);
  win->SetPhysicalViewDirection(nivdir.GetData());

  this->LastPercentage = percent;
}

// Interpolate between the current pose and a next pose
void mvPoseInterpolator::Interpolate(
  vtkOpenVRCameraPose *prior,
  vtkOpenVRCameraPose *next,
  double factor, // 0.0 to 1.0 how far along
  double lastFactor,
  double thetaThreshold, // how big a rotation before we rotate, radians
  mvLobby *lobby)
{
  vtkCamera *cam = lobby->GetRenderer()->GetActiveCamera();
  vtkOpenGLRenderWindow *win = lobby->GetRenderWindow();

  // compute physical translation
  vtkVector3d trans;
  trans[0] = (1.0 - factor)*prior->Translation[0] + factor*next->Translation[0];
  trans[1] = (1.0 - factor)*prior->Translation[1] + factor*next->Translation[1];
  trans[2] = (1.0 - factor)*prior->Translation[2] + factor*next->Translation[2];

  lobby->SetPhysicalTranslation(trans.GetData());
  lobby->SetPhysicalScale(
    (1.0 - factor)*prior->Distance + factor*next->Distance);

  auto ovrrw = vtkOpenVRRenderWindow::SafeDownCast(win);

  if (ovrrw)
  {
    vtkInteractorStyle3D *is =
      static_cast<vtkInteractorStyle3D *>(win->GetInteractor()->GetInteractorStyle());
    is->SetDollyPhysicalSpeed((1.0 - factor)*prior->MotionFactor + factor*next->MotionFactor);

    // adjust physcial view direction
    // compute rotation angle
    vtkVector3d cvup;
    ovrrw->GetPhysicalViewUp(cvup.GetData());
    vtkVector3d cvdir(prior->ViewDirection);
    vtkVector3d nvdir(next->ViewDirection);
    vtkVector3d civright = cvdir.Cross(cvup);
    double theta = acos(nvdir.Dot(cvdir));
    if (cvup.Dot(cvdir.Cross(nvdir)) < 0.0)
    {
      theta = -theta;
    }
    if (fabs(theta) < thetaThreshold)
    {
      theta = 0.0;
    }
    theta *= (factor - lastFactor);
    ovrrw->GetPhysicalViewDirection(cvdir.GetData());
    civright = cvdir.Cross(cvup);
    // rotate civdir by theta
    vtkVector3d nivdir = cvdir*cos(theta) - civright*sin(theta);
    ovrrw->SetPhysicalViewDirection(nivdir.GetData());
  }
  else
  {
    // compute physical translation
    vtkVector3d trans;
    trans[0] = (1.0 - factor)*prior->Position[0] + factor*next->Position[0];
    trans[1] = (1.0 - factor)*prior->Position[1] + factor*next->Position[1];
    trans[2] = (1.0 - factor)*prior->Position[2] + factor*next->Position[2];

    cam->SetPosition(trans.GetData());
    double dist = (1.0 - factor)*prior->Distance + factor*next->Distance;
    cam->SetDistance(dist);

    // adjust view direction
    // compute rotation angle
    {
      vtkVector3d cvdir(prior->ViewDirection);
      vtkVector3d nvdir(next->ViewDirection);
      vtkVector3d plane = cvdir.Cross(nvdir);
      vtkVector3d basis = plane.Cross(cvdir);
      double theta = factor*acos(nvdir.Dot(cvdir));

      // rotate civdir by theta
      vtkVector3d nivdir = cvdir*cos(theta) + basis*sin(theta);
      cam->SetFocalPoint(
        trans[0] + dist*nivdir[0],
        trans[1] + dist*nivdir[1],
        trans[2] + dist*nivdir[2]
        );
    }

    {
      vtkVector3d cvdir(prior->PhysicalViewDirection);
      vtkVector3d nvdir(next->PhysicalViewDirection);
      vtkVector3d plane = cvdir.Cross(nvdir);
      vtkVector3d basis = plane.Cross(cvdir);
      double theta = factor*acos(nvdir.Dot(cvdir));

      // rotate civdir by theta
      vtkVector3d nivdir = cvdir*cos(theta) + basis*sin(theta);
      lobby->SetPhysicalViewDirection(nivdir.GetData());
    }

    {
      vtkVector3d pvup(prior->PhysicalViewUp);
      vtkVector3d nvup(next->PhysicalViewUp);
      vtkVector3d plane = pvup.Cross(nvup);
      vtkVector3d basis = plane.Cross(pvup);
      double theta = factor*acos(nvup.Dot(pvup));

      vtkVector3d nivup = pvup*cos(theta) + basis*sin(theta);

      cam->SetViewUp(nivup[0], nivup[1], nivup[2]);
    }
  }
}

void mvPoseInterpolator::ComputeTranslationAndRotation(
  vtkOpenVRCameraPose *prior,
  vtkOpenVRCameraPose *next,
  vtkOpenVRRenderWindow *win,
  double thetaThreshold, // how big a rotation before we rotate, radians
  double *trans,
  double &rotation
  )
{
  trans[0] = next->Translation[0];
  trans[1] = next->Translation[1];
  trans[2] = next->Translation[2];

  // adjust physcial view direction
  // compute rotation angle
  vtkVector3d cvup;
  win->GetPhysicalViewUp(cvup.GetData());
  vtkVector3d cvdir(prior->ViewDirection);
  vtkVector3d nvdir(next->ViewDirection);
  vtkVector3d civright = cvdir.Cross(cvup);
  double theta = acos(nvdir.Dot(cvdir));
  if (cvup.Dot(cvdir.Cross(nvdir)) < 0.0)
  {
    theta = -theta;
  }
  if (fabs(theta) < thetaThreshold)
  {
    theta = 0.0;
  }
  rotation = theta;
}

void mvPoseInterpolator::ApplyTranslationAndRotation(
  vtkOpenVRCameraPose *prior,
  vtkOpenVRCameraPose *next,
  vtkOpenVRRenderWindow *win,
  double *trans,
  double rotation
  )
{
  next->Position[0] = next->Position[0] + next->Translation[0] - trans[0];
  next->Position[1] = next->Position[1] + next->Translation[1] - trans[1];
  next->Position[2] = next->Position[2] + next->Translation[2] - trans[2];

  next->Translation[0] = trans[0];
  next->Translation[1] = trans[1];
  next->Translation[2] = trans[2];

  // adjust physcial view direction
  // compute rotation angle
  vtkVector3d cvup;
  win->GetPhysicalViewUp(cvup.GetData());
  vtkVector3d cvdir(prior->ViewDirection);
  vtkVector3d cvright = cvdir.Cross(cvup);

  vtkVector3d nvdir;
  nvdir = cos(rotation)*cvdir - sin(rotation)*cvright;

  next->ViewDirection[0] = nvdir[0];
  next->ViewDirection[1] = nvdir[1];
  next->ViewDirection[2] = nvdir[2];
}
