//
// Created by martin on 21.11.21.
//

#include "AnimNodes/AnimSamplerNode.h"
#define OZZ_INCLUDE_PRIVATE_HEADER
#include "../src/animation/runtime/animation_keyframe.h"
#include "catch.hpp"
#include "ozzutils.h"

void get_bone_transform(
    int i_bone_idx,
    const ozz::vector<ozz::math::SoaTransform>& i_local_matrices,
    ozz::math::Transform& o_transform) {
  int matrix_index = i_bone_idx / 4;
  short simd_component = i_bone_idx % 4;
  o_transform.translation.x =
      i_local_matrices[matrix_index].translation.x[simd_component];
  o_transform.translation.y =
      i_local_matrices[matrix_index].translation.y[simd_component];
  o_transform.translation.z =
      i_local_matrices[matrix_index].translation.z[simd_component];
}

void sample_bone_transform(
    float ratio,
    int i_bone_idx,
    const ozz::animation::Animation* i_animation,
    ozz::animation::SamplingCache& io_cache,
    ozz::vector<ozz::math::SoaTransform>& io_local_matrices,
    ozz::math::Transform& o_transform) {
  ozz::animation::SamplingJob sampling_job;
  sampling_job.animation = i_animation;
  sampling_job.cache = &io_cache;
  sampling_job.ratio = ratio;
  sampling_job.output = make_span(io_local_matrices);

  if (!sampling_job.Run()) {
    ozz::log::Err() << "Error sampling animation." << std::endl;
  }

  get_bone_transform(i_bone_idx, io_local_matrices, o_transform);
}


TEST_CASE("Sample single bone channel", "[AnimSamplerNode]") {
  SkinnedMesh skinned_mesh;
  skinned_mesh.LoadSkeleton("../media/MixamoYBot-skeleton.ozz");
  skinned_mesh.LoadAnimation("../media/Walking-loop.ozz");

  ozz::animation::Animation* animation = skinned_mesh.m_animations[0];
  const int num_soa_joints = skinned_mesh.m_skeleton.num_soa_joints();
  const int num_joints = skinned_mesh.m_skeleton.num_joints();

  ozz::vector<ozz::math::SoaTransform> local_matrices;
  ozz::animation::SamplingCache sampling_cache;

  local_matrices.resize(num_soa_joints);
  sampling_cache.Resize(num_joints);

  // sample at ratio 0.
  float ratio = 0.f;

  ozz::animation::SamplingJob sampling_job;
  sampling_job.animation = animation;
  sampling_job.cache = &sampling_cache;
  sampling_job.ratio = ratio;
  sampling_job.output = make_span(local_matrices);

  if (!sampling_job.Run()) {
    ozz::log::Err() << "Error sampling animation." << std::endl;
  }

  ozz::math::Transform root_transform_0;
  sample_bone_transform(
      0.f,
      0,
      animation,
      sampling_cache,
      local_matrices,
      root_transform_0);

  int n_samples = 53;
  for (int i = 0; i <= n_samples; i++) {
    float ratio = i * 1.f / n_samples;
    ozz::math::Transform sampled_root_transform;
    sample_bone_transform(
        i * 1.f / n_samples,
        0,
        animation,
        sampling_cache,
        local_matrices,
        sampled_root_transform);

    ozz::math::Transform calculated_root_transform;
    calc_bone_translation(ratio, 0, animation, calculated_root_transform);

//    std::cout << "ratio: " << ratio << "\t" << "err: " << ozz::math::Length(sampled_root_transform.translation - calculated_root_transform.translation) << std::endl;
  }

  ozz::math::Transform root_transform_1;
  calc_bone_translation(0.f, 0, animation, root_transform_0);
  calc_bone_translation(1.f, 0, animation, root_transform_1);
}

TEST_CASE("Root Bone Sampling", "[AnimSamplerNode]") {
  SkinnedMesh skinned_mesh;
  skinned_mesh.LoadSkeleton("../media/MixamoYBot-skeleton.ozz");
  int anim_idx = 0;
  skinned_mesh.LoadAnimation("../media/Walking-loop.ozz");
  float walking_markers[] = {0.293, 0.762};
  skinned_mesh.m_animation_sync_track[anim_idx] = SyncTrack::CreateFromMarkers(
      skinned_mesh.m_animations[anim_idx]->duration(),
      2,
      walking_markers);

//  AnimationController anim_controller(&skinned_mesh);
//  AnimSamplerNode test_node(&anim_controller);
//  test_node.SetAnimation(skinned_mesh.m_animations[0], SyncTrack());
//
//  test_node.Reset();
//  test_node.UpdateTime(0.2f);
//
//  ozz::math::Transform root_transform;
//  test_node.Evaluate(&skinned_mesh.m_local_matrices, &root_transform);
}