#include "./funcs.hpp"

void swap(int *xp, int *yp) 
{ 
  int temp = *xp; 
  *xp = *yp; 
  *yp = temp; 
}

void find_controller(std::string& controllerName, bindoc& binDoc, int& index)
{
  index= -1;
  for(unsigned int ci= 0; ci< binDoc.armature_ctls.size(); ci++)
  {
    armature_ctl& ctl= binDoc.armature_ctls[ci];
    if(ctl.name.compare(controllerName)==0) index= ci;
  }
}

void read_controllers(TiXmlDocument* colladaDoc, bindoc& binDoc)
{
  std::vector<armature_ctl>& controllers= binDoc.armature_ctls;
  TiXmlElement* libraryControllersNode= colladaDoc->RootElement()->FirstChildElement("library_controllers");
  if(libraryControllersNode == NULL) return;

  TiXmlElement* controllerNode= NULL;
  for(controllerNode = libraryControllersNode->FirstChildElement("controller"); 
      controllerNode != NULL; 
      controllerNode = controllerNode->NextSiblingElement("controller")
  ){
    if(read_armature_controller(controllerNode, controllers)) continue;
  }
}

bool read_armature_controller(TiXmlElement* controllerNode, std::vector<armature_ctl>& controllers)
{
  //try read skin node
  TiXmlElement* skinNode= controllerNode->FirstChildElement("skin");   
  ASSERT(skinNode != NULL);
  if(skinNode == NULL) return false;

  //add controller, if there is a skin node
  controllers.push_back(armature_ctl());
  armature_ctl& ctrl= controllers.back();

  //read controller name
  const char* cntrlNameAttr= controllerNode->Attribute("name");
  if(cntrlNameAttr != NULL){
    ctrl.name= std::string(cntrlNameAttr);
  }
  
  //read controller id
  const char* cntrlIdAttr= controllerNode->Attribute("id");
  if(cntrlIdAttr != NULL){
    ctrl.ctrlId= std::string(cntrlIdAttr);
  }

  //read skin source name
  ctrl.skin_source= std::string(skinNode->Attribute("source"));

  //read bind shape matrix
  TiXmlElement* bindShapeMatrixEl= skinNode->FirstChildElement("bind_shape_matrix");
  ASSERT(bindShapeMatrixEl != NULL)
  mat4_from_node(bindShapeMatrixEl, ctrl.bind_shape_matrix, true);

  //read joints node
  TiXmlElement* jointsNode= skinNode->FirstChildElement("joints");
  //input joint input nodes
  TiXmlElement* inputNode= jointsNode->FirstChildElement("input");
  while(inputNode!= NULL){

    std::string sem(inputNode->Attribute("semantic"));
    std::string sourceid(inputNode->Attribute("source"));
    
    TiXmlElement* sourceNode= find_source(skinNode, sourceid.c_str());         
    
    if(sem.compare("JOINT")== 0){

      ctrl.joint_names= names_data();
      read_names_source(sourceNode, ctrl.joint_names);
    }
    else if(sem.compare("INV_BIND_MATRIX")== 0){
      
      ctrl.joint_inv_transforms= num1D_data();
      read_num1D_source(sourceNode, ctrl.joint_inv_transforms);
    }

    //read next input node
    inputNode= inputNode->NextSiblingElement("input");
  }

  //read joints  
  ctrl.joints.resize(ctrl.joint_names.size);

  for(uint32 ji= 0; ji< ctrl.joints.size(); ji++){
    ctrl.joints[ji]= armature_joint();
    ctrl.joints[ji].joint_arr_index= ji;
    ctrl.joints[ji].name= std::string(ctrl.joint_names.name_data[ji]);
    ctrl.joints[ji].nodeId= ctrl.name+"_"+ctrl.joints[ji].name;

    int invStride= ctrl.joint_inv_transforms.stride;
    float* invData= (float*)ctrl.joint_inv_transforms.num_data;

    //extract inverse bind transform for particular joint, from source
    float* destdat= &ctrl.joints[ji].inv_bind_transform.e00;
    memcpy(destdat, &invData[ji*invStride], invStride*sizeof(float));
    ctrl.joints[ji].inv_bind_transform= ctrl.joints[ji].inv_bind_transform.transpose();
  }

  //read vertex weights
  TiXmlElement* vertexWeightsNode= skinNode->FirstChildElement("vertex_weights");
  inputNode= vertexWeightsNode->FirstChildElement("input");
  while(inputNode!= NULL){

    std::string sem(inputNode->Attribute("semantic"));
    std::string sourceid(inputNode->Attribute("source"));
    TiXmlElement* sourceNode= find_source(skinNode, sourceid.c_str());

    if(sem.compare("WEIGHT")== 0){
      ctrl.vertex_joint_weights= num1D_data();
      read_num1D_source(sourceNode, ctrl.vertex_joint_weights);
    }

    //read next input node
    inputNode= inputNode->NextSiblingElement("input");
  }

  //read the "vcount" and determine how many joints affect each node - K
  TiXmlElement* vcountNode= vertexWeightsNode->FirstChildElement("vcount");
  TiXmlElement* vNode= vertexWeightsNode->FirstChildElement("v");
  char* vcount_raw= (char*)vcountNode->GetText();
  char* v_raw= (char*)vNode->GetText();
  int vertex_count= atoi(vertexWeightsNode->Attribute("count"));
  int* vwjcount= new int[vertex_count];

  int** vertexJointWeightData= new int*[vertex_count];
  vwjcount[0]= atoi(strtok(vcount_raw, " "));
  
  for(uint32 vi= 1; vi< vertex_count; vi++){
    vwjcount[vi]= atoi(strtok(NULL, " "));
  }
  
  int wjk= 0;
  int vwj= atoi(strtok(v_raw, " "));
  
  //for each node read K WEIGHT/JOINT pairs from the "v" node
  for(uint32 vwji= 0; vwji< vertex_count; vwji++){

    int indic[vwjcount[vwji]];
    int weightsa[vwjcount[vwji]];
    wjk= 0;
    
    if(vwji== 0){
      indic[0]= vwj;      
    }

    //read weights and indices arrays from raw string
    while(wjk< vwjcount[vwji]){
      if(vwji!= 0 || wjk> 0){
        vwj= atoi(strtok(NULL, " "));
        indic[wjk]= vwj;
      }
      vwj= atoi(strtok(NULL, " "));
      weightsa[wjk]= vwj;
      wjk++;
    }

    //sort the joint/weight pairs in a weight-factor descending order
    for (int sorti = 0; sorti < vwjcount[vwji]-1; sorti++){
      for (int sortj = 0; sortj < vwjcount[vwji]-sorti-1; sortj++){
        float& weightl= ((float*)ctrl.vertex_joint_weights.num_data)[weightsa[sortj]];
        float& weightr= ((float*)ctrl.vertex_joint_weights.num_data)[weightsa[sortj+1]];
        if (weightl < weightr){
          swap(&indic[sortj], &indic[sortj+1]);
          swap(&weightsa[sortj], &weightsa[sortj+1]);
        }
      }
    }

    //copy to main array
    vertexJointWeightData[vwji]= new int[2*vwjcount[vwji]];
    int hhk= 0;
    wjk= 0;
    while(wjk< vwjcount[vwji]){
      vertexJointWeightData[vwji][hhk]= indic[wjk];
      hhk++;
      vertexJointWeightData[vwji][hhk]= weightsa[wjk];
      hhk++;
      wjk++;
    }
  }

  /*
    - !!! vertexJointWeightData holds vertex_count arrays of size 2*(the number of joints affecting the vertex) integers.
    - the weights are already read in the "vertex_joint_weights" array.        
    - take the first three joints from the list and normalize the weight coefitients as: w_i/(w_1+w_2+w_3)
  */
  ctrl.vertex_joint_assoc= num1D_data();
  num1D_data& jas= ctrl.vertex_joint_assoc;
  jas.stride= 6;
  jas.size= vertex_count*6*sizeof(int);
  jas.gltype = GL_INT;
  jas.num_data= malloc(jas.size);
  int jai= 0;
  
  for(uint32 vi= 0; vi< vertex_count; vi++){
    int vwjcount_= 3;
    float sumw= 0.0f;
    int j= 0;
    int len= (vwjcount[vi] > 3)? 3: vwjcount[vi];
    
    for(j= 0; j< len; j++){
      float weight= ((float*)ctrl.vertex_joint_weights.num_data)[vertexJointWeightData[vi][j*2+1]];
      sumw+= weight;
    }

    int opk= 0;    
    for(j= 0; j< 3; j++){
      if(j< len){
        int ji= vertexJointWeightData[vi][j*2];
        int wi= vertexJointWeightData[vi][j*2+1];
        float& weight= ((float*)ctrl.vertex_joint_weights.num_data)[wi];
        weight/= sumw;        
        ((int*)jas.num_data)[jai]= ji;
        jai++;
        ((int*)jas.num_data)[jai]= wi;
        jai++;
      }
      else {
        ((int*)jas.num_data)[jai]= -1;
        jai++;
        ((int*)jas.num_data)[jai]= -1;
        jai++;
      }
    }
  }

  //free the vertex joint weight data array
  if(vertexJointWeightData!= NULL)
  {
    for(uint32 vjwi=0; vjwi< vertex_count; vjwi++){
      if(vertexJointWeightData[vjwi] != NULL) delete[] vertexJointWeightData[vjwi];
    }
    delete[] vertexJointWeightData;
  }

  return true;
}