#include "Body_skinned.h"

#include <stdio.h>

#include "../../Graphick/OpenGL/OpenGlSL.h"



#include "../../tools/tools.h"



Body_skinned::Body_skinned()

{

  m_body_type = TYPE_SKINNED;

  m_hw_skinning_prepared = false;

  m_bone_matrix = 0;

};



Body_skinned::~Body_skinned()

{

  if (m_bone_matrix)

    delete [] m_bone_matrix;

}



void Body_skinned::InitSkin()

{

  if (!m_mesh)

    return;



  //Allocate and copy vertex

  m_mesh->m_base_vertex.Create(m_mesh->m_vertex.m_point_count);

  memcpy(m_mesh->m_base_vertex.m_point, m_mesh->m_vertex.m_point, m_mesh->m_vertex.m_point_count*sizeof(Tpoint3f));



  if (m_mesh->m_normal.m_point_count > 0)

  {

    m_mesh->m_base_normal.Create(m_mesh->m_normal.m_point_count);

    memcpy(m_mesh->m_base_normal.m_point, m_mesh->m_normal.m_point, m_mesh->m_normal.m_point_count*sizeof(Tpoint3f));

  };

};



Bone* Body_skinned::AddBone(int weight_count, Body_base * body)

{

  Bone* bone = m_bone.add();

  bone->Initialize(weight_count, body);

  m_hw_skinning_prepared = false; //ReInit

  return bone;

};



Bone* Body_skinned::GetBone(int i)

{

  if (i < 0 || i >= m_bone.count)

    return 0;



  return (m_bone.item[i]);

};



void Body_skinned::InitializeHwSkinning()

{

  //HW skinning preparing

  m_mesh->m_skin_attrib_bone_index.Create(m_mesh->m_vertex.m_point_count);

  m_mesh->m_skin_attrib_bone_weight.Create(m_mesh->m_vertex.m_point_count);

  char* weight_count = new char[m_mesh->m_vertex.m_point_count];

  for (int i = 0; i < m_mesh->m_vertex.m_point_count; i ++)

  {

    weight_count[i] = 0;



    m_mesh->m_skin_attrib_bone_index.m_point[i].r = 255;

    m_mesh->m_skin_attrib_bone_index.m_point[i].g = 255;

    m_mesh->m_skin_attrib_bone_index.m_point[i].b = 255;

    m_mesh->m_skin_attrib_bone_index.m_point[i].a = 255;



    m_mesh->m_skin_attrib_bone_weight.m_point[i].a = 0.0f;

    m_mesh->m_skin_attrib_bone_weight.m_point[i].b = 0.0f;

    m_mesh->m_skin_attrib_bone_weight.m_point[i].c = 0.0f;

    m_mesh->m_skin_attrib_bone_weight.m_point[i].d = 0.0f;

  }



  //

  for (int i = 0; i < m_bone.count; i ++)

  {

    Bone* bone = m_bone.item[i];

    if (bone->m_weight_count == 0)

      continue;



    for (int n = 0; n < bone->m_weight_count; n ++)

    {

      int vertex_id = bone->m_index[n];

      float weight = bone->m_weight[n];

      if (weight_count[vertex_id] >= 4)

      {

        printf(" --- To many weight per vertex ---\n");

      } else

      {

        if (weight_count[vertex_id] == 0)

        {

          m_mesh->m_skin_attrib_bone_index.m_point[vertex_id].r = i;

          m_mesh->m_skin_attrib_bone_weight.m_point[vertex_id].a = weight;

        }

        if (weight_count[vertex_id] == 1)

        {

          m_mesh->m_skin_attrib_bone_index.m_point[vertex_id].g = i;

          m_mesh->m_skin_attrib_bone_weight.m_point[vertex_id].b = weight;

        }

        if (weight_count[vertex_id] == 2)

        {

          m_mesh->m_skin_attrib_bone_index.m_point[vertex_id].b = i;

          m_mesh->m_skin_attrib_bone_weight.m_point[vertex_id].c = weight;

        }

        if (weight_count[vertex_id] == 3)

        {

          m_mesh->m_skin_attrib_bone_index.m_point[vertex_id].a = i;

          m_mesh->m_skin_attrib_bone_weight.m_point[vertex_id].d = weight;

        }

        weight_count[vertex_id] ++;

      }

    }

  };

  delete [] weight_count;





  if (m_bone_matrix)

    delete m_bone_matrix;

  m_bone_matrix = new Tmatrix[m_bone.count];



  m_hw_skinning_prepared = true;

};



void Body_skinned::Control(double time)

