//
// Created by martin on 16.11.21.
//

#include "SkinnedMeshRenderer.h"

#include "sokol_gfx.h"
#include "util/sokol_gl.h"
#include "HandmadeMath.h"

static void draw_vec(const ozz::math::SimdFloat4& vec) {
  sgl_v3f(ozz::math::GetX(vec), ozz::math::GetY(vec), ozz::math::GetZ(vec));
}

static void draw_line(
    const ozz::math::SimdFloat4& v0,
    const ozz::math::SimdFloat4& v1) {
  draw_vec(v0);
  draw_vec(v1);
}

// this draws a wireframe 3d rhombus between the current and parent joints
void draw_joint (const SkinnedMesh& skinned_mesh, int joint_index, int parent_joint_index) {
  if (parent_joint_index < 0) {
    return;
  }

  using namespace ozz::math;

  const Float4x4& m0 = skinned_mesh.m_model_matrices[joint_index];
  const Float4x4& m1 = skinned_mesh.m_model_matrices[parent_joint_index];

  const SimdFloat4 p0 = m0.cols[3];
  const SimdFloat4 p1 = m1.cols[3];
  const SimdFloat4 ny = m1.cols[1];
  const SimdFloat4 nz = m1.cols[2];

  const SimdFloat4 len = SplatX(Length3(p1 - p0)) * simd_float4::Load1(0.1f);

  const SimdFloat4 pmid = p0 + (p1 - p0) * simd_float4::Load1(0.66f);
  const SimdFloat4 p2 = pmid + ny * len;
  const SimdFloat4 p3 = pmid + nz * len;
  const SimdFloat4 p4 = pmid - ny * len;
  const SimdFloat4 p5 = pmid - nz * len;

  sgl_c3f(1.0f, 1.0f, 0.0f);
  draw_line(p0, p2);
  draw_line(p0, p3);
  draw_line(p0, p4);
  draw_line(p0, p5);
  draw_line(p1, p2);
  draw_line(p1, p3);
  draw_line(p1, p4);
  draw_line(p1, p5);
  draw_line(p2, p3);
  draw_line(p3, p4);
  draw_line(p4, p5);
  draw_line(p5, p2);
}

void RenderSkinnedMesh (const SkinnedMesh& skinned_mesh) {
  sgl_matrix_mode_modelview();
  sgl_push_matrix();
  hmm_mat4 scale_mat = HMM_Scale(HMM_Vec3(0.01f, 0.01f, 0.01f));
  sgl_mult_matrix((const float*)&scale_mat);

  const int num_joints = skinned_mesh.m_skeleton.num_joints();
  ozz::span<const int16_t> joint_parents = skinned_mesh.m_skeleton.joint_parents();
  sgl_begin_lines();
  for (int joint_index = 0; joint_index < num_joints; joint_index++) {
    draw_joint(skinned_mesh, joint_index, joint_parents[joint_index]);
  }
  sgl_end();
  sgl_pop_matrix();
}