{

  if (!m_enable)

    return;



  if (!m_mesh || m_mesh->m_base_vertex.m_point_count <= 0)

    return;



  if (!m_hw_skinning_prepared)

    InitializeHwSkinning();



  m_control_counter ++;



  if (m_out_of_view)

    return;



  if (m_out_lod == 1 && m_control_counter % 4 == 0)

    return;



  if (m_out_lod == 2 && m_control_counter % 2 == 0)

    return;



  //if (m_control_counter % 3 == 0)

  //  return;



  //!!!!! need to implement zero bone !!!!

  //Faster is calculate initial_skin point array and removed zero initialization



  //Prepare to animation

  //memcpy(m_mesh->m_vertex.m_point, m_mesh->m_base_vertex.m_point, m_mesh->m_vertex.m_point_count*sizeof(Tpoint3f));





  bool hw_skinning = true;

  Tpoint3f* vertex = m_mesh->m_vertex.m_point;

  Tpoint3f* vertex_base = m_mesh->m_base_vertex.m_point;



  if (!hw_skinning)

    memset(vertex, 0, sizeof(Tpoint3f)*m_mesh->m_base_vertex.m_point_count);



  Tpoint3f* normal = 0;

  Tpoint3f* normal_base = 0;

  if (!hw_skinning)

    if (m_mesh->m_normal.m_point_count > 0 && m_mesh->m_base_normal.m_point_count > 0)

    {

      normal = m_mesh->m_normal.m_point;

      normal_base = m_mesh->m_base_normal.m_point;

      memset(normal, 0, sizeof(Tpoint3f)*m_mesh->m_base_normal.m_point_count);

    }



  //Apply bone animation

  for (int i = 0; i < m_bone.count; i ++)

  {

    Bone* bone = m_bone.item[i];

    if (bone->m_weight_count == 0)

      continue;



    //Get current deformation bone matrix

    Tmatrix body_matrix;

    bone->m_node_boby->GetMatrix(body_matrix);



    Tmatrix r_matrix;

    r_matrix.intialize();



    MultMatrixf(r_matrix.matrix.elements_16, m_inv_matrix.matrix.elements_16);

    MultMatrixf(r_matrix.matrix.elements_16, body_matrix.matrix.elements_16);

    MultMatrixf(r_matrix.matrix.elements_16, bone->m_init_matrix_inv.matrix.elements_16);

    MultMatrixf(r_matrix.matrix.elements_16, m_matrix.matrix.elements_16);

    if (hw_skinning)

      memcpy(m_bone_matrix + i, &r_matrix, sizeof(Tmatrix)); //copy matrix to buffer



    float* mt = r_matrix.matrix.elements_16;



    //Apply transformation

    if (!hw_skinning)

    for (int n = 0; n < bone->m_weight_count; n ++)

    {

      int id = bone->m_index[n];



      float x = vertex_base[id].x;

      float y = vertex_base[id].y;

      float z = vertex_base[id].z;



      float weight = bone->m_weight[n];

      //float iweight = 1.0f - weight;



      vertex[id].x += weight*(mt[0]*x + mt[4]*y + mt[8 ]*z + mt[12]);

      vertex[id].y += weight*(mt[1]*x + mt[5]*y + mt[9 ]*z + mt[13]);

      vertex[id].z += weight*(mt[2]*x + mt[6]*y + mt[10]*z + mt[14]);



      if (normal)

      {

        x = normal_base[id].x;

        y = normal_base[id].y;

        z = normal_base[id].z;

        normal[id].x += weight*(mt[0]*x + mt[4]*y + mt[8 ]*z);

        normal[id].y += weight*(mt[1]*x + mt[5]*y + mt[9 ]*z);

        normal[id].z += weight*(mt[2]*x + mt[6]*y + mt[10]*z);

      }

    };

  };



  //if (m_control_counter % 5 == 0)

  m_mesh->CaclulateBbox();

};



void Body_skinned::ReplaceBone(long long old_id, Body_base * body)

{

  for (int i = 0; i < m_bone.count; i ++)

  {

    if (m_bone.item[i]->m_node_boby->GetId() == old_id)

    {

      m_bone.item[i]->m_node_boby = body;

    };

  };

};



void Body_skinned::Paint()

{

  if (m_gr_shader)

  {

    m_gr_shader->Enable();

    m_gr_shader->SetBoneMatrixArray(m_bone_matrix, m_bone.count);

    m_gr_shader->Disable();

    m_mesh->m_skin_attrib_bone_index_location = ((GlslShader*)m_gr_shader)->m_location_bone_index;

    m_mesh->m_skin_attrib_bone_weight_location = ((GlslShader*)m_gr_shader)->m_location_bone_weight;

  }



  Body_base::Paint();

};